Daniele Lacamera
Free (GPLv2) TCP/IP stack developed by TASS Belgium
Dependents: lpc1768-picotcp-demo ZeroMQ_PicoTCP_Publisher_demo TCPSocket_HelloWorld_PicoTCP Pico_TCP_UDP_Test ... more
PicoTCP. Copyright (c) 2013 TASS Belgium NV.
Released under the GNU General Public License, version 2.
Different licensing models may exist, at the sole discretion of the Copyright holders.
Official homepage: http://www.picotcp.com
Bug tracker: https://github.com/tass-belgium/picotcp/issues
Development steps:
initial integration with mbed RTOSgeneric mbed Ethernet driverhigh performance NXP LPC1768 specific Ethernet driverMulti-threading support for mbed RTOSBerkeley sockets and integration with the New Socket APIFork of the apps running on top of the New Socket APIScheduling optimizations- Debugging/benchmarking/testing
Demo application (measuring TCP sender performance):
Import programlpc1768-picotcp-demo
A PicoTCP demo app testing the ethernet throughput on the lpc1768 mbed board.
Last commit 17 May 2013 by 
TASS Belgium
Revision 68:0847e35d08a6, committed 2013-09-19
- Comitter:
- tass
- Date:
- Thu Sep 19 13:26:14 2013 +0000
- Parent:
- 67:fb3098601f18
- Child:
- 69:43873e366ae1
- Commit message:
- Imported from masterbranch, again
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/arch/pico_mbed.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,94 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+Do not redistribute without a written permission by the Copyright
+holders.
+
+File: pico_mbed.h
+Author: Toon Peters
+*********************************************************************/
+
+#ifndef PICO_SUPPORT_MBED
+#define PICO_SUPPORT_MBED
+#include <stdio.h>
+
+//#include "mbed.h"
+//#include "serial_api.h"
+
+/*
+Debug needs initialization:
+* void serial_init (serial_t *obj, PinName tx, PinName rx);
+* void serial_baud (serial_t *obj, int baudrate);
+* void serial_format (serial_t *obj, int data_bits, SerialParity parity, int stop_bits);
+*/
+
+#define dbg(...)
+#define pico_zalloc(x) calloc(x, 1)
+#define pico_free(x) free(x)
+
+#ifdef MEMORY_MEASURE // in case, comment out the two defines above me.
+extern uint32_t max_mem;
+extern uint32_t cur_mem;
+
+static inline void * pico_zalloc(int x)
+{
+ uint32_t *ptr;
+ if ((cur_mem + x )> (10 * 1024))
+ return NULL;
+
+ ptr = (uint32_t *)calloc(x + 4, 1);
+ *ptr = (uint32_t)x;
+ cur_mem += x;
+ if (cur_mem > max_mem) {
+ max_mem = cur_mem;
+ printf("max mem: %lu\n", max_mem);
+ }
+ return (void*)(ptr + 1);
+}
+
+static inline void pico_free(void *x)
+{
+ uint32_t *ptr = (uint32_t*)(((uint8_t *)x) - 4);
+ cur_mem -= *ptr;
+ free(ptr);
+}
+#endif
+
+#define PICO_SUPPORT_MUTEX
+extern void *pico_mutex_init(void);
+extern void pico_mutex_lock(void*);
+extern void pico_mutex_unlock(void*);
+
+
+extern uint32_t os_time;
+
+static inline unsigned long PICO_TIME(void)
+{
+ return (unsigned long)os_time / 1000;
+}
+
+static inline unsigned long PICO_TIME_MS(void)
+{
+ return (unsigned long)os_time;
+}
+
+static inline void PICO_IDLE(void)
+{
+ // TODO needs implementation
+}
+/*
+static inline void PICO_DEBUG(const char * formatter, ... )
+{
+ char buffer[256];
+ char *ptr;
+ va_list args;
+ va_start(args, formatter);
+ vsnprintf(buffer, 256, formatter, args);
+ ptr = buffer;
+ while(*ptr != '\0')
+ serial_putc(serial_t *obj, (int) (*(ptr++)));
+ va_end(args);
+ //TODO implement serial_t
+}*/
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/heap.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,81 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+
+#define DECLARE_HEAP(type, orderby) \
+struct heap_##type { \
+ uint32_t size; \
+ uint32_t n; \
+ type *top; \
+}; \
+typedef struct heap_##type heap_##type; \
+static inline int heap_insert(struct heap_##type *heap, type *el) \
+{ \
+ int i; \
+ type * newTop; \
+ if (++heap->n >= heap->size) { \
+ newTop = pico_zalloc((heap->n + 1) * sizeof(type)); \
+ if(!newTop) \
+ return -1; \
+ if (heap->top) {\
+ memcpy(newTop,heap->top,heap->n*sizeof(type)); \
+ pico_free(heap->top); \
+ } \
+ heap->top = newTop; \
+ heap->size++; \
+ } \
+ if (heap->n == 1) { \
+ memcpy(&heap->top[1], el, sizeof(type)); \
+ return 0; \
+ } \
+ for (i = heap->n; ((i > 1) && (heap->top[i / 2].orderby > el->orderby)); i /= 2) { \
+ memcpy(&heap->top[i], &heap->top[i / 2], sizeof(type)); \
+ } \
+ memcpy(&heap->top[i], el, sizeof(type)); \
+ return 0; \
+} \
+static inline int heap_peek(struct heap_##type *heap, type *first) \
+{ \
+ type *last; \
+ uint32_t i, child; \
+ if(heap->n == 0) { \
+ return -1; \
+ } \
+ memcpy(first, &heap->top[1], sizeof(type)); \
+ last = &heap->top[heap->n--]; \
+ for(i = 1; (i * 2) <= heap->n; i = child) { \
+ child = 2 * i; \
+ if ((child != heap->n) && \
+ (heap->top[child + 1]).orderby \
+ < (heap->top[child]).orderby) \
+ child++; \
+ if (last->orderby > \
+ heap->top[child].orderby) \
+ memcpy(&heap->top[i], &heap->top[child],\
+ sizeof(type)); \
+ else \
+ break; \
+ } \
+ memcpy(&heap->top[i], last, sizeof(type)); \
+ return 0; \
+} \
+static inline type *heap_first(heap_##type *heap) \
+{ \
+ if (heap->n == 0) \
+ return NULL; \
+ return &heap->top[1]; \
+} \
+static inline heap_##type *heap_init(void) \
+{ \
+ heap_##type *p = (heap_##type *)pico_zalloc(sizeof(heap_##type)); \
+ return p; \
+} \
+static inline void heap_destroy(heap_##type *h) \
+{ \
+ pico_free(h->top); \
+ pico_free(h); \
+} \
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_addressing.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,52 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_ADDRESSING
+#define _INCLUDE_PICO_ADDRESSING
+#include <stdint.h>
+
+
+struct pico_ip4
+{
+ uint32_t addr;
+};
+#define PICO_SIZE_IP4 4
+
+
+struct pico_ip6
+{
+ uint8_t addr[16];
+};
+#define PICO_SIZE_IP6 16
+
+struct pico_eth
+{
+ uint8_t addr[6];
+ uint8_t padding[2];
+};
+#define PICO_SIZE_ETH 6
+
+extern const uint8_t PICO_ETHADDR_ALL[];
+
+
+struct pico_trans
+{
+ uint16_t sport;
+ uint16_t dport;
+
+};
+#define PICO_SIZE_TRANS 8
+
+
+/* Here are some protocols. */
+#define PICO_PROTO_IPV4 0
+#define PICO_PROTO_ICMP4 1
+#define PICO_PROTO_IGMP 2
+#define PICO_PROTO_TCP 6
+#define PICO_PROTO_UDP 17
+#define PICO_PROTO_IPV6 41
+#define PICO_PROTO_ICMP6 58
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/pico_arp.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,25 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +*********************************************************************/ +#ifndef _INCLUDE_PICO_ARP +#define _INCLUDE_PICO_ARP +#include "pico_eth.h" +#include "pico_device.h" + +int pico_arp_receive(struct pico_frame *); + + +struct pico_eth *pico_arp_get(struct pico_frame *f); +int pico_arp_query(struct pico_device *dev, struct pico_ip4 *dst); + +#define PICO_ARP_STATUS_REACHABLE 0x00 +#define PICO_ARP_STATUS_PERMANENT 0x01 +#define PICO_ARP_STATUS_STALE 0x02 + + +struct pico_eth *pico_arp_lookup(struct pico_ip4 *dst); +struct pico_ip4 *pico_arp_reverse_lookup(struct pico_eth *dst); +int pico_arp_create_entry(uint8_t* hwaddr, struct pico_ip4 ipv4, struct pico_device* dev); +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_constants.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,121 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_CONST
+#define _INCLUDE_PICO_CONST
+/* Included from pico_config.h */
+/** Endian-dependant constants **/
+
+extern volatile unsigned long long pico_tick;
+
+#ifdef PICO_BIGENDIAN
+
+# define PICO_IDETH_IPV4 0x0800
+# define PICO_IDETH_ARP 0x0806
+# define PICO_IDETH_IPV6 0x86DD
+
+# define PICO_ARP_REQUEST 0x0001
+# define PICO_ARP_REPLY 0x0002
+# define PICO_ARP_HTYPE_ETH 0x0001
+
+#define short_be(x) (x)
+#define long_be(x) (x)
+
+static inline uint16_t short_from(void *_p)
+{
+ unsigned char *p = (unsigned char *)_p;
+ uint16_t r, p0, p1;
+ p0 = p[0];
+ p1 = p[1];
+ r = (p0 << 8) + p1;
+ return r;
+}
+
+static inline uint32_t long_from(void *_p)
+{
+ unsigned char *p = (unsigned char *)_p;
+ uint32_t r, p0, p1, p2, p3;
+ p0 = p[0];
+ p1 = p[1];
+ p2 = p[2];
+ p3 = p[3];
+ r = (p0 << 24) + (p1 << 16) + (p2 << 8) + p3;
+ return r;
+}
+
+#else
+
+static inline uint16_t short_from(void *_p)
+{
+ unsigned char *p = (unsigned char *)_p;
+ uint16_t r, p0, p1;
+ p0 = p[0];
+ p1 = p[1];
+ r = (p1 << 8u) + p0;
+ return r;
+}
+
+static inline uint32_t long_from(void *_p)
+{
+ unsigned char *p = (unsigned char *)_p;
+ uint32_t r, p0, p1, p2, p3;
+ p0 = p[0];
+ p1 = p[1];
+ p2 = p[2];
+ p3 = p[3];
+ r = (p3 << 24) + (p2 << 16) + (p1 << 8) + p0;
+ return r;
+}
+
+
+# define PICO_IDETH_IPV4 0x0008
+# define PICO_IDETH_ARP 0x0608
+# define PICO_IDETH_IPV6 0xDD86
+
+# define PICO_ARP_REQUEST 0x0100
+# define PICO_ARP_REPLY 0x0200
+# define PICO_ARP_HTYPE_ETH 0x0100
+
+static inline uint16_t short_be(uint16_t le)
+{
+ return ((le & 0xFFu) << 8) | ((le >> 8u) & 0xFFu);
+}
+
+static inline uint32_t long_be(uint32_t le)
+{
+ uint8_t *b = (uint8_t *)≤
+ uint32_t be = 0;
+ uint32_t b0, b1, b2;
+ b0 = b[0];
+ b1 = b[1];
+ b2 = b[2];
+ be = b[3] + (b2 << 8) + (b1 << 16) + (b0 << 24);
+ return be;
+}
+#endif
+
+
+/* Add well-known host numbers here. (bigendian constants only beyond this point) */
+#define PICO_IP4_ANY (0x00000000U)
+#define PICO_IP4_BCAST (0xffffffffU)
+
+/* defined in modules/pico_ipv6.c */
+#ifdef PICO_SUPPORT_IPV6
+extern const uint8_t PICO_IPV6_ANY[PICO_SIZE_IP6];
+#endif
+
+static inline uint32_t pico_hash(const char *name)
+{
+ unsigned long hash = 5381;
+ int c;
+ while ((c = *name++))
+ hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
+ return hash;
+}
+
+/* Debug */
+//#define PICO_SUPPORT_DEBUG_MEMORY
+//#define PICO_SUPPORT_DEBUG_TOOLS
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_device.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,41 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_DEVICE
+#define _INCLUDE_PICO_DEVICE
+#include "pico_queue.h"
+#include "pico_frame.h"
+#include "pico_addressing.h"
+#include "pico_tree.h"
+#define MAX_DEVICE_NAME 16
+
+
+struct pico_ethdev {
+ struct pico_eth mac;
+};
+
+struct pico_device {
+ char name[MAX_DEVICE_NAME];
+ uint32_t hash;
+ uint32_t overhead;
+ struct pico_ethdev *eth; /* Null if non-ethernet */
+ struct pico_queue *q_in;
+ struct pico_queue *q_out;
+ int (*send)(struct pico_device *self, void *buf, int len); /* Send function. Return 0 if busy */
+ int (*poll)(struct pico_device *self, int loop_score);
+ void(*destroy)(struct pico_device *self);
+ int (*dsr)(struct pico_device *self, int loop_score);
+ int __serving_interrupt;
+ // used to signal the upper layer the number of events arrived since the last processing
+ volatile int eventCnt;
+};
+
+int pico_device_init(struct pico_device *dev, const char *name, uint8_t *mac);
+void pico_device_destroy(struct pico_device *dev);
+int pico_devices_loop(int loop_score, int direction);
+struct pico_device* pico_get_device(const char* name);
+int pico_device_broadcast(struct pico_frame * f);
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_eth.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,21 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_ETH
+#define _INCLUDE_PICO_ETH
+#include "pico_addressing.h"
+#include "pico_ipv4.h"
+#include "pico_ipv6.h"
+
+
+struct __attribute__((packed)) pico_eth_hdr {
+ uint8_t daddr[6];
+ uint8_t saddr[6];
+ uint16_t proto;
+};
+
+#define PICO_SIZE_ETHHDR 14
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_frame.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,95 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_FRAME
+#define _INCLUDE_PICO_FRAME
+#include "pico_config.h"
+
+
+#define PICO_FRAME_FLAG_BCAST (0x01)
+#define PICO_FRAME_FLAG_SACKED (0x80)
+#define IS_BCAST(f) ((f->flags & PICO_FRAME_FLAG_BCAST) == PICO_FRAME_FLAG_BCAST)
+
+
+struct pico_socket;
+
+
+struct pico_frame {
+
+ /* Connector for queues */
+ struct pico_frame *next;
+
+ /* Start of the whole buffer, total frame length. */
+ unsigned char *buffer;
+ uint32_t buffer_len;
+
+ /* For outgoing packets: this is the meaningful buffer. */
+ unsigned char *start;
+ uint32_t len;
+
+ /* Pointer to usage counter */
+ uint32_t *usage_count;
+
+ /* Pointer to protocol headers */
+ uint8_t *datalink_hdr;
+
+ uint8_t *net_hdr;
+ int net_len;
+ uint8_t *transport_hdr;
+ int transport_len;
+ uint8_t *app_hdr;
+ int app_len;
+
+ /* Pointer to the phisical device this packet belongs to.
+ * Should be valid in both routing directions
+ */
+ struct pico_device *dev;
+
+ unsigned long timestamp;
+
+ /* Failures due to bad datalink addressing. */
+ uint16_t failure_count;
+
+ /* Protocol over IP */
+ uint8_t proto;
+
+ /* PICO_FRAME_FLAG_* */
+ uint8_t flags;
+
+ /* Pointer to payload */
+ unsigned char *payload;
+ int payload_len;
+
+#ifdef PICO_SUPPORT_IPFRAG
+ /* Payload fragmentation info (big endian)*/
+ uint16_t frag;
+#endif
+
+ /* Pointer to socket */
+ struct pico_socket *sock;
+
+ /* Pointer to transport info, used to store remote UDP duple (IP + port) */
+ void *info;
+
+ /*Priority. "best-effort" priority, the default value is 0. Priority can be in between -10 and +10*/
+ int8_t priority;
+};
+
+/** frame alloc/dealloc/copy **/
+void pico_frame_discard(struct pico_frame *f);
+struct pico_frame *pico_frame_copy(struct pico_frame *f);
+struct pico_frame *pico_frame_deepcopy(struct pico_frame *f);
+struct pico_frame *pico_frame_alloc(int size);
+uint16_t pico_checksum(void *inbuf, int len);
+uint16_t pico_dualbuffer_checksum(void *b1, int len1, void *b2, int len2);
+
+static inline int pico_is_digit(char c)
+{
+ if (c < '0' || c > '9')
+ return 0;
+ return 1;
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_module_eth.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,33 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _PICO_MODULE_IPV4_H
+#define _PICO_MODULE_IPV4_H
+
+struct pico_arp_entry {
+ struct eth dest;
+#ifdef PICO_CONFIG_IPV4
+ struct ipv4 addr_ipv4;
+#endif
+ RB_ENTRY(pico_arp_entry) node;
+};
+
+/* Configured device */
+struct pico_eth_link {
+ struct pico_device *dev;
+ struct eth address;
+ struct eth netmask;
+ RB_ENTRY(pico_eth_link) node;
+};
+
+#ifndef IS_MODULE_ETH
+# define _mod extern
+#else
+# define _mod
+#endif
+_mod struct pico_module pico_module_eth;
+#undef _mod
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_protocol.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,95 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_PROTOCOL
+#define _INCLUDE_PICO_PROTOCOL
+#include <stdint.h>
+#include "pico_queue.h"
+
+#define PICO_LOOP_DIR_IN 1
+#define PICO_LOOP_DIR_OUT 2
+
+enum pico_layer {
+ PICO_LAYER_DATALINK = 2, /* Ethernet only. */
+ PICO_LAYER_NETWORK = 3, /* IPv4, IPv6, ARP. Arp is there because it communicates with L2 */
+ PICO_LAYER_TRANSPORT = 4, /* UDP, TCP, ICMP */
+ PICO_LAYER_SOCKET = 5 /* Socket management */
+};
+
+enum pico_err_e {
+ PICO_ERR_NOERR = 0,
+ PICO_ERR_EPERM,
+ PICO_ERR_ENOENT,
+ /* ... */
+ PICO_ERR_EINTR = 4,
+ PICO_ERR_EIO,
+ PICO_ERR_ENXIO,
+ /* ... */
+ PICO_ERR_EAGAIN = 11,
+ PICO_ERR_ENOMEM,
+ PICO_ERR_EACCESS,
+ PICO_ERR_EFAULT,
+ /* ... */
+ PICO_ERR_EBUSY = 16,
+ PICO_ERR_EEXIST = 17,
+ /* ... */
+ PICO_ERR_EINVAL = 22,
+ /* ... */
+ PICO_ERR_EPROTO = 71,
+ PICO_ERR_ENOPROTOOPT = 92,
+ PICO_ERR_EPROTONOSUPPORT = 93,
+
+ /* ... */
+ PICO_ERR_EADDRINUSE = 98,
+ PICO_ERR_EADDRNOTAVAIL,
+ PICO_ERR_ENETUNREACH,
+
+ /* ... */
+ PICO_ERR_ECONNRESET = 104,
+
+ /* ... */
+ PICO_ERR_EISCONN = 106,
+ PICO_ERR_ENOTCONN,
+ PICO_ERR_ESHUTDOWN,
+ /* ... */
+ PICO_ERR_ETIMEDOUT = 110,
+ PICO_ERR_ECONNREFUSED = 111,
+ PICO_ERR_EHOSTDOWN,
+ PICO_ERR_EHOSTUNREACH,
+ /* ... */
+ PICO_ERR_EOPNOTSUPP = 122,
+
+};
+
+typedef enum pico_err_e pico_err_t;
+extern volatile pico_err_t pico_err;
+
+#define IS_IPV6(f) ((((uint8_t *)(f->net_hdr))[0] & 0xf0) == 0x60)
+#define IS_IPV4(f) ((((uint8_t *)(f->net_hdr))[0] & 0xf0) == 0x40)
+
+#define MAX_PROTOCOL_NAME 16
+
+struct pico_protocol {
+ const char name[MAX_PROTOCOL_NAME];
+ uint32_t hash;
+ const enum pico_layer layer;
+ const int proto_number;
+ struct pico_queue * const q_in;
+ struct pico_queue * const q_out;
+ struct pico_frame *(* const alloc)(struct pico_protocol *self, int size); /* Frame allocation. */
+ int (* const push) (struct pico_protocol *self, struct pico_frame *p); /* Push function, for active outgoing pkts from above */
+ int (* const process_out)(struct pico_protocol *self, struct pico_frame *p); /* Send loop. */
+ int (* const process_in)(struct pico_protocol *self, struct pico_frame *p); /* Recv loop. */
+};
+
+int pico_protocols_loop(int loop_score);
+void pico_protocol_init(struct pico_protocol *p);
+
+int pico_protocol_datalink_loop(int loop_score, int direction);
+int pico_protocol_network_loop(int loop_score, int direction);
+int pico_protocol_transport_loop(int loop_score, int direction);
+int pico_protocol_socket_loop(int loop_score, int direction);
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_queue.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,137 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_QUEUE
+#define _INCLUDE_PICO_QUEUE
+#include <stdint.h>
+#include "pico_config.h"
+#include "pico_frame.h"
+
+#define Q_LIMIT 0
+
+#ifndef NULL
+#define NULL ((void *)0)
+#endif
+
+struct pico_queue {
+ uint32_t frames;
+ uint32_t size;
+ uint32_t max_frames;
+ uint32_t max_size;
+ struct pico_frame *head;
+ struct pico_frame *tail;
+#ifdef PICO_SUPPORT_MUTEX
+ void * mutex;
+#endif
+ uint8_t shared;
+};
+
+#ifdef PICO_SUPPORT_MUTEX
+#define LOCK(x) {\
+ if (x == NULL) \
+ x = pico_mutex_init(); \
+ pico_mutex_lock(x); \
+}
+#define UNLOCK(x) pico_mutex_unlock(x);
+
+#else
+#define LOCK(x) do{}while(0)
+#define UNLOCK(x) do{}while(0)
+#endif
+
+#ifdef PICO_SUPPORT_DEBUG_TOOLS
+static void debug_q(struct pico_queue *q)
+{
+ struct pico_frame *p = q->head;
+ dbg("%d: ", q->frames);
+ while(p) {
+ dbg("(%p)-->", p);
+ p = p->next;
+ }
+ dbg("X\n");
+}
+
+#else
+
+#define debug_q(x) do{}while(0)
+#endif
+
+static inline int pico_enqueue(struct pico_queue *q, struct pico_frame *p)
+{
+ if ((q->max_frames) && (q->max_frames <= q->frames))
+ return -1;
+
+ if ((Q_LIMIT) && (Q_LIMIT < p->buffer_len + q->size))
+ return -1;
+
+ if ((q->max_size) && (q->max_size < (p->buffer_len + q->size)))
+ return -1;
+
+ if (q->shared)
+ LOCK(q->mutex);
+
+ p->next = NULL;
+ if (!q->head) {
+ q->head = p;
+ q->tail = p;
+ q->size = 0;
+ q->frames = 0;
+ } else {
+ q->tail->next = p;
+ q->tail = p;
+ }
+ q->size += p->buffer_len;
+ q->frames++;
+ debug_q(q);
+
+ if (q->shared)
+ UNLOCK(q->mutex);
+ return q->size;
+}
+
+static inline struct pico_frame *pico_dequeue(struct pico_queue *q)
+{
+ struct pico_frame *p = q->head;
+ if (q->frames < 1)
+ return NULL;
+ if (q->shared)
+ LOCK(q->mutex);
+
+ q->head = p->next;
+ q->frames--;
+ q->size -= p->buffer_len;
+ if (q->head == NULL)
+ q->tail = NULL;
+ debug_q(q);
+ p->next = NULL;
+ if (q->shared)
+ UNLOCK(q->mutex);
+ return p;
+}
+
+static inline struct pico_frame *pico_queue_peek(struct pico_queue *q)
+{
+ struct pico_frame *p = q->head;
+ if (q->frames < 1)
+ return NULL;
+ debug_q(q);
+ return p;
+}
+
+static inline void pico_queue_empty(struct pico_queue *q)
+{
+ struct pico_frame *p = pico_dequeue(q);
+ while(p) {
+ pico_free(p);
+ p = pico_dequeue(q);
+ }
+}
+
+static inline void pico_queue_protect(struct pico_queue *q)
+{
+ q->shared = 1;
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_socket.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,208 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_SOCKET
+#define _INCLUDE_PICO_SOCKET
+#include "pico_queue.h"
+#include "pico_addressing.h"
+#include "pico_config.h"
+#include "pico_protocol.h"
+
+//#define PICO_DEFAULT_SOCKETQ (128 * 1024)
+#define PICO_DEFAULT_SOCKETQ (4 * 1024)
+
+#define PICO_SHUT_RD 1
+#define PICO_SHUT_WR 2
+#define PICO_SHUT_RDWR 3
+
+
+struct pico_socket {
+ struct pico_protocol *proto;
+ struct pico_protocol *net;
+
+ union {
+ struct pico_ip4 ip4;
+ struct pico_ip6 ip6;
+ } local_addr;
+
+ union {
+ struct pico_ip4 ip4;
+ struct pico_ip6 ip6;
+ } remote_addr;
+
+ uint16_t local_port;
+ uint16_t remote_port;
+
+ struct pico_queue q_in;
+ struct pico_queue q_out;
+
+ void (*wakeup)(uint16_t ev, struct pico_socket *s);
+
+
+#ifdef PICO_SUPPORT_TCP
+ /* For the TCP backlog queue */
+ struct pico_socket *backlog;
+ struct pico_socket *next;
+ struct pico_socket *parent;
+ int max_backlog;
+#endif
+#ifdef PICO_SUPPORT_MCAST
+ struct pico_tree *MCASTListen;
+#endif
+ uint16_t ev_pending;
+
+ struct pico_device *dev;
+
+ /* Private field. */
+ int id;
+ uint16_t state;
+ uint16_t opt_flags;
+ uint32_t timestamp;
+ void *priv;
+};
+
+struct pico_remote_duple {
+ union {
+ struct pico_ip4 ip4;
+ struct pico_ip6 ip6;
+ } remote_addr;
+
+ uint16_t remote_port;
+};
+
+
+/* request struct for multicast socket opt */
+struct pico_ip_mreq {
+ struct pico_ip4 mcast_group_addr;
+ struct pico_ip4 mcast_link_addr;
+};
+
+struct pico_ip_mreq_source {
+ struct pico_ip4 mcast_group_addr;
+ struct pico_ip4 mcast_source_addr;
+ struct pico_ip4 mcast_link_addr;
+};
+
+#define PICO_SOCKET_STATE_UNDEFINED 0x0000
+#define PICO_SOCKET_STATE_SHUT_LOCAL 0x0001
+#define PICO_SOCKET_STATE_SHUT_REMOTE 0x0002
+#define PICO_SOCKET_STATE_BOUND 0x0004
+#define PICO_SOCKET_STATE_CONNECTED 0x0008
+#define PICO_SOCKET_STATE_CLOSING 0x0010
+#define PICO_SOCKET_STATE_CLOSED 0x0020
+
+# define PICO_SOCKET_STATE_TCP 0xFF00
+# define PICO_SOCKET_STATE_TCP_UNDEF 0x00FF
+# define PICO_SOCKET_STATE_TCP_CLOSED 0x0100
+# define PICO_SOCKET_STATE_TCP_LISTEN 0x0200
+# define PICO_SOCKET_STATE_TCP_SYN_SENT 0x0300
+# define PICO_SOCKET_STATE_TCP_SYN_RECV 0x0400
+# define PICO_SOCKET_STATE_TCP_ESTABLISHED 0x0500
+# define PICO_SOCKET_STATE_TCP_CLOSE_WAIT 0x0600
+# define PICO_SOCKET_STATE_TCP_LAST_ACK 0x0700
+# define PICO_SOCKET_STATE_TCP_FIN_WAIT1 0x0800
+# define PICO_SOCKET_STATE_TCP_FIN_WAIT2 0x0900
+# define PICO_SOCKET_STATE_TCP_CLOSING 0x0a00
+# define PICO_SOCKET_STATE_TCP_TIME_WAIT 0x0b00
+# define PICO_SOCKET_STATE_TCP_ARRAYSIZ 0x0c
+
+# define PICO_TCP_NODELAY 1
+
+# define PICO_SOCKET_OPT_TCPNODELAY 0x0000
+
+# define PICO_IP_MULTICAST_EXCLUDE 0
+# define PICO_IP_MULTICAST_INCLUDE 1
+# define PICO_IP_MULTICAST_IF 32
+# define PICO_IP_MULTICAST_TTL 33
+# define PICO_IP_MULTICAST_LOOP 34
+# define PICO_IP_ADD_MEMBERSHIP 35
+# define PICO_IP_DROP_MEMBERSHIP 36
+# define PICO_IP_UNBLOCK_SOURCE 37
+# define PICO_IP_BLOCK_SOURCE 38
+# define PICO_IP_ADD_SOURCE_MEMBERSHIP 39
+# define PICO_IP_DROP_SOURCE_MEMBERSHIP 40
+
+# define PICO_SOCKET_OPT_MULTICAST_LOOP 1
+
+# define PICO_IP_DEFAULT_MULTICAST_TTL 1
+# define PICO_IP_DEFAULT_MULTICAST_LOOP 1
+
+#define PICO_SOCKET_TIMEOUT 90000u //1.5 minutes
+
+#define PICO_SOCKET_SHUTDOWN_WRITE 0x01
+#define PICO_SOCKET_SHUTDOWN_READ 0x02
+#define TCPSTATE(s) ((s)->state & PICO_SOCKET_STATE_TCP)
+
+#define PICO_SOCK_EV_RD 1
+#define PICO_SOCK_EV_WR 2
+#define PICO_SOCK_EV_CONN 4
+#define PICO_SOCK_EV_CLOSE 8
+#define PICO_SOCK_EV_FIN 0x10
+#define PICO_SOCK_EV_ERR 0x80
+
+
+struct pico_socket *pico_socket_open(uint16_t net, uint16_t proto, void (*wakeup)(uint16_t ev, struct pico_socket *s));
+
+int pico_socket_read(struct pico_socket *s, void *buf, int len);
+int pico_socket_write(struct pico_socket *s, const void *buf, int len);
+
+int pico_socket_sendto(struct pico_socket *s, const void *buf, int len, void *dst, uint16_t remote_port);
+int pico_socket_recvfrom(struct pico_socket *s, void *buf, int len, void *orig, uint16_t *local_port);
+
+int pico_socket_send(struct pico_socket *s, const void *buf, int len);
+int pico_socket_recv(struct pico_socket *s, void *buf, int len);
+
+int pico_socket_bind(struct pico_socket *s, void *local_addr, uint16_t *port);
+int pico_socket_connect(struct pico_socket *s, const void *srv_addr, uint16_t remote_port);
+int pico_socket_listen(struct pico_socket *s, const int backlog);
+struct pico_socket *pico_socket_accept(struct pico_socket *s, void *orig, uint16_t *port);
+int pico_socket_del(struct pico_socket *s);
+
+int pico_socket_setoption(struct pico_socket *s, int option, void *value);
+int pico_socket_getoption(struct pico_socket *s, int option, void *value);
+
+int pico_socket_shutdown(struct pico_socket *s, int mode);
+int pico_socket_close(struct pico_socket *s);
+
+struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, int len);
+
+#ifdef PICO_SUPPORT_IPV4
+# define is_sock_ipv4(x) (x->net == &pico_proto_ipv4)
+#else
+# define is_sock_ipv4(x) (0)
+#endif
+
+#ifdef PICO_SUPPORT_IPV6
+# define is_sock_ipv6(x) (x->net == &pico_proto_ipv6)
+#else
+# define is_sock_ipv6(x) (0)
+#endif
+
+#ifdef PICO_SUPPORT_UDP
+# define is_sock_udp(x) (x->net == &pico_proto_udp)
+#else
+# define is_sock_udp(x) (0)
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+# define is_sock_tcp(x) (x->net == &pico_proto_tcp)
+#else
+# define is_sock_tcp(x) (0)
+#endif
+
+/* Interface towards transport protocol */
+int pico_transport_process_in(struct pico_protocol *self, struct pico_frame *f);
+struct pico_socket *pico_socket_clone(struct pico_socket *facsimile);
+int pico_socket_add(struct pico_socket *s);
+int pico_transport_error(struct pico_frame *f, uint8_t proto, int code);
+
+/* Socket loop */
+int pico_sockets_loop(int loop_score);
+struct pico_socket* pico_sockets_find(uint16_t local,uint16_t remote);
+/* Port check */
+int pico_is_port_free(uint16_t proto, uint16_t port, void *addr, void *net);
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/pico_stack.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,68 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +*********************************************************************/ +#ifndef _INCLUDE_PICO_STACK +#define _INCLUDE_PICO_STACK +#include "pico_config.h" +#include "pico_frame.h" + +#define PICO_MAX_TIMERS 20 + +#define PICO_ETH_MTU 1514 +#define PICO_IP_MTU 1500 + +/* ===== RECEIVING FUNCTIONS (from dev up to socket) ===== */ + +/* TRANSPORT LEVEL */ +/* interface towards network */ +int pico_transport_receive(struct pico_frame *f, uint8_t proto); + +/* NETWORK LEVEL */ +/* interface towards ethernet */ +int pico_network_receive(struct pico_frame *f); + +/* The pico_ethernet_receive() function is used by + * those devices supporting ETH in order to push packets up + * into the stack. + */ +/* DATALINK LEVEL */ +int pico_ethernet_receive(struct pico_frame *f); + +/* LOWEST LEVEL: interface towards devices. */ +/* Device driver will call this function which returns immediately. + * Incoming packet will be processed later on in the dev loop. + */ +int pico_stack_recv(struct pico_device *dev, uint8_t *buffer, int len); + + +/* ===== SENDIING FUNCTIONS (from socket down to dev) ===== */ + +int pico_transport_send(struct pico_frame *f); +int pico_network_send(struct pico_frame *f); +int pico_ethernet_send(struct pico_frame *f); +int pico_sendto_dev(struct pico_frame *f); + +/* ----- Initialization ----- */ +void pico_stack_init(void); + +/* ----- Loop Function. ----- */ +void pico_stack_tick(void); +void pico_stack_loop(void); + +/* ---- Notifications for stack errors */ +int pico_notify_socket_unreachable(struct pico_frame *f); +int pico_notify_proto_unreachable(struct pico_frame *f); +int pico_notify_dest_unreachable(struct pico_frame *f); +int pico_notify_ttl_expired(struct pico_frame *f); + +/* Various. */ +int pico_source_is_local(struct pico_frame *f); +int pico_destination_is_local(struct pico_frame *f); +void pico_store_network_origin(void *src, struct pico_frame *f); +void pico_timer_add(unsigned long expire, void (*timer)(unsigned long, void *), void *arg); +uint32_t pico_rand(void); +void pico_rand_feed(uint32_t feed); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_tree.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,84 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Author: Andrei Carp <andrei.carp@tass.be>
+*********************************************************************/
+
+#ifndef __PICO_RBTREE_H__
+#define __PICO_RBTREE_H__
+
+#include <stdint.h>
+#include "pico_config.h"
+
+// This is used to declare a new tree, leaf root by default
+#define PICO_TREE_DECLARE(name,compareFunction) \
+ struct pico_tree name =\
+ { \
+ &LEAF, \
+ compareFunction \
+ }
+
+struct pico_tree_node
+{
+ void* keyValue; // generic key
+ struct pico_tree_node* parent;
+ struct pico_tree_node* leftChild;
+ struct pico_tree_node* rightChild;
+ uint8_t color;
+};
+
+struct pico_tree
+{
+ struct pico_tree_node * root; // root of the tree
+
+ // this function directly provides the keys as parameters not the nodes.
+ int (*compare)(void* keyA, void* keyB);
+};
+
+extern struct pico_tree_node LEAF; // generic leaf node
+/*
+ * Manipulation functions
+ */
+void * pico_tree_insert(struct pico_tree * tree, void * key);
+void * pico_tree_delete(struct pico_tree * tree, void * key);
+void * pico_tree_findKey(struct pico_tree * tree, void * key);
+void pico_tree_drop(struct pico_tree * tree);
+int pico_tree_empty(struct pico_tree * tree);
+struct pico_tree_node * pico_tree_findNode(struct pico_tree * tree, void * key);
+
+void * pico_tree_first(struct pico_tree * tree);
+void * pico_tree_last(struct pico_tree * tree);
+/*
+ * Traverse functions
+ */
+struct pico_tree_node * pico_tree_lastNode(struct pico_tree_node * node);
+struct pico_tree_node * pico_tree_firstNode(struct pico_tree_node * node);
+struct pico_tree_node * pico_tree_next(struct pico_tree_node * node);
+struct pico_tree_node * pico_tree_prev(struct pico_tree_node * node);
+
+/*
+ * For each macros
+ */
+
+#define pico_tree_foreach(idx,tree) \
+ for ((idx) = pico_tree_firstNode((tree)->root); \
+ (idx) != &LEAF; \
+ (idx) = pico_tree_next(idx))
+
+#define pico_tree_foreach_reverse(idx,tree) \
+ for ((idx) = pico_tree_lastNode((tree)->root); \
+ (idx) != &LEAF; \
+ (idx) = pico_tree_prev(idx))
+
+#define pico_tree_foreach_safe(idx,tree,idx2) \
+ for ((idx) = pico_tree_firstNode((tree)->root); \
+ ((idx) != &LEAF) && ((idx2) = pico_tree_next(idx), 1); \
+ (idx) = (idx2))
+
+#define pico_tree_foreach_reverse_safe(idx,tree,idx2) \
+ for ((idx) = pico_tree_lastNode((tree)->root); \
+ ((idx) != &LEAF) && ((idx2) = pico_tree_prev(idx), 1); \
+ (idx) = (idx2))
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dev_loop.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,70 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_device.h"
+#include "pico_dev_loop.h"
+#include "pico_stack.h"
+
+
+#define LOOP_MTU 1500
+static uint8_t l_buf[LOOP_MTU];
+static int l_bufsize = 0;
+
+
+static int pico_loop_send(struct pico_device *dev, void *buf, int len)
+{
+ IGNORE_PARAMETER(dev);
+ if (len > LOOP_MTU)
+ return 0;
+
+ if (l_bufsize == 0) {
+ memcpy(l_buf, buf, len);
+ l_bufsize+=len;
+ return len;
+ }
+ return 0;
+}
+
+static int pico_loop_poll(struct pico_device *dev, int loop_score)
+{
+ if (loop_score <= 0)
+ return 0;
+
+ if (l_bufsize > 0) {
+ pico_stack_recv(dev, l_buf, l_bufsize);
+ l_bufsize = 0;
+ loop_score--;
+ }
+ return loop_score;
+}
+
+/* Public interface: create/destroy. */
+
+void pico_loop_destroy(struct pico_device *dev)
+{
+ IGNORE_PARAMETER(dev);
+}
+
+struct pico_device *pico_loop_create(void)
+{
+ struct pico_device *loop = pico_zalloc(sizeof(struct pico_device));
+ if (!loop)
+ return NULL;
+
+ if( 0 != pico_device_init((struct pico_device *)loop, "loop", NULL)) {
+ dbg ("Loop init failed.\n");
+ pico_loop_destroy((struct pico_device *)loop);
+ return NULL;
+ }
+ loop->send = pico_loop_send;
+ loop->poll = pico_loop_poll;
+ loop->destroy = pico_loop_destroy;
+ dbg("Device %s created.\n", loop->name);
+ return (struct pico_device *)loop;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_dev_loop.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,15 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +*********************************************************************/ +#ifndef _INCLUDE_PICO_LOOP +#define _INCLUDE_PICO_LOOP +#include "pico_config.h" +#include "pico_device.h" + +void pico_loop_destroy(struct pico_device *loop); +struct pico_device *pico_loop_create(void); + +#endif +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dev_mbed.cpp Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,118 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+Do not redistribute without a written permission by the Copyright
+holders.
+
+Authors: Toon Peters, Maxime Vincent
+*********************************************************************/
+#include "mbed.h"
+extern "C" {
+#include "pico_device.h"
+#include "pico_dev_mbed.h"
+#include "pico_stack.h"
+#include "ethernet_api.h"
+}
+
+struct pico_device_mbed {
+ struct pico_device dev;
+ int bytes_left_in_frame;
+};
+
+#define ETH_MTU 1514
+uint8_t buf[ETH_MTU];
+
+Serial pc(p9, p10, "Serial port"); // tx, rx
+
+extern "C" {
+
+static int pico_mbed_send(struct pico_device *dev, void *buf, int len)
+{
+ int ret, sent;
+ struct pico_device_mbed *mb = (struct pico_device_mbed *) dev;
+
+ if (len > ETH_MTU)
+ return -1;
+
+ /* Write buf content to dev and return amount written */
+ ret = ethernet_write((const char *)buf, len);
+ sent = ethernet_send();
+
+ pc.printf("ETH> sent %d bytes\r\n",ret);
+ if (len != ret || sent != ret)
+ return -1;
+ else
+ return ret;
+}
+
+static int pico_mbed_poll(struct pico_device *dev, int loop_score)
+{
+ int len;
+ struct pico_device_mbed *mb = (struct pico_device_mbed *) dev;
+
+ while(loop_score > 0)
+ {
+ /* check for new frame(s) */
+ len = (int) ethernet_receive();
+
+ /* return if no frame has arrived */
+ if (!len)
+ return loop_score;
+
+ /* read and process frame */
+ len = ethernet_read((char*)buf, ETH_MTU);
+ pc.printf("ETH> recv %d bytes: %x:%x\r\n", len, buf[0],buf[1]);
+ pico_stack_recv(dev, buf, len);
+ loop_score--;
+ }
+ return loop_score;
+}
+
+/* Public interface: create/destroy. */
+void pico_mbed_destroy(struct pico_device *dev)
+{
+ ethernet_free();
+ pico_device_destroy(dev);
+}
+
+struct pico_device *pico_mbed_create(char *name)
+{
+ std::uint8_t mac[PICO_SIZE_ETH];
+ struct pico_device_mbed *mb = (struct pico_device_mbed*) pico_zalloc(sizeof(struct pico_device_mbed));
+
+ if (!mb)
+ return NULL;
+
+ ethernet_address((char *)mac);
+ pc.printf("ETH> Set MAC address to: %x:%x:%x:%x:%x:%x\r\n", mac[0],mac[1],mac[2],mac[3],mac[4],mac[5]);
+
+ if(0 != pico_device_init((struct pico_device *)mb, name, mac)) {
+ pc.printf ("ETH> Loop init failed.\n");
+ //pico_loop_destroy(mb);
+ return NULL;
+ }
+
+ mb->dev.send = pico_mbed_send;
+ mb->dev.poll = pico_mbed_poll;
+ mb->dev.destroy = pico_mbed_destroy;
+ mb->bytes_left_in_frame = 0;
+
+ if(0 != ethernet_init()) {
+ pc.printf("ETH> Failed to initialize hardware.\r\n");
+ pico_device_destroy((struct pico_device *)mb);
+ return NULL;
+ }
+
+ // future work: make the mac address configurable
+
+ pc.printf("ETH> Device %s created.\r\n", mb->dev.name);
+
+ return (struct pico_device *)mb;
+}
+
+void pico_mbed_get_address(char *mac)
+{
+ ethernet_address(mac);
+}
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_dev_mbed.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,21 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. +Do not redistribute without a written permission by the Copyright +holders. + +File: pico_dev_mbed.h +Author: Toon Peters +*********************************************************************/ + +#ifndef PICO_DEV_MBED_H +#define PICO_DEV_MBED_H + +#include "pico_config.h" +#include "pico_device.h" + +void pico_mbed_destroy(struct pico_device *vde); +struct pico_device *pico_mbed_create(char *name); + +#endif /* PICO_DEV_MBED_H */ +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_client.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,920 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Kristof Roelants, Frederik Van Slycken
+*********************************************************************/
+
+
+#include "pico_dhcp_client.h"
+#include "pico_stack.h"
+#include "pico_config.h"
+#include "pico_device.h"
+#include "pico_ipv4.h"
+#include "pico_socket.h"
+#include "pico_eth.h"
+
+#ifdef PICO_SUPPORT_DHCPC
+#define dhcpc_dbg(...) do{}while(0)
+//#define dhcpc_dbg dbg
+
+/* timer values */
+#define DHCP_CLIENT_REINIT 3000 /* msec */
+#define DHCP_CLIENT_RETRANS 4 /* sec */
+#define DHCP_CLIENT_RETRIES 3
+
+#define DHCP_CLIENT_TIMER_STOPPED 0
+#define DHCP_CLIENT_TIMER_STARTED 1
+
+/* custom statuses */
+#define DHCP_CLIENT_STATUS_INIT 0
+#define DHCP_CLIENT_STATUS_SOCKET 1
+#define DHCP_CLIENT_STATUS_BOUND 2
+#define DHCP_CLIENT_STATUS_LINKED 3
+#define DHCP_CLIENT_STATUS_TRANSMITTED 4
+#define DHCP_CLIENT_STATUS_INITIALIZED 5
+
+/* maximum size of a DHCP message */
+#define DHCP_CLIENT_MAXMSGZISE PICO_IP_MTU
+
+/* serialize client negotiations if multiple devices */
+/* NOTE: ONLY initialization is serialized! */
+static uint8_t pico_dhcp_client_mutex = 1;
+
+enum dhcp_client_state {
+ DHCP_CLIENT_STATE_INIT_REBOOT = 0,
+ DHCP_CLIENT_STATE_REBOOTING,
+ DHCP_CLIENT_STATE_INIT,
+ DHCP_CLIENT_STATE_SELECTING,
+ DHCP_CLIENT_STATE_REQUESTING,
+ DHCP_CLIENT_STATE_BOUND,
+ DHCP_CLIENT_STATE_RENEWING,
+ DHCP_CLIENT_STATE_REBINDING
+};
+
+struct dhcp_client_timer
+{
+ uint8_t state;
+ uint32_t time;
+};
+
+struct pico_dhcp_client_cookie
+{
+ uint8_t status;
+ uint8_t event;
+ uint8_t retry;
+ uint32_t xid;
+ uint32_t *uid;
+ enum dhcp_client_state state;
+ void (*cb)(void* dhcpc, int code);
+ unsigned long init_timestamp;
+ struct pico_socket *s;
+ struct pico_ip4 address;
+ struct pico_ip4 netmask;
+ struct pico_ip4 gateway;
+ struct pico_ip4 nameserver;
+ struct pico_ip4 server_id;
+ struct pico_device *dev;
+ struct dhcp_client_timer init_timer;
+ struct dhcp_client_timer requesting_timer;
+ struct dhcp_client_timer renewing_timer;
+ struct dhcp_client_timer rebinding_timer;
+ struct dhcp_client_timer T1_timer;
+ struct dhcp_client_timer T2_timer;
+ struct dhcp_client_timer lease_timer;
+};
+
+static int pico_dhcp_client_init(struct pico_dhcp_client_cookie *dhcpc);
+static int reset(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+static int pico_dhcp_client_msg(struct pico_dhcp_client_cookie *dhcpc, uint8_t msg_type);
+static void pico_dhcp_client_wakeup(uint16_t ev, struct pico_socket *s);
+static void pico_dhcp_state_machine(uint8_t event, struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+
+static int dhcp_cookies_cmp(void *ka, void *kb)
+{
+ struct pico_dhcp_client_cookie *a = ka, *b = kb;
+ if (a->xid == b->xid)
+ return 0;
+ return (a->xid < b->xid) ? -1 : 1;
+}
+PICO_TREE_DECLARE(DHCPCookies, dhcp_cookies_cmp);
+
+static struct pico_dhcp_client_cookie *pico_dhcp_client_add_cookie(uint32_t xid, struct pico_device *dev, void (*cb)(void *dhcpc, int code), uint32_t *uid)
+{
+ struct pico_dhcp_client_cookie *dhcpc = NULL, *found = NULL, test = {0};
+
+ test.xid = xid;
+ found = pico_tree_findKey(&DHCPCookies, &test);
+ if (found) {
+ pico_err = PICO_ERR_EAGAIN;
+ return NULL;
+ }
+
+ dhcpc = pico_zalloc(sizeof(struct pico_dhcp_client_cookie));
+ if (!dhcpc) {
+ pico_err = PICO_ERR_ENOMEM;
+ return NULL;
+ }
+
+ dhcpc->state = DHCP_CLIENT_STATE_INIT;
+ dhcpc->status = DHCP_CLIENT_STATUS_INIT;
+ dhcpc->xid = xid;
+ dhcpc->uid = uid;
+ *(dhcpc->uid) = 0;
+ dhcpc->cb = cb;
+ dhcpc->dev = dev;
+
+ pico_tree_insert(&DHCPCookies, dhcpc);
+ return dhcpc;
+}
+
+static int pico_dhcp_client_del_cookie(uint32_t xid)
+{
+ struct pico_dhcp_client_cookie test = {0}, *found = NULL;
+
+ test.xid = xid;
+ found = pico_tree_findKey(&DHCPCookies, &test);
+ if (!found)
+ return -1;
+
+ pico_socket_close(found->s);
+ pico_ipv4_link_del(found->dev, found->address);
+ pico_tree_delete(&DHCPCookies, found);
+ pico_free(found);
+ return 0;
+}
+
+static struct pico_dhcp_client_cookie *pico_dhcp_client_find_cookie(uint32_t xid)
+{
+ struct pico_dhcp_client_cookie test = {0}, *found = NULL;
+
+ test.xid = xid;
+ found = pico_tree_findKey(&DHCPCookies, &test);
+ if (found)
+ return found;
+ else
+ return NULL;
+}
+
+static void pico_dhcp_client_init_timer(unsigned long __attribute__((unused)) now, void *arg)
+{
+ struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg;
+
+ if (dhcpc->init_timer.state == DHCP_CLIENT_TIMER_STOPPED)
+ return;
+
+ if (++dhcpc->retry >= DHCP_CLIENT_RETRIES) {
+ pico_err = PICO_ERR_EAGAIN;
+ dhcpc->cb(dhcpc, PICO_DHCP_ERROR);
+ pico_dhcp_client_del_cookie(dhcpc->xid);
+ pico_dhcp_client_mutex++;
+ return;
+ }
+
+ /* init_timer is restarted in retransmit function,
+ * otherwise an old init_timer would go on indefinitely */
+ dhcpc->event = PICO_DHCP_EVENT_RETRANSMIT;
+ pico_dhcp_state_machine(dhcpc->event, dhcpc, NULL);
+ return;
+}
+
+static void pico_dhcp_client_requesting_timer(unsigned long __attribute__((unused)) now, void *arg)
+{
+ struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg;
+
+ if (dhcpc->requesting_timer.state == DHCP_CLIENT_TIMER_STOPPED)
+ return;
+
+ if (++dhcpc->retry > DHCP_CLIENT_RETRIES) {
+ pico_dhcp_client_mutex++;
+ reset(dhcpc, NULL);
+ return;
+ }
+
+ /* requesting_timer is restarted in retransmit function,
+ * otherwise an old requesting_timer would go on indefinitely */
+ dhcpc->event = PICO_DHCP_EVENT_RETRANSMIT;
+ pico_dhcp_state_machine(dhcpc->event, dhcpc, NULL);
+ return;
+}
+
+static void pico_dhcp_client_renewing_timer(unsigned long __attribute__((unused)) now, void *arg)
+{
+ struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg;
+
+ if (dhcpc->renewing_timer.state == DHCP_CLIENT_TIMER_STOPPED)
+ return;
+
+ /* renewing_timer is restarted in retransmit function,
+ * otherwise an old renewing_timer would go on indefinitely */
+ dhcpc->retry++;
+ dhcpc->event = PICO_DHCP_EVENT_RETRANSMIT;
+ pico_dhcp_state_machine(dhcpc->event, dhcpc, NULL);
+ return;
+}
+
+static void pico_dhcp_client_rebinding_timer(unsigned long __attribute__((unused)) now, void *arg)
+{
+ struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg;
+
+ if (dhcpc->rebinding_timer.state == DHCP_CLIENT_TIMER_STOPPED)
+ return;
+
+ /* rebinding_timer is restarted in retransmit function,
+ * otherwise an old rebinding_timer would go on indefinitely */
+ dhcpc->retry++;
+ dhcpc->event = PICO_DHCP_EVENT_RETRANSMIT;
+ pico_dhcp_state_machine(dhcpc->event, dhcpc, NULL);
+ return;
+}
+
+static void pico_dhcp_client_T1_timer(unsigned long __attribute__((unused)) now, void *arg)
+{
+ struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg;
+
+ if (dhcpc->T1_timer.state == DHCP_CLIENT_TIMER_STOPPED)
+ return;
+
+ /* T1 state is set to stopped in renew function,
+ * otherwise an old T1 could stop a valid T1 */
+ dhcpc->event = PICO_DHCP_EVENT_T1;
+ pico_dhcp_state_machine(dhcpc->event, dhcpc, NULL);
+ return;
+}
+
+static void pico_dhcp_client_T2_timer(unsigned long __attribute__((unused)) now, void *arg)
+{
+ struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg;
+
+ if (dhcpc->T2_timer.state == DHCP_CLIENT_TIMER_STOPPED)
+ return;
+
+ /* T2 state is set to stopped in rebind function,
+ * otherwise an old T2 could stop a valid T2.
+ * Likewise for renewing_timer */
+ dhcpc->event = PICO_DHCP_EVENT_T2;
+ pico_dhcp_state_machine(dhcpc->event, dhcpc, NULL);
+ return;
+}
+
+static void pico_dhcp_client_lease_timer(unsigned long __attribute__((unused)) now, void *arg)
+{
+ struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg;
+
+ if (dhcpc->lease_timer.state == DHCP_CLIENT_TIMER_STOPPED)
+ return;
+
+ /* lease state is set to stopped in reset function,
+ * otherwise an old lease could stop a valid lease.
+ * Likewise for rebinding_timer */
+ dhcpc->event = PICO_DHCP_EVENT_LEASE;
+ pico_dhcp_state_machine(dhcpc->event, dhcpc, NULL);
+ return;
+}
+
+static void pico_dhcp_client_reinit(unsigned long __attribute__((unused)) now, void *arg)
+{
+ struct pico_dhcp_client_cookie *dhcpc = (struct pico_dhcp_client_cookie *)arg;
+
+ if (++dhcpc->retry > DHCP_CLIENT_RETRIES) {
+ pico_err = PICO_ERR_EAGAIN;
+ dhcpc->cb(dhcpc, PICO_DHCP_ERROR);
+ pico_dhcp_client_del_cookie(dhcpc->xid);
+ return;
+ }
+ pico_dhcp_client_init(dhcpc);
+ return;
+}
+
+static void pico_dhcp_client_stop_timers(struct pico_dhcp_client_cookie *dhcpc)
+{
+ dhcpc->retry = 0;
+ dhcpc->init_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->requesting_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->renewing_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->rebinding_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->T1_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->T2_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->lease_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+
+ return;
+}
+
+static void pico_dhcp_client_start_init_timer(struct pico_dhcp_client_cookie *dhcpc)
+{
+ uint32_t time = 0;
+
+ /* timer value is doubled with every retry (exponential backoff) */
+ dhcpc->init_timer.state = DHCP_CLIENT_TIMER_STARTED;
+ dhcpc->init_timer.time = DHCP_CLIENT_RETRANS;
+ time = dhcpc->init_timer.time << dhcpc->retry;
+ pico_timer_add(time * 1000, pico_dhcp_client_init_timer, dhcpc);
+
+ return;
+}
+
+static void pico_dhcp_client_start_requesting_timer(struct pico_dhcp_client_cookie *dhcpc)
+{
+ uint32_t time = 0;
+
+ /* timer value is doubled with every retry (exponential backoff) */
+ dhcpc->init_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->requesting_timer.state = DHCP_CLIENT_TIMER_STARTED;
+ dhcpc->requesting_timer.time = DHCP_CLIENT_RETRANS;
+ time = dhcpc->requesting_timer.time << dhcpc->retry;
+ pico_timer_add(time * 1000, pico_dhcp_client_requesting_timer, dhcpc);
+
+ return;
+}
+
+static void pico_dhcp_client_start_renewing_timer(struct pico_dhcp_client_cookie *dhcpc)
+{
+ uint32_t halftime = 0;
+
+ /* wait one-half of the remaining time until T2, down to a minimum of 60 seconds */
+ /* (dhcpc->retry + 1): initial -> divide by 2, 1st retry -> divide by 4, 2nd retry -> divide by 8, etc */
+ dhcpc->T1_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->renewing_timer.state = DHCP_CLIENT_TIMER_STARTED;
+ halftime = dhcpc->renewing_timer.time >> (dhcpc->retry + 1);
+ if (halftime < 60)
+ halftime = 60;
+ pico_timer_add(halftime * 1000, pico_dhcp_client_renewing_timer, dhcpc);
+
+ return;
+}
+
+static void pico_dhcp_client_start_rebinding_timer(struct pico_dhcp_client_cookie *dhcpc)
+{
+ uint32_t halftime = 0;
+
+ /* wait one-half of the remaining time until T2, down to a minimum of 60 seconds */
+ /* (dhcpc->retry + 1): initial -> divide by 2, 1st retry -> divide by 4, 2nd retry -> divide by 8, etc */
+ dhcpc->T2_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->renewing_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->rebinding_timer.state = DHCP_CLIENT_TIMER_STARTED;
+ halftime = dhcpc->rebinding_timer.time >> (dhcpc->retry + 1);
+ if (halftime < 60)
+ halftime = 60;
+ pico_timer_add(halftime * 1000, pico_dhcp_client_rebinding_timer, dhcpc);
+
+ return;
+}
+
+static void pico_dhcp_client_start_reacquisition_timers(struct pico_dhcp_client_cookie *dhcpc)
+{
+ dhcpc->requesting_timer.state = DHCP_CLIENT_TIMER_STOPPED;
+ dhcpc->T1_timer.state = DHCP_CLIENT_TIMER_STARTED;
+ dhcpc->T2_timer.state = DHCP_CLIENT_TIMER_STARTED;
+ dhcpc->lease_timer.state = DHCP_CLIENT_TIMER_STARTED;
+ pico_timer_add(dhcpc->T1_timer.time * 1000, pico_dhcp_client_T1_timer, dhcpc);
+ pico_timer_add(dhcpc->T2_timer.time * 1000, pico_dhcp_client_T2_timer, dhcpc);
+ pico_timer_add(dhcpc->lease_timer.time * 1000, pico_dhcp_client_lease_timer, dhcpc);
+
+ return;
+}
+
+static int pico_dhcp_client_init(struct pico_dhcp_client_cookie *dhcpc)
+{
+ uint16_t port = PICO_DHCP_CLIENT_PORT;
+ struct pico_ip4 inaddr_any = {0}, netmask = {0};
+
+ /* serialize client negotations if multiple devices */
+ /* NOTE: ONLY initialization is serialized! */
+ if (!pico_dhcp_client_mutex) {
+ pico_timer_add(DHCP_CLIENT_REINIT, pico_dhcp_client_reinit, dhcpc);
+ return 0;
+ }
+ pico_dhcp_client_mutex--;
+
+ switch (dhcpc->status)
+ {
+ case DHCP_CLIENT_STATUS_INIT:
+ dhcpc->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcp_client_wakeup);
+ if (!dhcpc->s) {
+ pico_dhcp_client_mutex++;
+ pico_timer_add(DHCP_CLIENT_REINIT, pico_dhcp_client_reinit, dhcpc);
+ break;
+ }
+ dhcpc->s->dev = dhcpc->dev;
+ dhcpc->status = DHCP_CLIENT_STATUS_SOCKET;
+ /* fallthrough */
+
+ case DHCP_CLIENT_STATUS_SOCKET:
+ if (pico_socket_bind(dhcpc->s, &inaddr_any, &port) < 0) {
+ pico_dhcp_client_mutex++;
+ pico_timer_add(DHCP_CLIENT_REINIT, pico_dhcp_client_reinit, dhcpc);
+ break;
+ }
+ dhcpc->status = DHCP_CLIENT_STATUS_BOUND;
+ /* fallthrough */
+
+ case DHCP_CLIENT_STATUS_BOUND:
+ /* adding a link with address 0.0.0.0 and netmask 0.0.0.0,
+ * automatically adds a route for a global broadcast */
+ if (pico_ipv4_link_add(dhcpc->dev, inaddr_any, netmask) < 0) {
+ pico_dhcp_client_mutex++;
+ pico_timer_add(DHCP_CLIENT_REINIT, pico_dhcp_client_reinit, dhcpc);
+ break;
+ }
+ dhcpc->status = DHCP_CLIENT_STATUS_LINKED;
+ /* fallthrough */
+
+ case DHCP_CLIENT_STATUS_LINKED:
+ if (pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_DISCOVER) < 0) {
+ pico_dhcp_client_mutex++;
+ pico_timer_add(DHCP_CLIENT_REINIT, pico_dhcp_client_reinit, dhcpc);
+ break;
+ }
+ dhcpc->status = DHCP_CLIENT_STATUS_TRANSMITTED;
+ /* fallthrough */
+
+ case DHCP_CLIENT_STATUS_TRANSMITTED:
+ dhcpc->retry = 0;
+ dhcpc->init_timestamp = PICO_TIME_MS();
+ pico_dhcp_client_start_init_timer(dhcpc);
+ break;
+
+ default:
+ return -1;
+ }
+ return 0;
+}
+
+int pico_dhcp_initiate_negotiation(struct pico_device *dev, void (*cb)(void *dhcpc, int code), uint32_t *uid)
+{
+ uint8_t retry = 32;
+ uint32_t xid = 0;
+ struct pico_dhcp_client_cookie *dhcpc = NULL;
+
+ if (!dev || !cb || !uid) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (!dev->eth) {
+ pico_err = PICO_ERR_EOPNOTSUPP;
+ return -1;
+ }
+
+ /* attempt to generate a correct xid, else fail */
+ do {
+ xid = pico_rand();
+ } while (!xid && --retry);
+
+ if (!xid) {
+ pico_err = PICO_ERR_EAGAIN;
+ return -1;
+ }
+
+ dhcpc = pico_dhcp_client_add_cookie(xid, dev, cb, uid);
+ if (!dhcpc)
+ return -1;
+
+ dhcpc_dbg("DHCP client: cookie with xid %u\n", dhcpc->xid);
+ return pico_dhcp_client_init(dhcpc);
+}
+
+static void pico_dhcp_client_recv_params(struct pico_dhcp_client_cookie *dhcpc, struct pico_dhcp_opt *opt)
+{
+ do {
+ switch (opt->code)
+ {
+ case PICO_DHCP_OPT_PAD:
+ break;
+
+ case PICO_DHCP_OPT_END:
+ break;
+
+ case PICO_DHCP_OPT_MSGTYPE:
+ dhcpc->event = opt->ext.msg_type.type;
+ dhcpc_dbg("DHCP client: message type %u\n", dhcpc->event);
+ break;
+
+ case PICO_DHCP_OPT_LEASETIME:
+ dhcpc->lease_timer.time = long_be(opt->ext.lease_time.time);
+ dhcpc_dbg("DHCP client: lease time %u\n", dhcpc->lease_timer.time);
+ break;
+
+ case PICO_DHCP_OPT_RENEWALTIME:
+ dhcpc->T1_timer.time = long_be(opt->ext.renewal_time.time);
+ dhcpc_dbg("DHCP client: renewal time %u\n", dhcpc->T1_timer.time);
+ break;
+
+ case PICO_DHCP_OPT_REBINDINGTIME:
+ dhcpc->T2_timer.time = long_be(opt->ext.rebinding_time.time);
+ dhcpc_dbg("DHCP client: rebinding time %u\n", dhcpc->T2_timer.time);
+ break;
+
+ case PICO_DHCP_OPT_ROUTER:
+ dhcpc->gateway = opt->ext.router.ip;
+ dhcpc_dbg("DHCP client: router %08X\n", dhcpc->gateway.addr);
+ break;
+
+ case PICO_DHCP_OPT_DNS:
+ dhcpc->nameserver = opt->ext.dns.ip;
+ dhcpc_dbg("DHCP client: dns %08X\n", dhcpc->nameserver.addr);
+ break;
+
+ case PICO_DHCP_OPT_NETMASK:
+ dhcpc->netmask = opt->ext.netmask.ip;
+ dhcpc_dbg("DHCP client: netmask %08X\n", dhcpc->netmask.addr);
+ break;
+
+ case PICO_DHCP_OPT_SERVERID:
+ dhcpc->server_id = opt->ext.server_id.ip;
+ dhcpc_dbg("DHCP client: server ID %08X\n", dhcpc->server_id.addr);
+ break;
+
+ case PICO_DHCP_OPT_OPTOVERLOAD:
+ dhcpc_dbg("DHCP client: WARNING option overload present (not processed)");
+ break;
+
+ default:
+ dhcpc_dbg("DHCP client: WARNING unsupported option %u\n", opt->code);
+ break;
+ }
+ } while (pico_dhcp_next_option(&opt));
+
+ /* default values for T1 and T2 when not provided */
+ if (!dhcpc->T1_timer.time)
+ dhcpc->T1_timer.time = dhcpc->lease_timer.time >> 1;
+ if (!dhcpc->T2_timer.time)
+ dhcpc->T2_timer.time = (dhcpc->lease_timer.time * 875) / 1000;
+
+ return;
+}
+
+static int recv_offer(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf)
+{
+ struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf;
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)hdr->options;
+
+ pico_dhcp_client_recv_params(dhcpc, opt);
+ if ((dhcpc->event != PICO_DHCP_MSG_OFFER) || !dhcpc->server_id.addr || !dhcpc->netmask.addr || !dhcpc->lease_timer.time)
+ return -1;
+ dhcpc->address.addr = hdr->yiaddr;
+
+ /* we skip state SELECTING, process first offer received */
+ dhcpc->state = DHCP_CLIENT_STATE_REQUESTING;
+ dhcpc->retry = 0;
+ pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST);
+ pico_dhcp_client_start_requesting_timer(dhcpc);
+ return 0;
+}
+
+static int recv_ack(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf)
+{
+ struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf;
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)hdr->options;
+ struct pico_ip4 address = {0}, netmask = {0}, inaddr_any = {0}, bcast = { .addr = 0xFFFFFFFF };
+
+ pico_dhcp_client_recv_params(dhcpc, opt);
+ if ((dhcpc->event != PICO_DHCP_MSG_ACK) || !dhcpc->server_id.addr || !dhcpc->netmask.addr || !dhcpc->lease_timer.time)
+ return -1;
+
+ /* close the socket used for address (re)acquisition */
+ pico_socket_close(dhcpc->s);
+ /* delete the link with address 0.0.0.0, add new link with acquired address */
+ if (dhcpc->status == DHCP_CLIENT_STATUS_TRANSMITTED) {
+ pico_ipv4_link_del(dhcpc->dev, address);
+ pico_ipv4_link_add(dhcpc->dev, dhcpc->address, dhcpc->netmask);
+ dhcpc->status = DHCP_CLIENT_STATUS_INITIALIZED;
+ }
+ /* delete the default route for our global broadcast messages, otherwise another interface can not rebind */
+ if (dhcpc->state == DHCP_CLIENT_STATE_REBINDING)
+ pico_ipv4_route_del(bcast, netmask, inaddr_any, 1, pico_ipv4_link_get(&dhcpc->address));
+
+ dbg("DHCP client: renewal time (T1) %u\n", dhcpc->T1_timer.time);
+ dbg("DHCP client: rebinding time (T2) %u\n", dhcpc->T2_timer.time);
+ dbg("DHCP client: lease time %u\n", dhcpc->lease_timer.time);
+
+ dhcpc->retry = 0;
+ dhcpc->renewing_timer.time = dhcpc->T2_timer.time - dhcpc->T1_timer.time;
+ dhcpc->rebinding_timer.time = dhcpc->lease_timer.time - dhcpc->T2_timer.time;
+ pico_dhcp_client_start_reacquisition_timers(dhcpc);
+
+ pico_dhcp_client_mutex++;
+ *(dhcpc->uid) = dhcpc->xid;
+ dhcpc->cb(dhcpc, PICO_DHCP_SUCCESS);
+ dhcpc->state = DHCP_CLIENT_STATE_BOUND;
+ return 0;
+}
+
+static int renew(struct pico_dhcp_client_cookie *dhcpc, uint8_t __attribute__((unused)) *buf)
+{
+ uint16_t port = PICO_DHCP_CLIENT_PORT;
+
+ dhcpc->state = DHCP_CLIENT_STATE_RENEWING;
+ dhcpc->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcp_client_wakeup);
+ if (!dhcpc->s) {
+ dhcpc_dbg("DHCP client ERROR: failure opening socket on renew, aborting DHCP! (%s)\n", strerror(pico_err));
+ dhcpc->cb(dhcpc, PICO_DHCP_ERROR);
+ return -1;
+ }
+ if (pico_socket_bind(dhcpc->s, &dhcpc->address, &port) != 0) {
+ dhcpc_dbg("DHCP client ERROR: failure binding socket on renew, aborting DHCP! (%s)\n", strerror(pico_err));
+ pico_socket_close(dhcpc->s);
+ dhcpc->cb(dhcpc, PICO_DHCP_ERROR);
+ return -1;
+ }
+
+ dhcpc->retry = 0;
+ pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST);
+ pico_dhcp_client_start_renewing_timer(dhcpc);
+
+ return 0;
+}
+
+static int rebind(struct pico_dhcp_client_cookie *dhcpc, uint8_t __attribute__((unused)) *buf)
+{
+ struct pico_ip4 bcast = { .addr = 0xFFFFFFFF }, netmask = {0}, inaddr_any = {0};
+
+ dhcpc->state = DHCP_CLIENT_STATE_REBINDING;
+ dhcpc->retry = 0;
+ /* we need a default route for our global broadcast messages, otherwise they get dropped. */
+ pico_ipv4_route_add(bcast, netmask, inaddr_any, 1, pico_ipv4_link_get(&dhcpc->address));
+ pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST);
+ pico_dhcp_client_start_rebinding_timer(dhcpc);
+
+ return 0;
+}
+
+static int reset(struct pico_dhcp_client_cookie *dhcpc, uint8_t __attribute__((unused)) *buf)
+{
+ struct pico_ip4 address = {0}, netmask = {0}, inaddr_any = {0}, bcast = { .addr = 0xFFFFFFFF };
+
+ if (dhcpc->state == DHCP_CLIENT_STATE_REQUESTING)
+ address.addr = PICO_IP4_ANY;
+ else
+ address.addr = dhcpc->address.addr;
+
+ /* delete the default route for our global broadcast messages, otherwise another interface can not rebind */
+ if (dhcpc->state == DHCP_CLIENT_STATE_REBINDING)
+ pico_ipv4_route_del(bcast, netmask, inaddr_any, 1, pico_ipv4_link_get(&dhcpc->address));
+
+ /* close the socket used for address (re)acquisition */
+ pico_socket_close(dhcpc->s);
+ /* delete the link with the currently in use address */
+ pico_ipv4_link_del(dhcpc->dev, address);
+
+ dhcpc->cb(dhcpc, PICO_DHCP_RESET);
+ dhcpc->state = DHCP_CLIENT_STATE_INIT;
+ dhcpc->status = DHCP_CLIENT_STATUS_INIT;
+ pico_dhcp_client_stop_timers(dhcpc);
+ pico_dhcp_client_init(dhcpc);
+ return 0;
+}
+
+static int retransmit(struct pico_dhcp_client_cookie *dhcpc, uint8_t __attribute__((unused)) *buf)
+{
+ switch (dhcpc->state)
+ {
+ case DHCP_CLIENT_STATE_INIT:
+ pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_DISCOVER);
+ pico_dhcp_client_start_init_timer(dhcpc);
+ break;
+
+ case DHCP_CLIENT_STATE_REQUESTING:
+ pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST);
+ pico_dhcp_client_start_requesting_timer(dhcpc);
+ break;
+
+ case DHCP_CLIENT_STATE_RENEWING:
+ pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_REQUEST);
+ pico_dhcp_client_start_renewing_timer(dhcpc);
+ break;
+
+ case DHCP_CLIENT_STATE_REBINDING:
+ pico_dhcp_client_msg(dhcpc, PICO_DHCP_MSG_DISCOVER);
+ pico_dhcp_client_start_rebinding_timer(dhcpc);
+ break;
+
+ default:
+ dhcpc_dbg("DHCP client WARNING: retransmit in incorrect state (%u)!\n", dhcpc->state);
+ return -1;
+ }
+ return 0;
+}
+
+struct dhcp_action_entry {
+ int (*offer)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+ int (*ack)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+ int (*nak)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+ int (*timer1)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+ int (*timer2)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+ int (*timer_lease)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+ int (*timer_retransmit)(struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf);
+};
+
+static struct dhcp_action_entry dhcp_fsm[] =
+{ /* event |offer |ack |nak |T1 |T2 |lease |retransmit */
+/* state init-reboot */ { NULL, NULL, NULL, NULL, NULL, NULL, NULL },
+/* state rebooting */ { NULL, NULL, NULL, NULL, NULL, NULL, NULL },
+/* state init */ { recv_offer, NULL, NULL, NULL, NULL, NULL, retransmit },
+/* state selecting */ { NULL, NULL, NULL, NULL, NULL, NULL, NULL },
+/* state requesting */ { NULL, recv_ack, reset, NULL, NULL, NULL, retransmit },
+/* state bound */ { NULL, NULL, NULL, renew, NULL, NULL, NULL },
+/* state renewing */ { NULL, recv_ack, reset, NULL, rebind, NULL, retransmit },
+/* state rebinding */ { NULL, recv_ack, reset, NULL, NULL, reset, retransmit },
+};
+
+/* TIMERS REMARK:
+ * In state bound we have T1, T2 and the lease timer running. If T1 goes off, we attempt to renew.
+ * If the renew succeeds a new T1, T2 and lease timer is started. The former T2 and lease timer is
+ * still running though. This poses no concerns as the T2 and lease event in state bound have a NULL
+ * pointer in the fsm. If the former T2 or lease timer goes off, nothing happens. Same situation
+ * applies for T2 and a succesfull rebind. */
+
+static void pico_dhcp_state_machine(uint8_t event, struct pico_dhcp_client_cookie *dhcpc, uint8_t *buf)
+{
+ switch (event)
+ {
+ case PICO_DHCP_MSG_OFFER:
+ dhcpc_dbg("DHCP client: received OFFER\n");
+ if (dhcp_fsm[dhcpc->state].offer)
+ dhcp_fsm[dhcpc->state].offer(dhcpc, buf);
+ break;
+
+ case PICO_DHCP_MSG_ACK:
+ dhcpc_dbg("DHCP client: received ACK\n");
+ if (dhcp_fsm[dhcpc->state].ack)
+ dhcp_fsm[dhcpc->state].ack(dhcpc, buf);
+ break;
+
+ case PICO_DHCP_MSG_NAK:
+ dhcpc_dbg("DHCP client: received NAK\n");
+ if (dhcp_fsm[dhcpc->state].nak)
+ dhcp_fsm[dhcpc->state].nak(dhcpc, buf);
+ break;
+
+ case PICO_DHCP_EVENT_T1:
+ dhcpc_dbg("DHCP client: received T1 timeout\n");
+ if (dhcp_fsm[dhcpc->state].timer1)
+ dhcp_fsm[dhcpc->state].timer1(dhcpc, NULL);
+ break;
+
+ case PICO_DHCP_EVENT_T2:
+ dhcpc_dbg("DHCP client: received T2 timeout\n");
+ if (dhcp_fsm[dhcpc->state].timer2)
+ dhcp_fsm[dhcpc->state].timer2(dhcpc, NULL);
+ break;
+
+ case PICO_DHCP_EVENT_LEASE:
+ dhcpc_dbg("DHCP client: received LEASE timeout\n");
+ if (dhcp_fsm[dhcpc->state].timer_lease)
+ dhcp_fsm[dhcpc->state].timer_lease(dhcpc, NULL);
+ break;
+
+ case PICO_DHCP_EVENT_RETRANSMIT:
+ dhcpc_dbg("DHCP client: received RETRANSMIT timeout\n");
+ if (dhcp_fsm[dhcpc->state].timer_retransmit)
+ dhcp_fsm[dhcpc->state].timer_retransmit(dhcpc, NULL);
+ break;
+
+ default:
+ dhcpc_dbg("DHCP client WARNING: unrecognized event (%u)!\n", dhcpc->event);
+ return;
+ }
+ return;
+}
+
+static int pico_dhcp_client_opt_parse(void *ptr, int len)
+{
+ int optlen = len - sizeof(struct pico_dhcp_hdr);
+ struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)ptr;
+ struct pico_dhcp_opt *opt = NULL;
+
+ if (hdr->dhcp_magic != PICO_DHCPD_MAGIC_COOKIE)
+ return -1;
+ if (!pico_dhcp_are_options_valid(hdr->options, optlen))
+ return -1;
+
+ opt = (struct pico_dhcp_opt *)hdr->options;
+ do {
+ if (opt->code == PICO_DHCP_OPT_MSGTYPE)
+ return opt->ext.msg_type.type;
+ } while (pico_dhcp_next_option(&opt));
+
+ return -1;
+}
+
+static int pico_dhcp_client_msg(struct pico_dhcp_client_cookie *dhcpc, uint8_t msg_type)
+{
+ int r = 0, optlen = 0, offset = 0;
+ struct pico_ip4 destination = { .addr = 0xFFFFFFFF};
+ struct pico_dhcp_hdr *hdr = NULL;
+
+ switch (msg_type)
+ {
+ case PICO_DHCP_MSG_DISCOVER:
+ dhcpc_dbg("DHCP client: sent DHCPDISCOVER\n");
+ optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_MAXMSGSIZE + PICO_DHCP_OPTLEN_PARAMLIST + PICO_DHCP_OPTLEN_END;
+ hdr = pico_zalloc(sizeof(struct pico_dhcp_hdr) + optlen);
+ /* specific options */
+ offset += pico_dhcp_opt_maxmsgsize(&hdr->options[offset], DHCP_CLIENT_MAXMSGZISE);
+ break;
+
+ case PICO_DHCP_MSG_REQUEST:
+ dhcpc_dbg("DHCP client: sent DHCPREQUEST\n");
+ optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_MAXMSGSIZE + PICO_DHCP_OPTLEN_PARAMLIST + PICO_DHCP_OPTLEN_REQIP + PICO_DHCP_OPTLEN_SERVERID
+ + PICO_DHCP_OPTLEN_END;
+ hdr = pico_zalloc(sizeof(struct pico_dhcp_hdr) + optlen);
+ /* specific options */
+ offset += pico_dhcp_opt_maxmsgsize(&hdr->options[offset], DHCP_CLIENT_MAXMSGZISE);
+ if (dhcpc->state == DHCP_CLIENT_STATE_REQUESTING) {
+ offset += pico_dhcp_opt_reqip(&hdr->options[offset], &dhcpc->address);
+ offset += pico_dhcp_opt_serverid(&hdr->options[offset], &dhcpc->server_id);
+ }
+ break;
+
+ default:
+ return -1;
+ }
+
+ /* common options */
+ offset += pico_dhcp_opt_msgtype(&hdr->options[offset], msg_type);
+ offset += pico_dhcp_opt_paramlist(&hdr->options[offset]);
+ offset += pico_dhcp_opt_end(&hdr->options[offset]);
+
+ switch (dhcpc->state)
+ {
+ case DHCP_CLIENT_STATE_BOUND:
+ destination.addr = dhcpc->server_id.addr;
+ hdr->ciaddr = dhcpc->address.addr;
+ break;
+
+ case DHCP_CLIENT_STATE_RENEWING:
+ destination.addr = dhcpc->server_id.addr;
+ hdr->ciaddr = dhcpc->address.addr;
+ break;
+
+ case DHCP_CLIENT_STATE_REBINDING:
+ hdr->ciaddr = dhcpc->address.addr;
+ break;
+
+ default:
+ /* do nothing */
+ break;
+ }
+
+ /* header information */
+ hdr->op = PICO_DHCP_OP_REQUEST;
+ hdr->htype = PICO_DHCP_HTYPE_ETH;
+ hdr->hlen = PICO_SIZE_ETH;
+ hdr->xid = dhcpc->xid;
+ hdr->flags = short_be(PICO_DHCP_FLAG_BROADCAST);
+ hdr->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
+ /* copy client hardware address */
+ memcpy(hdr->hwaddr, &dhcpc->dev->eth->mac, PICO_SIZE_ETH);
+
+ r = pico_socket_sendto(dhcpc->s, hdr, sizeof(struct pico_dhcp_hdr) + optlen, &destination, PICO_DHCPD_PORT);
+ pico_free(hdr);
+ if (r < 0)
+ return -1;
+
+ return 0;
+}
+
+static void pico_dhcp_client_wakeup(uint16_t ev, struct pico_socket *s)
+{
+ uint8_t buf[DHCP_CLIENT_MAXMSGZISE] = {0};
+ int r = 0;
+ struct pico_dhcp_hdr *hdr = NULL;
+ struct pico_dhcp_client_cookie *dhcpc = NULL;
+
+ if (ev != PICO_SOCK_EV_RD)
+ return;
+ r = pico_socket_recvfrom(s, buf, DHCP_CLIENT_MAXMSGZISE, NULL, NULL);
+ if (r < 0)
+ return;
+
+ /* If the 'xid' of an arriving message does not match the 'xid'
+ * of the most recent transmitted message, the message must be
+ * silently discarded. */
+ hdr = (struct pico_dhcp_hdr *)buf;
+ dhcpc = pico_dhcp_client_find_cookie(hdr->xid);
+ if (!dhcpc)
+ return;
+ dhcpc->event = pico_dhcp_client_opt_parse(buf, r);
+ pico_dhcp_state_machine(dhcpc->event, dhcpc, buf);
+}
+
+void *pico_dhcp_get_identifier(uint32_t xid)
+{
+ return (void *)pico_dhcp_client_find_cookie(xid);
+}
+
+struct pico_ip4 pico_dhcp_get_address(void* dhcpc)
+{
+ return ((struct pico_dhcp_client_cookie*)dhcpc)->address;
+}
+
+struct pico_ip4 pico_dhcp_get_gateway(void* dhcpc)
+{
+ return ((struct pico_dhcp_client_cookie*)dhcpc)->gateway;
+}
+
+struct pico_ip4 pico_dhcp_get_nameserver(void* dhcpc)
+{
+ return ((struct pico_dhcp_client_cookie*)dhcpc)->nameserver;
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_dhcp_client.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,26 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +. + +*********************************************************************/ +#ifndef _INCLUDE_PICO_DHCP_CLIENT +#define _INCLUDE_PICO_DHCP_CLIENT +#include "pico_dhcp_common.h" +#include "pico_addressing.h" +#include "pico_protocol.h" + +int pico_dhcp_initiate_negotiation(struct pico_device *device, void (*callback)(void* cli, int code), uint32_t *xid); +void pico_dhcp_process_incoming_message(uint8_t *data, int len); +void *pico_dhcp_get_identifier(uint32_t xid); +struct pico_ip4 pico_dhcp_get_address(void *cli); +struct pico_ip4 pico_dhcp_get_gateway(void *cli); +struct pico_ip4 pico_dhcp_get_nameserver(void* cli); + +/* possible codes for the callback */ +#define PICO_DHCP_SUCCESS 0 +#define PICO_DHCP_ERROR 1 +#define PICO_DHCP_RESET 2 + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_common.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,185 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Frederik Van Slycken
+*********************************************************************/
+
+#include "pico_config.h"
+#include "pico_stack.h"
+#include "pico_dhcp_common.h"
+
+#if defined (PICO_SUPPORT_DHCPC) || defined (PICO_SUPPORT_DHCPD)
+/* pico_dhcp_are_options_valid needs to be called first to prevent illegal memory access */
+/* The argument pointer is moved forward to the next option */
+struct pico_dhcp_opt *pico_dhcp_next_option(struct pico_dhcp_opt **ptr)
+{
+ uint8_t **p = (uint8_t **)ptr;
+ struct pico_dhcp_opt *opt = *ptr;
+
+ if (opt->code == PICO_DHCP_OPT_END)
+ return NULL;
+ if (opt->code == PICO_DHCP_OPT_PAD) {
+ *p += 1;
+ return *ptr;
+ }
+
+ *p += (opt->len + 2); /* (len + 2) to account for code and len octet */
+ return *ptr;
+}
+
+int pico_dhcp_are_options_valid(void *ptr, int len)
+{
+ uint8_t optlen = 0, *p = ptr;
+
+ while (len > 0) {
+ switch (*p)
+ {
+ case PICO_DHCP_OPT_END:
+ return 1;
+
+ case PICO_DHCP_OPT_PAD:
+ p++;
+ len--;
+ break;
+
+ default:
+ p++; /* move pointer from code octet to len octet */
+ if ((--len <= 0) || (len - (*p + 1) < 0)) /* (*p + 1) to account for len octet */
+ return 0;
+ optlen = *p;
+ p += optlen + 1;
+ len -= optlen;
+ break;
+ }
+ }
+ return 0;
+}
+
+int pico_dhcp_opt_netmask(void *ptr, struct pico_ip4 *ip)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: netmask */
+ opt->code = PICO_DHCP_OPT_NETMASK;
+ opt->len = PICO_DHCP_OPTLEN_NETMASK - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.netmask.ip = *ip;
+ return PICO_DHCP_OPTLEN_NETMASK;
+}
+
+int pico_dhcp_opt_router(void *ptr, struct pico_ip4 *ip)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: router */
+ opt->code = PICO_DHCP_OPT_ROUTER;
+ opt->len = PICO_DHCP_OPTLEN_ROUTER - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.router.ip = *ip;
+ return PICO_DHCP_OPTLEN_ROUTER;
+}
+
+int pico_dhcp_opt_dns(void *ptr, struct pico_ip4 *ip)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: dns */
+ opt->code = PICO_DHCP_OPT_DNS;
+ opt->len = PICO_DHCP_OPTLEN_DNS - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.dns.ip = *ip;
+ return PICO_DHCP_OPTLEN_DNS;
+}
+
+int pico_dhcp_opt_broadcast(void *ptr, struct pico_ip4 *ip)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: broadcast */
+ opt->code = PICO_DHCP_OPT_BROADCAST;
+ opt->len = PICO_DHCP_OPTLEN_BROADCAST - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.broadcast.ip = *ip;
+ return PICO_DHCP_OPTLEN_BROADCAST;
+}
+
+int pico_dhcp_opt_reqip(void *ptr, struct pico_ip4 *ip)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: request IP address */
+ opt->code = PICO_DHCP_OPT_REQIP;
+ opt->len = PICO_DHCP_OPTLEN_REQIP - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.req_ip.ip = *ip;
+ return PICO_DHCP_OPTLEN_REQIP;
+}
+
+int pico_dhcp_opt_leasetime(void *ptr, uint32_t time)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: lease time */
+ opt->code = PICO_DHCP_OPT_LEASETIME;
+ opt->len = PICO_DHCP_OPTLEN_LEASETIME - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.lease_time.time = time;
+ return PICO_DHCP_OPTLEN_LEASETIME;
+}
+
+int pico_dhcp_opt_msgtype(void *ptr, uint8_t type)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: message type */
+ opt->code = PICO_DHCP_OPT_MSGTYPE;
+ opt->len = PICO_DHCP_OPTLEN_MSGTYPE - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.msg_type.type = type;
+ return PICO_DHCP_OPTLEN_MSGTYPE;
+}
+
+int pico_dhcp_opt_serverid(void *ptr, struct pico_ip4 *ip)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: server identifier */
+ opt->code = PICO_DHCP_OPT_SERVERID;
+ opt->len = PICO_DHCP_OPTLEN_SERVERID - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.server_id.ip = *ip;
+ return PICO_DHCP_OPTLEN_SERVERID;
+}
+
+int pico_dhcp_opt_paramlist(void *ptr)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: parameter list */
+ opt->code = PICO_DHCP_OPT_PARAMLIST;
+ opt->len = PICO_DHCP_OPTLEN_PARAMLIST - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.param_list.code[0] = PICO_DHCP_OPT_NETMASK;
+ opt->ext.param_list.code[1] = PICO_DHCP_OPT_TIME;
+ opt->ext.param_list.code[2] = PICO_DHCP_OPT_ROUTER;
+ opt->ext.param_list.code[3] = PICO_DHCP_OPT_HOSTNAME;
+ opt->ext.param_list.code[4] = PICO_DHCP_OPT_RENEWALTIME;
+ opt->ext.param_list.code[5] = PICO_DHCP_OPT_REBINDINGTIME;
+ return PICO_DHCP_OPTLEN_PARAMLIST;
+}
+
+int pico_dhcp_opt_maxmsgsize(void *ptr, uint16_t size)
+{
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)ptr;
+
+ /* option: maximum message size */
+ opt->code = PICO_DHCP_OPT_MAXMSGSIZE;
+ opt->len = PICO_DHCP_OPTLEN_MAXMSGSIZE - PICO_DHCP_OPTLEN_HDR;
+ opt->ext.max_msg_size.size = short_be(size);
+ return PICO_DHCP_OPTLEN_MAXMSGSIZE;
+}
+
+int pico_dhcp_opt_end(void *ptr)
+{
+ uint8_t *opt = (uint8_t *)ptr;
+
+ /* option: end of options */
+ *opt = PICO_DHCP_OPT_END;
+ return PICO_DHCP_OPTLEN_END;
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_common.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,183 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_DHCP_COMMON
+#define _INCLUDE_PICO_DHCP_COMMON
+#include "pico_addressing.h"
+
+#define PICO_DHCPD_PORT (short_be(67))
+#define PICO_DHCP_CLIENT_PORT (short_be(68))
+#define PICO_DHCPD_MAGIC_COOKIE (long_be(0x63825363))
+#define PICO_DHCP_HTYPE_ETH 1
+
+/* flags */
+#define PICO_DHCP_FLAG_BROADCAST 0x8000
+
+/* options */
+#define PICO_DHCP_OPT_PAD 0x00
+#define PICO_DHCP_OPT_NETMASK 0x01
+#define PICO_DHCP_OPT_TIME 0x02
+#define PICO_DHCP_OPT_ROUTER 0x03
+#define PICO_DHCP_OPT_DNS 0x06
+#define PICO_DHCP_OPT_HOSTNAME 0x0c
+#define PICO_DHCP_OPT_DOMAINNAME 0x0f
+#define PICO_DHCP_OPT_MTU 0x1a
+#define PICO_DHCP_OPT_BROADCAST 0x1c
+#define PICO_DHCP_OPT_NETBIOSNS 0x2c
+#define PICO_DHCP_OPT_NETBIOSSCOPE 0x2f
+#define PICO_DHCP_OPT_REQIP 0x32
+#define PICO_DHCP_OPT_LEASETIME 0x33
+#define PICO_DHCP_OPT_OPTOVERLOAD 0x34
+#define PICO_DHCP_OPT_MSGTYPE 0x35
+#define PICO_DHCP_OPT_SERVERID 0x36
+#define PICO_DHCP_OPT_PARAMLIST 0x37
+#define PICO_DHCP_OPT_MESSAGE 0x38
+#define PICO_DHCP_OPT_MAXMSGSIZE 0x39
+#define PICO_DHCP_OPT_RENEWALTIME 0x3a
+#define PICO_DHCP_OPT_REBINDINGTIME 0x3b
+#define PICO_DHCP_OPT_VENDORID 0x3c
+#define PICO_DHCP_OPT_CLIENTID 0x3d
+#define PICO_DHCP_OPT_DOMAINSEARCH 0x77
+#define PICO_DHCP_OPT_STATICROUTE 0x79
+#define PICO_DHCP_OPT_END 0xFF
+
+/* options len */
+#define PICO_DHCP_OPTLEN_HDR 2 /* account for code and len field */
+#define PICO_DHCP_OPTLEN_NETMASK 6
+#define PICO_DHCP_OPTLEN_ROUTER 6
+#define PICO_DHCP_OPTLEN_DNS 6
+#define PICO_DHCP_OPTLEN_BROADCAST 6
+#define PICO_DHCP_OPTLEN_REQIP 6
+#define PICO_DHCP_OPTLEN_LEASETIME 6
+#define PICO_DHCP_OPTLEN_OPTOVERLOAD 3
+#define PICO_DHCP_OPTLEN_MSGTYPE 3
+#define PICO_DHCP_OPTLEN_SERVERID 6
+#define PICO_DHCP_OPTLEN_PARAMLIST 8 /* PicoTCP specific */
+#define PICO_DHCP_OPTLEN_MAXMSGSIZE 4
+#define PICO_DHCP_OPTLEN_RENEWALTIME 6
+#define PICO_DHCP_OPTLEN_REBINDINGTIME 6
+#define PICO_DHCP_OPTLEN_END 1
+
+/* op codes */
+#define PICO_DHCP_OP_REQUEST 1
+#define PICO_DHCP_OP_REPLY 2
+
+/* rfc message types */
+#define PICO_DHCP_MSG_DISCOVER 1
+#define PICO_DHCP_MSG_OFFER 2
+#define PICO_DHCP_MSG_REQUEST 3
+#define PICO_DHCP_MSG_DECLINE 4
+#define PICO_DHCP_MSG_ACK 5
+#define PICO_DHCP_MSG_NAK 6
+#define PICO_DHCP_MSG_RELEASE 7
+#define PICO_DHCP_MSG_INFORM 8
+
+/* custom message types */
+#define PICO_DHCP_EVENT_T1 9
+#define PICO_DHCP_EVENT_T2 10
+#define PICO_DHCP_EVENT_LEASE 11
+#define PICO_DHCP_EVENT_RETRANSMIT 12
+
+struct __attribute__((packed)) pico_dhcp_hdr
+{
+ uint8_t op;
+ uint8_t htype;
+ uint8_t hlen;
+ uint8_t hops; //zero
+ uint32_t xid; //store this in the request
+ uint16_t secs; // ignore
+ uint16_t flags;
+ uint32_t ciaddr; // client address - if asking for renewal
+ uint32_t yiaddr; // your address (client)
+ uint32_t siaddr; // dhcp offered address
+ uint32_t giaddr; // relay agent, bootp.
+ uint8_t hwaddr[6];
+ uint8_t hwaddr_padding[10];
+ char hostname[64];
+ char bootp_filename[128];
+ uint32_t dhcp_magic;
+ uint8_t options[0];
+};
+
+struct __attribute__((packed)) pico_dhcp_opt
+{
+ uint8_t code;
+ uint8_t len;
+ union {
+ struct {
+ struct pico_ip4 ip;
+ } netmask;
+ struct {
+ struct pico_ip4 ip;
+ } router;
+ struct {
+ struct pico_ip4 ip;
+ } dns;
+ struct {
+ struct pico_ip4 ip;
+ } broadcast;
+ struct {
+ struct pico_ip4 ip;
+ } req_ip;
+ struct {
+ uint32_t time;
+ } lease_time;
+ struct {
+ uint8_t value;
+ } opt_overload;
+ struct {
+ char name[0];
+ } tftp_server;
+ struct {
+ char name[0];
+ } bootfile;
+ struct {
+ uint8_t type;
+ } msg_type;
+ struct {
+ struct pico_ip4 ip;
+ } server_id;
+ struct {
+ uint8_t code[0];
+ } param_list;
+ struct {
+ char error[0];
+ } message;
+ struct {
+ uint16_t size;
+ } max_msg_size;
+ struct {
+ uint32_t time;
+ } renewal_time;
+ struct {
+ uint32_t time;
+ } rebinding_time;
+ struct {
+ uint8_t id[0];
+ } vendor_id;
+ struct {
+ uint8_t id[0];
+ } client_id;
+ } ext;
+};
+
+uint8_t dhcp_get_next_option(uint8_t *begin, uint8_t *data, int *len, uint8_t **nextopt);
+struct pico_dhcp_opt *pico_dhcp_next_option(struct pico_dhcp_opt **ptr);
+int pico_dhcp_are_options_valid(void *ptr, int len);
+
+int pico_dhcp_opt_netmask(void *ptr, struct pico_ip4 *ip);
+int pico_dhcp_opt_router(void *ptr, struct pico_ip4 *ip);
+int pico_dhcp_opt_dns(void *ptr, struct pico_ip4 *ip);
+int pico_dhcp_opt_broadcast(void *ptr, struct pico_ip4 *ip);
+int pico_dhcp_opt_reqip(void *ptr, struct pico_ip4 *ip);
+int pico_dhcp_opt_leasetime(void *ptr, uint32_t time);
+int pico_dhcp_opt_msgtype(void *ptr, uint8_t type);
+int pico_dhcp_opt_serverid(void *ptr, struct pico_ip4 *ip);
+int pico_dhcp_opt_paramlist(void *ptr);
+int pico_dhcp_opt_maxmsgsize(void *ptr, uint16_t size);
+int pico_dhcp_opt_end(void *ptr);
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_server.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,317 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+
+Authors: Frederik Van Slycken, Kristof Roelants
+*********************************************************************/
+
+#ifdef PICO_SUPPORT_DHCPD
+#include "pico_dhcp_server.h"
+#include "pico_config.h"
+#include "pico_addressing.h"
+#include "pico_socket.h"
+#include "pico_udp.h"
+#include "pico_stack.h"
+#include "pico_arp.h"
+
+#define dhcps_dbg(...) do{}while(0)
+//#define dhcps_dbg dbg
+
+/* default configurations */
+#define DHCP_SERVER_OPENDNS long_be(0xd043dede) /* OpenDNS DNS server 208.67.222.222 */
+#define DHCP_SERVER_POOL_START long_be(0x00000064)
+#define DHCP_SERVER_POOL_END long_be(0x000000fe)
+#define DHCP_SERVER_LEASE_TIME long_be(0x00000078)
+
+/* maximum size of a DHCP message */
+#define DHCP_SERVER_MAXMSGSIZE (PICO_IP_MTU - sizeof(struct pico_ipv4_hdr) - sizeof(struct pico_udp_hdr))
+
+#define ip_inrange(x) ((long_be(x) >= long_be(dhcpn->dhcps->pool_start)) && (long_be(x) <= long_be(dhcpn->dhcps->pool_end)))
+
+enum dhcp_server_state {
+ PICO_DHCP_STATE_DISCOVER = 0,
+ PICO_DHCP_STATE_OFFER,
+ PICO_DHCP_STATE_REQUEST,
+ PICO_DHCP_STATE_BOUND,
+ PICO_DHCP_STATE_RENEWING
+};
+
+struct pico_dhcp_server_negotiation {
+ uint32_t xid;
+ enum dhcp_server_state state;
+ struct pico_dhcp_server_setting *dhcps;
+ struct pico_ip4 ciaddr;
+ struct pico_eth hwaddr;
+};
+
+static void pico_dhcpd_wakeup(uint16_t ev, struct pico_socket *s);
+
+static int dhcp_settings_cmp(void *ka, void *kb)
+{
+ struct pico_dhcp_server_setting *a = ka, *b = kb;
+ if (a->dev == b->dev)
+ return 0;
+ return (a->dev < b->dev) ? -1 : 1;
+}
+PICO_TREE_DECLARE(DHCPSettings, dhcp_settings_cmp);
+
+static int dhcp_negotiations_cmp(void *ka, void *kb)
+{
+ struct pico_dhcp_server_negotiation *a = ka, *b = kb;
+ if (a->xid == b->xid)
+ return 0;
+ return (a->xid < b->xid) ? -1 : 1;
+}
+PICO_TREE_DECLARE(DHCPNegotiations, dhcp_negotiations_cmp);
+
+static struct pico_dhcp_server_setting *pico_dhcp_server_add_setting(struct pico_dhcp_server_setting *setting)
+{
+ uint16_t port = PICO_DHCPD_PORT;
+ struct pico_dhcp_server_setting *dhcps = NULL, *found = NULL, test = {0};
+ struct pico_ipv4_link *link = NULL;
+
+ link = pico_ipv4_link_get(&setting->server_ip);
+ if (!link) {
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+ }
+ test.dev = setting->dev;
+ found = pico_tree_findKey(&DHCPSettings, &test);
+ if (found) {
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+ }
+ dhcps = pico_zalloc(sizeof(struct pico_dhcp_server_setting));
+ if (!dhcps) {
+ pico_err = PICO_ERR_ENOMEM;
+ return NULL;
+ }
+
+ dhcps->lease_time = setting->lease_time;
+ dhcps->pool_start = setting->pool_start;
+ dhcps->pool_next = setting->pool_next;
+ dhcps->pool_end = setting->pool_end;
+ dhcps->dev = link->dev;
+ dhcps->server_ip = link->address;
+ dhcps->netmask = link->netmask;
+
+ /* default values if not provided */
+ if (!dhcps->pool_start)
+ dhcps->pool_start = (dhcps->server_ip.addr & dhcps->netmask.addr) | DHCP_SERVER_POOL_START;
+ if (!dhcps->pool_end)
+ dhcps->pool_end = (dhcps->server_ip.addr & dhcps->netmask.addr) | DHCP_SERVER_POOL_END;
+ if (!dhcps->lease_time)
+ dhcps->lease_time = DHCP_SERVER_LEASE_TIME;
+ dhcps->pool_next = dhcps->pool_start;
+
+ dhcps->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcpd_wakeup);
+ if (!dhcps->s) {
+ dhcps_dbg("DHCP server ERROR: failure opening socket (%s)\n", strerror(pico_err));
+ pico_free(dhcps);
+ return NULL;
+ }
+ if (pico_socket_bind(dhcps->s, &dhcps->server_ip, &port) < 0) {
+ dhcps_dbg("DHCP server ERROR: failure binding socket (%s)\n", strerror(pico_err));
+ pico_free(dhcps);
+ return NULL;
+ }
+
+ pico_tree_insert(&DHCPSettings, dhcps);
+ return dhcps;
+}
+
+static struct pico_dhcp_server_negotiation *pico_dhcp_server_find_negotiation(uint32_t xid)
+{
+ struct pico_dhcp_server_negotiation test = {0}, *found = NULL;
+
+ test.xid = xid;
+ found = pico_tree_findKey(&DHCPNegotiations, &test);
+ if (found)
+ return found;
+ else
+ return NULL;
+}
+
+static struct pico_dhcp_server_negotiation *pico_dhcp_server_add_negotiation(struct pico_device *dev, struct pico_dhcp_hdr *hdr)
+{
+ struct pico_dhcp_server_negotiation *dhcpn = NULL;
+ struct pico_dhcp_server_setting test = {0};
+ struct pico_ip4 *ciaddr = NULL;
+
+ if (pico_dhcp_server_find_negotiation(hdr->xid))
+ return NULL;
+
+ dhcpn = pico_zalloc(sizeof(struct pico_dhcp_server_negotiation));
+ if (!dhcpn) {
+ pico_err = PICO_ERR_ENOMEM;
+ return NULL;
+ }
+
+ dhcpn->xid = hdr->xid;
+ dhcpn->state = PICO_DHCP_STATE_DISCOVER;
+ memcpy(dhcpn->hwaddr.addr, hdr->hwaddr, PICO_SIZE_ETH);
+
+ test.dev = dev;
+ dhcpn->dhcps = pico_tree_findKey(&DHCPSettings, &test);
+ if (!dhcpn->dhcps) {
+ dhcps_dbg("DHCP server WARNING: received DHCP message on unconfigured link %s\n", dev->name);
+ pico_free(dhcpn);
+ return NULL;
+ }
+
+ ciaddr = pico_arp_reverse_lookup(&dhcpn->hwaddr);
+ if (!ciaddr) {
+ dhcpn->ciaddr.addr = dhcpn->dhcps->pool_next;
+ dhcpn->dhcps->pool_next = long_be(long_be(dhcpn->dhcps->pool_next) + 1);
+ pico_arp_create_entry(dhcpn->hwaddr.addr, dhcpn->ciaddr, dhcpn->dhcps->dev);
+ } else {
+ dhcpn->ciaddr = *ciaddr;
+ }
+
+ pico_tree_insert(&DHCPNegotiations, dhcpn);
+ return dhcpn;
+}
+
+static void dhcpd_make_reply(struct pico_dhcp_server_negotiation *dhcpn, uint8_t msg_type)
+{
+ int r = 0, optlen = 0, offset = 0;
+ struct pico_ip4 broadcast = {0}, dns = {0}, destination = { .addr = 0xFFFFFFFF};
+ struct pico_dhcp_hdr *hdr = NULL;
+
+ dns.addr = DHCP_SERVER_OPENDNS;
+ broadcast.addr = dhcpn->dhcps->server_ip.addr | ~(dhcpn->dhcps->netmask.addr);
+
+ optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_SERVERID + PICO_DHCP_OPTLEN_LEASETIME + PICO_DHCP_OPTLEN_NETMASK + PICO_DHCP_OPTLEN_ROUTER
+ + PICO_DHCP_OPTLEN_BROADCAST + PICO_DHCP_OPTLEN_DNS + PICO_DHCP_OPTLEN_END;
+ hdr = pico_zalloc(sizeof(struct pico_dhcp_hdr) + optlen);
+
+ hdr->op = PICO_DHCP_OP_REPLY;
+ hdr->htype = PICO_DHCP_HTYPE_ETH;
+ hdr->hlen = PICO_SIZE_ETH;
+ hdr->xid = dhcpn->xid;
+ hdr->yiaddr = dhcpn->ciaddr.addr;
+ hdr->siaddr = dhcpn->dhcps->server_ip.addr;
+ hdr->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
+ memcpy(hdr->hwaddr, dhcpn->hwaddr.addr, PICO_SIZE_ETH);
+
+ /* options */
+ offset += pico_dhcp_opt_msgtype(&hdr->options[offset], msg_type);
+ offset += pico_dhcp_opt_serverid(&hdr->options[offset], &dhcpn->dhcps->server_ip);
+ offset += pico_dhcp_opt_leasetime(&hdr->options[offset], dhcpn->dhcps->lease_time);
+ offset += pico_dhcp_opt_netmask(&hdr->options[offset], &dhcpn->dhcps->netmask);
+ offset += pico_dhcp_opt_router(&hdr->options[offset], &dhcpn->dhcps->server_ip);
+ offset += pico_dhcp_opt_broadcast(&hdr->options[offset], &broadcast);
+ offset += pico_dhcp_opt_dns(&hdr->options[offset], &dns);
+ offset += pico_dhcp_opt_end(&hdr->options[offset]);
+
+ destination.addr = hdr->yiaddr;
+ r = pico_socket_sendto(dhcpn->dhcps->s, hdr, sizeof(struct pico_dhcp_hdr) + optlen, &destination, PICO_DHCP_CLIENT_PORT);
+ if (r < 0)
+ dhcps_dbg("DHCP server WARNING: failure sending: %s!\n", strerror(pico_err));
+
+ return;
+}
+
+static void pico_dhcp_server_recv(struct pico_socket *s, uint8_t *buf, int len)
+{
+ uint8_t msgtype = 0;
+ int optlen = len - sizeof(struct pico_dhcp_hdr);
+ struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf;
+ struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)hdr->options;
+ struct pico_dhcp_server_negotiation *dhcpn = NULL;
+ struct pico_ip4 reqip = {0}, server_id = {0};
+ struct pico_device *dev = NULL;
+
+ if (!pico_dhcp_are_options_valid(hdr->options, optlen))
+ return;
+
+ dev = pico_ipv4_link_find(&s->local_addr.ip4);
+ dhcpn = pico_dhcp_server_find_negotiation(hdr->xid);
+ if (!dhcpn)
+ dhcpn = pico_dhcp_server_add_negotiation(dev, hdr);
+
+ if (!ip_inrange(dhcpn->ciaddr.addr))
+ return;
+
+ do {
+ switch (opt->code)
+ {
+ case PICO_DHCP_OPT_PAD:
+ break;
+
+ case PICO_DHCP_OPT_END:
+ break;
+
+ case PICO_DHCP_OPT_MSGTYPE:
+ msgtype = opt->ext.msg_type.type;
+ dhcps_dbg("DHCP server: message type %u\n", msgtype);
+ break;
+
+ case PICO_DHCP_OPT_REQIP:
+ reqip = opt->ext.req_ip.ip;
+ dhcps_dbg("DHCP server: requested IP %08X\n", reqip.addr);
+ break;
+
+ case PICO_DHCP_OPT_SERVERID:
+ server_id = opt->ext.server_id.ip;
+ dhcps_dbg("DHCP server: server ID %08X\n", server_id.addr);
+ break;
+
+ default:
+ dhcps_dbg("DHCP server WARNING: unsupported option %u\n", opt->code);
+ break;
+ }
+ } while (pico_dhcp_next_option(&opt));
+
+ switch (msgtype)
+ {
+ case PICO_DHCP_MSG_DISCOVER:
+ dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_OFFER);
+ dhcpn->state = PICO_DHCP_STATE_OFFER;
+ break;
+
+ case PICO_DHCP_MSG_REQUEST:
+ if ((dhcpn->state == PICO_DHCP_STATE_BOUND) && (!reqip.addr) && (!server_id.addr) && (hdr->ciaddr == dhcpn->ciaddr.addr))
+ dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
+ if (dhcpn->state == PICO_DHCP_STATE_OFFER) {
+ dhcpn->state = PICO_DHCP_STATE_BOUND;
+ dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
+ }
+ break;
+
+ default:
+ dhcps_dbg("DHCP server WARNING: unsupported message type %u\n", msgtype);
+ break;
+ }
+ return;
+}
+
+static void pico_dhcpd_wakeup(uint16_t ev, struct pico_socket *s)
+{
+ uint8_t buf[DHCP_SERVER_MAXMSGSIZE] = {0};
+ int r = 0;
+
+ if (ev != PICO_SOCK_EV_RD)
+ return;
+ r = pico_socket_recvfrom(s, buf, DHCP_SERVER_MAXMSGSIZE, NULL, NULL);
+ if (r < 0)
+ return;
+
+ pico_dhcp_server_recv(s, buf, r);
+ return;
+}
+
+int pico_dhcp_server_initiate(struct pico_dhcp_server_setting *setting)
+{
+ if (!setting || !setting->server_ip.addr) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if (pico_dhcp_server_add_setting(setting) == NULL)
+ return -1;
+
+ return 0;
+}
+#endif /* PICO_SUPPORT_DHCP */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_server.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,28 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_DHCP_SERVER
+#define _INCLUDE_PICO_DHCP_SERVER
+
+#include "pico_dhcp_common.h"
+#include "pico_addressing.h"
+
+struct pico_dhcp_server_setting
+{
+ uint32_t pool_start;
+ uint32_t pool_next;
+ uint32_t pool_end;
+ uint32_t lease_time;
+ struct pico_device *dev;
+ struct pico_socket *s;
+ struct pico_ip4 server_ip;
+ struct pico_ip4 netmask;
+ uint8_t flags; /* unused atm */
+};
+
+/* required field: IP address of the interface to serve, only IPs of this network will be served. */
+int pico_dhcp_server_initiate(struct pico_dhcp_server_setting *dhcps);
+
+#endif /* _INCLUDE_PICO_DHCP_SERVER */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dns_client.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,755 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Kristof Roelants
+*********************************************************************/
+#include "pico_config.h"
+#include "pico_stack.h"
+#include "pico_addressing.h"
+#include "pico_socket.h"
+#include "pico_ipv4.h"
+#include "pico_dns_client.h"
+#include "pico_tree.h"
+
+#ifdef PICO_SUPPORT_DNS_CLIENT
+
+#define dns_dbg(...) do{}while(0)
+//#define dns_dbg dbg
+
+/* DNS response length */
+#define PICO_DNS_MAX_RESPONSE_LEN 256
+
+/* DNS client retransmission time (msec) + frequency */
+#define PICO_DNS_CLIENT_RETRANS 4000
+#define PICO_DNS_CLIENT_MAX_RETRANS 3
+
+/* Default nameservers + port */
+#define PICO_DNS_NS_GOOGLE "8.8.8.8"
+#define PICO_DNS_NS_PORT 53
+
+/* FLAG values */
+#define PICO_DNS_QR_QUERY 0
+#define PICO_DNS_QR_RESPONSE 1
+#define PICO_DNS_OPCODE_QUERY 0
+#define PICO_DNS_OPCODE_IQUERY 1
+#define PICO_DNS_OPCODE_STATUS 2
+#define PICO_DNS_AA_NO_AUTHORITY 0
+#define PICO_DNS_AA_IS_AUTHORITY 1
+#define PICO_DNS_TC_NO_TRUNCATION 0
+#define PICO_DNS_TC_IS_TRUNCATED 1
+#define PICO_DNS_RD_NO_DESIRE 0
+#define PICO_DNS_RD_IS_DESIRED 1
+#define PICO_DNS_RA_NO_SUPPORT 0
+#define PICO_DNS_RA_IS_SUPPORTED 1
+#define PICO_DNS_RCODE_NO_ERROR 0
+#define PICO_DNS_RCODE_EFORMAT 1
+#define PICO_DNS_RCODE_ESERVER 2
+#define PICO_DNS_RCODE_ENAME 3
+#define PICO_DNS_RCODE_ENOIMP 4
+#define PICO_DNS_RCODE_EREFUSED 5
+
+/* QTYPE values */
+#define PICO_DNS_TYPE_A 1
+#define PICO_DNS_TYPE_PTR 12
+
+/* QCLASS values */
+#define PICO_DNS_CLASS_IN 1
+
+/* Compression values */
+#define PICO_DNS_LABEL 0
+#define PICO_DNS_POINTER 3
+
+/* Label len */
+#define PICO_DNS_LABEL_INITIAL 1
+#define PICO_DNS_LABEL_ROOT 1
+
+/* TTL values */
+#define PICO_DNS_MAX_TTL 604800 /* one week */
+
+/* Len of an IPv4 address string */
+#define PICO_DNS_IPV4_ADDR_LEN 16
+
+static void pico_dns_client_callback(uint16_t ev, struct pico_socket *s);
+static void pico_dns_client_retransmission(unsigned long now, void *arg);
+
+/* RFC 1035 section 4. MESSAGES */
+struct __attribute__((packed)) pico_dns_name
+{
+ char name[0];
+};
+
+/* prefix = header + name pointer
+ * flags splitted in 2x uint8 due to endianness */
+struct __attribute__((packed)) pico_dns_prefix
+{
+ uint16_t id;
+ uint8_t rd:1; /* recursion desired */
+ uint8_t tc:1; /* truncation */
+ uint8_t aa:1; /* authoritative answer */
+ uint8_t opcode:4; /* opcode */
+ uint8_t qr:1; /* query */
+ uint8_t rcode:4; /* response code */
+ uint8_t z:3; /* zero */
+ uint8_t ra:1; /* recursion available */
+ uint16_t qdcount;
+ uint16_t ancount;
+ uint16_t nscount;
+ uint16_t arcount;
+ struct pico_dns_name domain;
+};
+
+struct __attribute__((packed)) pico_dns_query_suffix
+{
+ uint16_t qtype;
+ uint16_t qclass;
+};
+
+struct __attribute__((packed)) pico_dns_answer_suffix
+{
+ uint16_t qtype;
+ uint16_t qclass;
+ uint32_t ttl;
+ uint16_t rdlength;
+ uint8_t rdata[0];
+};
+
+struct pico_dns_ns
+{
+ struct pico_ip4 ns; /* nameserver */
+};
+
+static int dns_ns_cmp(void *ka, void *kb)
+{
+ struct pico_dns_ns *a = ka, *b = kb;
+ if (a->ns.addr == b->ns.addr)
+ return 0;
+ return (a->ns.addr < b->ns.addr) ? -1 : 1;
+}
+PICO_TREE_DECLARE(NSTable, dns_ns_cmp);
+
+struct pico_dns_query
+{
+ char *query;
+ uint16_t len;
+ uint16_t id;
+ uint16_t qtype;
+ uint16_t qclass;
+ uint8_t retrans;
+ struct pico_dns_ns q_ns;
+ struct pico_socket *s;
+ void (*callback)(char *, void *);
+ void *arg;
+};
+
+static int dns_query_cmp(void *ka, void *kb)
+{
+ struct pico_dns_query *a = ka,*b = kb;
+ if (a->id == b->id)
+ return 0;
+ return (a->id < b->id) ? -1 : 1;
+}
+PICO_TREE_DECLARE(DNSTable, dns_query_cmp);
+
+static int pico_dns_client_del_ns(struct pico_ip4 *ns_addr)
+{
+ struct pico_dns_ns test = {{0}}, *found = NULL;
+
+ test.ns = *ns_addr;
+ found = pico_tree_findKey(&NSTable, &test);
+ if (!found)
+ return -1;
+
+ pico_tree_delete(&NSTable, found);
+ pico_free(found);
+
+ /* no NS left, add default NS */
+ if (pico_tree_empty(&NSTable))
+ pico_dns_client_init();
+
+ return 0;
+}
+
+static struct pico_dns_ns *pico_dns_client_add_ns(struct pico_ip4 *ns_addr)
+{
+ struct pico_dns_ns *dns = NULL, *found = NULL, test = {{0}};
+
+ dns = pico_zalloc(sizeof(struct pico_dns_ns));
+ if (!dns) {
+ pico_err = PICO_ERR_ENOMEM;
+ return NULL;
+ }
+ dns->ns = *ns_addr;
+
+ found = pico_tree_insert(&NSTable, dns);
+ if (found) { /* nameserver already present */
+ pico_free(dns);
+ return found;
+ }
+
+ /* default NS found, remove it */
+ pico_string_to_ipv4(PICO_DNS_NS_GOOGLE, &test.ns.addr);
+ found = pico_tree_findKey(&NSTable, &test);
+ if (found && (found->ns.addr != ns_addr->addr))
+ pico_dns_client_del_ns(&found->ns);
+
+ return dns;
+}
+
+static struct pico_dns_ns pico_dns_client_next_ns(struct pico_ip4 *ns_addr)
+{
+ struct pico_dns_ns dns = {{0}}, *nxtdns = NULL;
+ struct pico_tree_node *node = NULL, *nxtnode = NULL;
+
+ dns.ns = *ns_addr;
+ node = pico_tree_findNode(&NSTable, &dns);
+ if (!node)
+ return dns; /* keep using current NS */
+
+ nxtnode = pico_tree_next(node);
+ nxtdns = nxtnode->keyValue;
+ if (!nxtdns)
+ nxtdns = (struct pico_dns_ns *)pico_tree_first(&NSTable);
+ return *nxtdns;
+}
+
+static struct pico_dns_query *pico_dns_client_add_query(struct pico_dns_prefix *hdr, uint16_t len, struct pico_dns_query_suffix *suffix,
+ void (*callback)(char *, void *), void *arg)
+{
+ struct pico_dns_query *q = NULL, *found = NULL;
+
+ q = pico_zalloc(sizeof(struct pico_dns_query));
+ if (!q)
+ return NULL;
+
+ q->query = (char *)hdr;
+ q->len = len;
+ q->id = short_be(hdr->id);
+ q->qtype = short_be(suffix->qtype);
+ q->qclass = short_be(suffix->qclass);
+ q->retrans = 1;
+ q->q_ns = *((struct pico_dns_ns *)pico_tree_first(&NSTable));
+ q->callback = callback;
+ q->arg = arg;
+ q->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dns_client_callback);
+ if (!q->s) {
+ pico_free(q);
+ return NULL;
+ }
+
+ found = pico_tree_insert(&DNSTable, q);
+ if (found) {
+ pico_err = PICO_ERR_EAGAIN;
+ pico_socket_close(q->s);
+ pico_free(q);
+ return NULL;
+ }
+
+ return q;
+}
+
+static int pico_dns_client_del_query(uint16_t id)
+{
+ struct pico_dns_query test = {0}, *found = NULL;
+
+ test.id = id;
+ found = pico_tree_findKey(&DNSTable, &test);
+ if (!found)
+ return -1;
+
+ pico_free(found->query);
+ pico_socket_close(found->s);
+ pico_tree_delete(&DNSTable, found);
+ pico_free(found);
+ return 0;
+}
+
+static struct pico_dns_query *pico_dns_client_find_query(uint16_t id)
+{
+ struct pico_dns_query test = {0}, *found = NULL;
+
+ test.id = id;
+ found = pico_tree_findKey(&DNSTable, &test);
+ if (found)
+ return found;
+ else
+ return NULL;
+}
+
+/* determine len of string */
+static uint16_t pico_dns_client_strlen(const char *url)
+{
+ char p = 0;
+ uint16_t len = 0;
+
+ if (!url)
+ return 0;
+
+ while ((p = *url++) != 0) {
+ len++;
+ }
+ return len;
+}
+
+/* seek end of string */
+static char *pico_dns_client_seek(char *ptr)
+{
+ char p = 0;
+
+ if (!ptr)
+ return NULL;
+
+ while ((p = *ptr++) != 0);
+ return ptr++;
+}
+
+/* mirror ip address numbers
+ * f.e. 192.168.0.1 => 1.0.168.192 */
+static int pico_dns_client_mirror(char *ptr)
+{
+ const unsigned char *addr = NULL;
+ char *m = ptr;
+ uint32_t ip = 0;
+ int i = 0;
+
+ if (pico_string_to_ipv4(ptr, &ip) < 0)
+ return -1;
+
+ ptr = m;
+ addr = (unsigned char *)&ip;
+ for (i = 3; i >= 0; i--) {
+ if (addr[i] > 99) {
+ *ptr++ = '0' + (addr[i] / 100);
+ *ptr++ = '0' + ((addr[i] % 100) / 10);
+ *ptr++ = '0' + ((addr[i] % 100) % 10);
+ } else if(addr[i] > 9) {
+ *ptr++ = '0' + (addr[i] / 10);
+ *ptr++ = '0' + (addr[i] % 10);
+ } else {
+ *ptr++ = '0' + addr[i];
+ }
+ if(i > 0)
+ *ptr++ = '.';
+ }
+ *ptr = '\0';
+
+ return 0;
+}
+
+static struct pico_dns_query *pico_dns_client_idcheck(uint16_t id)
+{
+ struct pico_dns_query test = {0};
+
+ test.id = id;
+ return pico_tree_findKey(&DNSTable, &test);
+}
+
+static int pico_dns_client_query_prefix(struct pico_dns_prefix *pre)
+{
+ uint16_t id = 0;
+ uint8_t retry = 32;
+
+ do {
+ id = (uint16_t)(pico_rand() & 0xFFFFU);
+ dns_dbg("DNS: generated id %u\n", id);
+ } while (retry-- && pico_dns_client_idcheck(id));
+ if (!retry)
+ return -1;
+
+ pre->id = short_be(id);
+ pre->qr = PICO_DNS_QR_QUERY;
+ pre->opcode = PICO_DNS_OPCODE_QUERY;
+ pre->aa = PICO_DNS_AA_NO_AUTHORITY;
+ pre->tc = PICO_DNS_TC_NO_TRUNCATION;
+ pre->rd = PICO_DNS_RD_IS_DESIRED;
+ pre->ra = PICO_DNS_RA_NO_SUPPORT;
+ pre->z = 0;
+ pre->rcode = PICO_DNS_RCODE_NO_ERROR;
+ pre->qdcount = short_be(1);
+ pre->ancount = short_be(0);
+ pre->nscount = short_be(0);
+ pre->arcount = short_be(0);
+
+ return 0;
+}
+
+static int pico_dns_client_query_suffix(struct pico_dns_query_suffix *suf, uint16_t type, uint16_t class)
+{
+ suf->qtype = short_be(type);
+ suf->qclass = short_be(class);
+ return 0;
+}
+
+/* replace '.' in the domain name by the label length
+ * f.e. www.google.be => 3www6google2be0 */
+static int pico_dns_client_query_domain(char *ptr)
+{
+ char p = 0, *label = NULL;
+ uint8_t len = 0;
+
+ if (!ptr)
+ return -1;
+
+ label = ptr++;
+ while ((p = *ptr++) != 0){
+ if (p == '.') {
+ *label = len;
+ label = ptr - 1;
+ len = 0;
+ } else {
+ len++;
+ }
+ }
+ *label = len;
+ return 0;
+}
+
+/* replace the label length in the domain name by '.'
+ * f.e. 3www6google2be0 => .www.google.be */
+static int pico_dns_client_answer_domain(char *ptr)
+{
+ char p = 0, *label = NULL;
+
+ if (!ptr)
+ return -1;
+
+ label = ptr;
+ while ((p = *ptr++) != 0){
+ ptr += p;
+ *label = '.';
+ label = ptr;
+ }
+ return 0;
+}
+
+static int pico_dns_client_check_prefix(struct pico_dns_prefix *pre)
+{
+ if (pre->qr != PICO_DNS_QR_RESPONSE || pre->opcode != PICO_DNS_OPCODE_QUERY || pre->rcode != PICO_DNS_RCODE_NO_ERROR) {
+ dns_dbg("DNS ERROR: OPCODE %d | TC %d | RCODE %d\n", pre->opcode, pre->tc, pre->rcode);
+ return -1;
+ }
+ if (short_be(pre->ancount) < 1) {
+ dns_dbg("DNS ERROR: ancount < 1\n");
+ return -1;
+ }
+ return 0;
+}
+
+static int pico_dns_client_check_qsuffix(struct pico_dns_query_suffix *suf, struct pico_dns_query *q)
+{
+ if (short_be(suf->qtype) != q->qtype || short_be(suf->qclass) != q->qclass) {
+ dns_dbg("DNS ERROR: received qtype (%u) or qclass (%u) incorrect\n", short_be(suf->qtype), short_be(suf->qclass));
+ return -1;
+ }
+ return 0;
+}
+
+static int pico_dns_client_check_asuffix(struct pico_dns_answer_suffix *suf, struct pico_dns_query *q)
+{
+ if (short_be(suf->qtype) != q->qtype || short_be(suf->qclass) != q->qclass) {
+ dns_dbg("DNS WARNING: received qtype (%u) or qclass (%u) incorrect\n", short_be(suf->qtype), short_be(suf->qclass));
+ return -1;
+ }
+ if (long_be(suf->ttl) > PICO_DNS_MAX_TTL) {
+ dns_dbg("DNS WARNING: received TTL (%u) > MAX (%u)\n", short_be(suf->ttl), PICO_DNS_MAX_TTL);
+ return -1;
+ }
+ return 0;
+}
+
+static char *pico_dns_client_seek_suffix(char *suf, struct pico_dns_prefix *pre, struct pico_dns_query *q)
+{
+ struct pico_dns_answer_suffix *asuffix = NULL;
+ uint16_t comp = 0, compression = 0;
+ uint16_t i = 0;
+ char *psuffix = suf;
+
+ while (i++ < short_be(pre->ancount)) {
+ comp = short_from(psuffix);
+ compression = short_be(comp);
+ switch (compression >> 14)
+ {
+ case PICO_DNS_POINTER:
+ while (compression >> 14 == PICO_DNS_POINTER) {
+ dns_dbg("DNS: pointer\n");
+ psuffix += sizeof(uint16_t);
+ comp = short_from(psuffix);
+ compression = short_be(comp);
+ }
+ break;
+
+ case PICO_DNS_LABEL:
+ dns_dbg("DNS: label\n");
+ psuffix = pico_dns_client_seek(psuffix);
+ break;
+
+ default:
+ dns_dbg("DNS ERROR: incorrect compression (%u) value\n", compression);
+ return NULL;
+ }
+
+ asuffix = (struct pico_dns_answer_suffix *)psuffix;
+ if (pico_dns_client_check_asuffix(asuffix, q) < 0) {
+ psuffix += (sizeof(struct pico_dns_answer_suffix) + short_be(asuffix->rdlength));
+ continue;
+ }
+ return psuffix;
+ }
+ return NULL;
+}
+
+static int pico_dns_client_send(struct pico_dns_query *q)
+{
+ uint16_t * paramID = pico_zalloc(sizeof(uint16_t));
+ dns_dbg("DNS: sending query to %08X\n", q->q_ns.ns.addr);
+ if (!q->s)
+ return -1;
+ if (pico_socket_connect(q->s, &q->q_ns.ns, short_be(PICO_DNS_NS_PORT)) < 0)
+ return -1;
+
+ pico_socket_send(q->s, q->query, q->len);
+ *paramID = q->id;
+ pico_timer_add(PICO_DNS_CLIENT_RETRANS, pico_dns_client_retransmission, paramID);
+
+ return 0;
+}
+
+static void pico_dns_client_retransmission(unsigned long now, void *arg)
+{
+ struct pico_dns_query *q = NULL;
+ struct pico_dns_query dummy;
+ IGNORE_PARAMETER(now);
+
+ if(!arg)
+ return;
+ // search for the dns query and free used space
+ dummy.id = *(uint16_t *)arg;
+ q = (struct pico_dns_query *)pico_tree_findKey(&DNSTable,&dummy);
+ pico_free(arg);
+
+ /* dns query successful? */
+ if (!q) {
+ return;
+ }
+
+ if (q->retrans++ <= PICO_DNS_CLIENT_MAX_RETRANS) {
+ q->q_ns = pico_dns_client_next_ns(&q->q_ns.ns);
+ pico_dns_client_send(q);
+ } else {
+ pico_err = PICO_ERR_EIO;
+ q->callback(NULL, q->arg);
+ pico_dns_client_del_query(q->id);
+ }
+}
+
+static int pico_dns_client_user_callback(struct pico_dns_answer_suffix *asuffix, struct pico_dns_query *q)
+{
+ uint32_t ip = 0;
+ char *str = NULL;
+
+ switch (q->qtype)
+ {
+ case PICO_DNS_TYPE_A:
+ ip = long_from(asuffix->rdata);
+ str = pico_zalloc(PICO_DNS_IPV4_ADDR_LEN);
+ pico_ipv4_to_string(str, ip);
+ break;
+
+ case PICO_DNS_TYPE_PTR:
+ pico_dns_client_answer_domain((char *)asuffix->rdata);
+ str = pico_zalloc(asuffix->rdlength - PICO_DNS_LABEL_INITIAL);
+ memcpy(str, asuffix->rdata + PICO_DNS_LABEL_INITIAL, short_be(asuffix->rdlength) - PICO_DNS_LABEL_INITIAL);
+ break;
+
+ default:
+ dns_dbg("DNS ERROR: incorrect qtype (%u)\n", q->qtype);
+ break;
+ }
+
+ if (q->retrans) {
+ q->callback(str, q->arg);
+ q->retrans = 0;
+ pico_dns_client_del_query(q->id);
+ }
+ return 0;
+}
+
+static void pico_dns_client_callback(uint16_t ev, struct pico_socket *s)
+{
+ struct pico_dns_prefix *prefix = NULL;
+ struct pico_dns_name *domain = NULL;
+ struct pico_dns_query_suffix *qsuffix = NULL;
+ struct pico_dns_answer_suffix *asuffix = NULL;
+ struct pico_dns_query *q = NULL;
+ char *p_asuffix = NULL;
+ char msg[PICO_DNS_MAX_RESPONSE_LEN] = {0};
+
+ if (ev == PICO_SOCK_EV_ERR) {
+ dns_dbg("DNS: socket error received\n");
+ return;
+ }
+
+ if (ev & PICO_SOCK_EV_RD) {
+ if (pico_socket_read(s, msg, PICO_DNS_MAX_RESPONSE_LEN) <= 0)
+ return;
+ }
+
+ prefix = (struct pico_dns_prefix *)msg;
+ domain = &prefix->domain;
+ qsuffix = (struct pico_dns_query_suffix *)pico_dns_client_seek(domain->name);
+ /* valid asuffix is determined dynamically later on */
+
+ if (pico_dns_client_check_prefix(prefix) < 0)
+ return;
+ q = pico_dns_client_find_query(short_be(prefix->id));
+ if (!q)
+ return;
+ if (pico_dns_client_check_qsuffix(qsuffix, q) < 0)
+ return;
+
+ p_asuffix = (char *)qsuffix + sizeof(struct pico_dns_query_suffix);
+ p_asuffix = pico_dns_client_seek_suffix(p_asuffix, prefix, q);
+ if (!p_asuffix)
+ return;
+
+ asuffix = (struct pico_dns_answer_suffix *)p_asuffix;
+ pico_dns_client_user_callback(asuffix, q);
+
+ return;
+}
+
+int pico_dns_client_getaddr(const char *url, void (*callback)(char *, void *), void *arg)
+{
+ char *msg = NULL;
+ struct pico_dns_prefix *prefix = NULL;
+ struct pico_dns_name *domain = NULL;
+ struct pico_dns_query_suffix *qsuffix = NULL;
+ struct pico_dns_query *q = NULL;
+ uint16_t len = 0, lblen = 0, strlen = 0;
+
+ if (!url || !callback) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ strlen = pico_dns_client_strlen(url);
+ lblen = PICO_DNS_LABEL_INITIAL + strlen + PICO_DNS_LABEL_ROOT;
+ len = sizeof(struct pico_dns_prefix) + lblen + sizeof(struct pico_dns_query_suffix);
+ msg = pico_zalloc(len);
+ if (!msg) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ prefix = (struct pico_dns_prefix *)msg;
+ domain = &prefix->domain;
+ qsuffix = (struct pico_dns_query_suffix *)(domain->name + lblen);
+ memcpy(domain->name + PICO_DNS_LABEL_INITIAL, url, strlen);
+
+ /* assemble dns message */
+ pico_dns_client_query_prefix(prefix);
+ pico_dns_client_query_domain(domain->name);
+ pico_dns_client_query_suffix(qsuffix, PICO_DNS_TYPE_A, PICO_DNS_CLASS_IN);
+
+ q = pico_dns_client_add_query(prefix, len, qsuffix, callback, arg);
+ if (!q) {
+ pico_free(msg);
+ return -1;
+ }
+ if (pico_dns_client_send(q) < 0) {
+ pico_dns_client_del_query(q->id); /* frees msg */
+ return -1;
+ }
+
+ return 0;
+}
+
+int pico_dns_client_getname(const char *ip, void (*callback)(char *, void *), void *arg)
+{
+ const char *inaddr_arpa = ".in-addr.arpa";
+ char *msg = NULL;
+ struct pico_dns_prefix *prefix = NULL;
+ struct pico_dns_name *domain = NULL;
+ struct pico_dns_query_suffix *qsuffix = NULL;
+ struct pico_dns_query *q = NULL;
+ uint16_t len = 0, lblen = 0, strlen = 0, arpalen = 0;
+
+ if (!ip || !callback) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ strlen = pico_dns_client_strlen(ip);
+ arpalen = pico_dns_client_strlen(inaddr_arpa);
+ lblen = PICO_DNS_LABEL_INITIAL + strlen + arpalen + PICO_DNS_LABEL_ROOT;
+ len = sizeof(struct pico_dns_prefix) + lblen + sizeof(struct pico_dns_query_suffix);
+ msg = pico_zalloc(len);
+ if (!msg) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ prefix = (struct pico_dns_prefix *)msg;
+ domain = &prefix->domain;
+ qsuffix = (struct pico_dns_query_suffix *)(prefix->domain.name + lblen);
+ memcpy(domain->name + PICO_DNS_LABEL_INITIAL, ip, strlen);
+ pico_dns_client_mirror(domain->name + PICO_DNS_LABEL_INITIAL);
+ memcpy(domain->name + PICO_DNS_LABEL_INITIAL + strlen, inaddr_arpa, arpalen);
+
+ /* assemble dns message */
+ pico_dns_client_query_prefix(prefix);
+ pico_dns_client_query_domain(domain->name);
+ pico_dns_client_query_suffix(qsuffix, PICO_DNS_TYPE_PTR, PICO_DNS_CLASS_IN);
+
+ q = pico_dns_client_add_query(prefix, len, qsuffix, callback, arg);
+ if (!q) {
+ pico_free(msg);
+ return -1;
+ }
+ if (pico_dns_client_send(q) < 0) {
+ pico_dns_client_del_query(q->id); /* frees msg */
+ return -1;
+ }
+
+ return 0;
+}
+
+int pico_dns_client_nameserver(struct pico_ip4 *ns, uint8_t flag)
+{
+ if (!ns) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ switch (flag)
+ {
+ case PICO_DNS_NS_ADD:
+ if (!pico_dns_client_add_ns(ns))
+ return -1;
+ break;
+
+ case PICO_DNS_NS_DEL:
+ if (pico_dns_client_del_ns(ns) < 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ break;
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ return 0;
+}
+
+int pico_dns_client_init()
+{
+ struct pico_ip4 default_ns = {0};
+
+ if (pico_string_to_ipv4(PICO_DNS_NS_GOOGLE, &default_ns.addr) < 0)
+ return -1;
+
+ return pico_dns_client_nameserver(&default_ns, PICO_DNS_NS_ADD);
+}
+
+#endif /* PICO_SUPPORT_DNS_CLIENT */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_dns_client.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,23 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +. + +Authors: Kristof Roelants +*********************************************************************/ + +#ifndef _INCLUDE_PICO_DNS_CLIENT +#define _INCLUDE_PICO_DNS_CLIENT + +#define PICO_DNS_NS_DEL 0 +#define PICO_DNS_NS_ADD 1 +#include <stdint.h> + +int pico_dns_client_init(); +/* flag is PICO_DNS_NS_DEL or PICO_DNS_NS_ADD */ +int pico_dns_client_nameserver(struct pico_ip4 *ns, uint8_t flag); +int pico_dns_client_getaddr(const char *url, void (*callback)(char *ip, void *arg), void *arg); +int pico_dns_client_getname(const char *ip, void (*callback)(char *url, void *arg), void *arg); + +#endif /* _INCLUDE_PICO_DNS_CLIENT */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_client.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,701 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Author: Andrei Carp <andrei.carp@tass.be>
+*********************************************************************/
+#include <string.h>
+#include <stdint.h>
+#include "pico_tree.h"
+#include "pico_config.h"
+#include "pico_socket.h"
+#include "pico_tcp.h"
+#include "pico_dns_client.h"
+#include "pico_http_client.h"
+#include "pico_ipv4.h"
+#include "pico_stack.h"
+
+/*
+ * This is the size of the following header
+ *
+ * GET <resource> HTTP/1.1<CRLF>
+ * Host: <host>:<port><CRLF>
+ * User-Agent: picoTCP<CRLF>
+ * Connection: close<CRLF>
+ * <CRLF>
+ *
+ * where <resource>,<host> and <port> will be added later.
+ */
+
+#ifdef PICO_SUPPORT_HTTP_CLIENT
+
+#define HTTP_GET_BASIC_SIZE 63u
+#define HTTP_HEADER_LINE_SIZE 50u
+#define RESPONSE_INDEX 9u
+
+#define HTTP_CHUNK_ERROR 0xFFFFFFFFu
+
+#ifdef dbg
+ #undef dbg
+ #define dbg(...) do{}while(0);
+#endif
+
+#define consumeChar(c) (pico_socket_read(client->sck,&c,1u))
+#define isLocation(line) (memcmp(line,"Location",8u) == 0)
+#define isContentLength(line) (memcmp(line,"Content-Length",14u) == 0u)
+#define isTransferEncoding(line) (memcmp(line,"Transfer-Encoding",17u) == 0u)
+#define isChunked(line) (memcmp(line," chunked",8u) == 0u)
+#define isNotHTTPv1(line) (memcmp(line,"HTTP/1.",7u))
+#define is_hex_digit(x) ( ('0' <= x && x <= '9') || ('a' <= x && x <= 'f') )
+#define hex_digit_to_dec(x) ( ('0' <= x && x <= '9') ? x-'0' : ( ('a' <= x && x <= 'f') ? x-'a' + 10 : -1) )
+
+struct pico_http_client
+{
+ uint16_t connectionID;
+ uint8_t state;
+ struct pico_socket * sck;
+ void (*wakeup)(uint16_t ev, uint16_t conn);
+ struct pico_ip4 ip;
+ struct pico_http_uri * uriKey;
+ struct pico_http_header * header;
+};
+
+// HTTP Client internal states
+#define HTTP_READING_HEADER 0
+#define HTTP_READING_BODY 1
+#define HTTP_READING_CHUNK_VALUE 2
+#define HTTP_READING_CHUNK_TRAIL 3
+
+
+static int compareClients(void * ka, void * kb)
+{
+ return ((struct pico_http_client *)ka)->connectionID - ((struct pico_http_client *)kb)->connectionID;
+}
+
+PICO_TREE_DECLARE(pico_client_list,compareClients);
+
+// Local functions
+int parseHeaderFromServer(struct pico_http_client * client, struct pico_http_header * header);
+int readChunkLine(struct pico_http_client * client);
+
+void tcpCallback(uint16_t ev, struct pico_socket *s)
+{
+
+ struct pico_http_client * client = NULL;
+ struct pico_tree_node * index;
+
+ // find httpClient
+ pico_tree_foreach(index,&pico_client_list)
+ {
+ if( ((struct pico_http_client *)index->keyValue)->sck == s )
+ {
+ client = (struct pico_http_client *)index->keyValue;
+ break;
+ }
+ }
+
+ if(!client)
+ {
+ dbg("Client not found...Something went wrong !\n");
+ return;
+ }
+
+ if(ev & PICO_SOCK_EV_CONN)
+ client->wakeup(EV_HTTP_CON,client->connectionID);
+
+ if(ev & PICO_SOCK_EV_RD)
+ {
+
+ // read the header, if not read
+ if(client->state == HTTP_READING_HEADER)
+ {
+ // wait for header
+ client->header = pico_zalloc(sizeof(struct pico_http_header));
+ if(!client->header)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ return;
+ }
+
+ // wait for header
+ if(parseHeaderFromServer(client,client->header) < 0)
+ {
+ client->wakeup(EV_HTTP_ERROR,client->connectionID);
+ }
+ else
+ {
+ // call wakeup
+ if(client->header->responseCode != HTTP_CONTINUE)
+ {
+ client->wakeup(
+ client->header->responseCode == HTTP_OK ?
+ EV_HTTP_REQ | EV_HTTP_BODY : // data comes for sure only when 200 is received
+ EV_HTTP_REQ
+ ,client->connectionID);
+ }
+ }
+ }
+ else
+ {
+ // just let the user know that data has arrived, if chunked data comes, will be treated in the
+ // read api.
+ client->wakeup(EV_HTTP_BODY,client->connectionID);
+ }
+ }
+
+ if(ev & PICO_SOCK_EV_ERR)
+ {
+ client->wakeup(EV_HTTP_ERROR,client->connectionID);
+ }
+
+ if( (ev & PICO_SOCK_EV_CLOSE) || (ev & PICO_SOCK_EV_FIN) )
+ {
+ client->wakeup(EV_HTTP_CLOSE,client->connectionID);
+ }
+
+}
+
+// used for getting a response from DNS servers
+static void dnsCallback(char *ip, void * ptr)
+{
+ struct pico_http_client * client = (struct pico_http_client *)ptr;
+
+ if(!client)
+ {
+ dbg("Who made the request ?!\n");
+ return;
+ }
+
+ if(ip)
+ {
+ client->wakeup(EV_HTTP_DNS,client->connectionID);
+
+ // add the ip address to the client, and start a tcp connection socket
+ pico_string_to_ipv4(ip,&client->ip.addr);
+ pico_free(ip);
+ client->sck = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_TCP, &tcpCallback);
+ if(!client->sck)
+ {
+ client->wakeup(EV_HTTP_ERROR,client->connectionID);
+ return;
+ }
+
+ if(pico_socket_connect(client->sck,&client->ip,short_be(client->uriKey->port)) < 0)
+ {
+ client->wakeup(EV_HTTP_ERROR,client->connectionID);
+ return;
+ }
+
+ }
+ else
+ {
+ // wakeup client and let know error occured
+ client->wakeup(EV_HTTP_ERROR,client->connectionID);
+
+ // close the client (free used heap)
+ pico_http_client_close(client->connectionID);
+ }
+}
+
+/*
+ * API used for opening a new HTTP Client.
+ *
+ * The accepted uri's are [http://]hostname[:port]/resource
+ * no relative uri's are accepted.
+ *
+ * The function returns a connection ID >= 0 if successful
+ * -1 if an error occured.
+ */
+int pico_http_client_open(char * uri, void (*wakeup)(uint16_t ev, uint16_t conn))
+{
+ struct pico_http_client * client;
+
+ if(!wakeup)
+ {
+ pico_err = PICO_ERR_EINVAL;
+ return HTTP_RETURN_ERROR;
+ }
+
+ client = pico_zalloc(sizeof(struct pico_http_client));
+ if(!client)
+ {
+ // memory error
+ pico_err = PICO_ERR_ENOMEM;
+ return HTTP_RETURN_ERROR;
+ }
+
+ client->wakeup = wakeup;
+ client->connectionID = (uint16_t)pico_rand() & 0x7FFFu; // negative values mean error, still not good generation
+
+ client->uriKey = pico_zalloc(sizeof(struct pico_http_uri));
+
+ if(!client->uriKey)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ pico_free(client);
+ return HTTP_RETURN_ERROR;
+ }
+
+ pico_processURI(uri,client->uriKey);
+
+ if(pico_tree_insert(&pico_client_list,client))
+ {
+ // already in
+ pico_err = PICO_ERR_EEXIST;
+ pico_free(client->uriKey);
+ pico_free(client);
+ return HTTP_RETURN_ERROR;
+ }
+
+ // dns query
+ dbg("Querying : %s \n",client->uriKey->host);
+ pico_dns_client_getaddr(client->uriKey->host, dnsCallback,client);
+
+ // return the connection ID
+ return client->connectionID;
+}
+
+/*
+ * API for sending a header to the client.
+ *
+ * if hdr == HTTP_HEADER_RAW , then the parameter header
+ * is sent as it is to client.
+ *
+ * if hdr == HTTP_HEADER_DEFAULT, then the parameter header
+ * is ignored and the library will build the response header
+ * based on the uri passed when opening the client.
+ *
+ */
+int pico_http_client_sendHeader(uint16_t conn, char * header, int hdr)
+{
+ struct pico_http_client search = {.connectionID = conn};
+ struct pico_http_client * http = pico_tree_findKey(&pico_client_list,&search);
+ int length ;
+ if(!http)
+ {
+ dbg("Client not found !\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ // the api gives the possibility to the user to build the GET header
+ // based on the uri passed when opening the client, less headache for the user
+ if(hdr == HTTP_HEADER_DEFAULT)
+ {
+ header = pico_http_client_buildHeader(http->uriKey);
+
+ if(!header)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ return HTTP_RETURN_ERROR;
+ }
+ }
+
+ length = pico_socket_write(http->sck,(void *)header,strlen(header)+1);
+
+ if(hdr == HTTP_HEADER_DEFAULT)
+ pico_free(header);
+
+ return length;
+}
+
+/*
+ * API for reading received data.
+ *
+ * This api hides from the user if the transfer-encoding
+ * was chunked or a full length was provided, in case of
+ * a chunked transfer encoding will "de-chunk" the data
+ * and pass it to the user.
+ */
+int pico_http_client_readData(uint16_t conn, char * data, uint16_t size)
+{
+ struct pico_http_client dummy = {.connectionID = conn};
+ struct pico_http_client * client = pico_tree_findKey(&pico_client_list,&dummy);
+
+ if(!client)
+ {
+ dbg("Wrong connection id !\n");
+ pico_err = PICO_ERR_EINVAL;
+ return HTTP_RETURN_ERROR;
+ }
+
+ // for the moment just read the data, do not care if it's chunked or not
+ if(client->header->transferCoding == HTTP_TRANSFER_FULL)
+ return pico_socket_read(client->sck,(void *)data,size);
+ else
+ {
+ int lenRead = 0;
+
+ // read the chunk line
+ if(readChunkLine(client) == HTTP_RETURN_ERROR)
+ {
+ dbg("Probably the chunk is malformed or parsed wrong...\n");
+ client->wakeup(EV_HTTP_ERROR,client->connectionID);
+ return HTTP_RETURN_ERROR;
+ }
+
+ // nothing to read, no use to try
+ if(client->state != HTTP_READING_BODY)
+ {
+ pico_err = PICO_ERR_EAGAIN;
+ return HTTP_RETURN_OK;
+ }
+
+ // check if we need more than one chunk
+ if(size >= client->header->contentLengthOrChunk)
+ {
+ // read the rest of the chunk, if chunk is done, proceed to the next chunk
+ while(lenRead <= size)
+ {
+ int tmpLenRead = 0;
+
+ if(client->state == HTTP_READING_BODY)
+ {
+
+ // if needed truncate the data
+ tmpLenRead = pico_socket_read(client->sck,data + lenRead,
+ client->header->contentLengthOrChunk < (uint32_t)(size-lenRead) ? client->header->contentLengthOrChunk : (uint32_t)(size-lenRead));
+
+ if(tmpLenRead > 0)
+ {
+ client->header->contentLengthOrChunk -= tmpLenRead;
+ }
+ else if(tmpLenRead < 0)
+ {
+ // error on reading
+ dbg(">>> Error returned pico_socket_read\n");
+ pico_err = PICO_ERR_EBUSY;
+ // return how much data was read until now
+ return lenRead;
+ }
+ }
+
+ lenRead += tmpLenRead;
+ if(readChunkLine(client) == HTTP_RETURN_ERROR)
+ {
+ dbg("Probably the chunk is malformed or parsed wrong...\n");
+ client->wakeup(EV_HTTP_ERROR,client->connectionID);
+ return HTTP_RETURN_ERROR;
+ }
+
+ if(client->state != HTTP_READING_BODY || !tmpLenRead) break;
+
+ }
+ }
+ else
+ {
+ // read the data from the chunk
+ lenRead = pico_socket_read(client->sck,(void *)data,size);
+
+ if(lenRead)
+ client->header->contentLengthOrChunk -= lenRead;
+ }
+
+ return lenRead;
+ }
+}
+
+/*
+ * API for reading received data.
+ *
+ * Reads out the header struct received from server.
+ */
+struct pico_http_header * pico_http_client_readHeader(uint16_t conn)
+{
+ struct pico_http_client dummy = {.connectionID = conn};
+ struct pico_http_client * client = pico_tree_findKey(&pico_client_list,&dummy);
+
+ if(client)
+ {
+ return client->header;
+ }
+ else
+ {
+ // not found
+ dbg("Wrong connection id !\n");
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+ }
+}
+
+/*
+ * API for reading received data.
+ *
+ * Reads out the uri struct after was processed.
+ */
+struct pico_http_uri * pico_http_client_readUriData(uint16_t conn)
+{
+ struct pico_http_client dummy = {.connectionID = conn};
+ struct pico_http_client * client = pico_tree_findKey(&pico_client_list,&dummy);
+ //
+ if(client)
+ return client->uriKey;
+ else
+ {
+ // not found
+ dbg("Wrong connection id !\n");
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+ }
+}
+
+/*
+ * API for reading received data.
+ *
+ * Close the client.
+ */
+int pico_http_client_close(uint16_t conn)
+{
+ struct pico_http_client * toBeRemoved = NULL;
+ struct pico_http_client dummy = {0};
+ dummy.connectionID = conn;
+
+ dbg("Closing the client...\n");
+ toBeRemoved = pico_tree_delete(&pico_client_list,&dummy);
+ if(!toBeRemoved)
+ {
+ dbg("Warning ! Element not found ...");
+ return HTTP_RETURN_ERROR;
+ }
+
+ // close socket
+ if(toBeRemoved->sck)
+ pico_socket_close(toBeRemoved->sck);
+
+
+ if(toBeRemoved->header)
+ {
+ // free space used
+ if(toBeRemoved->header->location)
+ pico_free(toBeRemoved->header->location);
+
+ pico_free(toBeRemoved->header);
+ }
+
+ if(toBeRemoved->uriKey)
+ {
+ if(toBeRemoved->uriKey->host)
+ pico_free(toBeRemoved->uriKey->host);
+
+ if(toBeRemoved->uriKey->resource)
+ pico_free(toBeRemoved->uriKey->resource);
+ pico_free(toBeRemoved->uriKey);
+ }
+ pico_free(toBeRemoved);
+
+ return 0;
+}
+
+/*
+ * API for reading received data.
+ *
+ * Builds a GET header based on the fields on the uri.
+ */
+char * pico_http_client_buildHeader(const struct pico_http_uri * uriData)
+{
+ char * header;
+ char port[6u]; // 6 = max length of a uint16 + \0
+
+ uint16_t headerSize = HTTP_GET_BASIC_SIZE;
+
+ if(!uriData->host || !uriData->resource || !uriData->port)
+ {
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+ }
+
+ //
+ headerSize += strlen(uriData->host) + strlen(uriData->resource) + pico_itoa(uriData->port,port) + 4u; // 3 = size(CRLF + \0)
+ header = pico_zalloc(headerSize);
+
+ if(!header)
+ {
+ // not enought memory
+ pico_err = PICO_ERR_ENOMEM;
+ return NULL;
+ }
+
+ // build the actual header
+ strcpy(header,"GET ");
+ strcat(header,uriData->resource);
+ strcat(header," HTTP/1.1\r\n");
+ strcat(header,"Host: ");
+ strcat(header,uriData->host);
+ strcat(header,":");
+ strcat(header,port);
+ strcat(header,"\r\n");
+ strcat(header,"User-Agent: picoTCP\r\nConnection: close\r\n\r\n"); //?
+
+ return header;
+}
+
+int parseHeaderFromServer(struct pico_http_client * client, struct pico_http_header * header)
+{
+ char line[HTTP_HEADER_LINE_SIZE];
+ char c;
+ uint32_t index = 0;
+
+ // read the first line of the header
+ while(consumeChar(c)>0 && c!='\r')
+ {
+ if(index < HTTP_HEADER_LINE_SIZE) // truncate if too long
+ line[index++] = c;
+ }
+
+ consumeChar(c); // consume \n
+
+ // check the integrity of the response
+ // make sure we have enough characters to include the response code
+ // make sure the server response starts with HTTP/1.
+ if(index < RESPONSE_INDEX+2 || isNotHTTPv1(line))
+ {
+ // wrong format of the the response
+ pico_err = PICO_ERR_EINVAL;
+ return HTTP_RETURN_ERROR;
+ }
+
+ // extract response code
+ header->responseCode = (line[RESPONSE_INDEX] - '0') * 100u +
+ (line[RESPONSE_INDEX+1] - '0') * 10u +
+ (line[RESPONSE_INDEX+2] - '0');
+
+
+ if(header->responseCode/100u > 5u)
+ {
+ // invalid response type
+ header->responseCode = 0;
+ return HTTP_RETURN_ERROR;
+ }
+
+ dbg("Server response : %d \n",header->responseCode);
+
+ // parse the rest of the header
+ while(consumeChar(c)>0)
+ {
+ if(c==':')
+ {
+ // check for interesting fields
+
+ // Location:
+ if(isLocation(line))
+ {
+ index = 0;
+ while(consumeChar(c)>0 && c!='\r')
+ {
+ line[index++] = c;
+ }
+
+ // allocate space for the field
+ header->location = pico_zalloc(index+1u);
+ if(!header->location)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ return HTTP_RETURN_ERROR;
+ }
+
+ memcpy(header->location,line,index);
+
+ }// Content-Length:
+ else if(isContentLength(line))
+ {
+ header->contentLengthOrChunk = 0u;
+ header->transferCoding = HTTP_TRANSFER_FULL;
+ // consume the first space
+ consumeChar(c);
+ while(consumeChar(c)>0 && c!='\r')
+ {
+ header->contentLengthOrChunk = header->contentLengthOrChunk*10u + (c-'0');
+ }
+
+ }// Transfer-Encoding: chunked
+ else if(isTransferEncoding(line))
+ {
+ index = 0;
+ while(consumeChar(c)>0 && c!='\r')
+ {
+ line[index++] = c;
+ }
+
+ if(isChunked(line))
+ {
+ header->contentLengthOrChunk = 0u;
+ header->transferCoding = HTTP_TRANSFER_CHUNKED;
+ }
+
+ }// just ignore the line
+ else
+ {
+ while(consumeChar(c)>0 && c!='\r');
+ }
+
+ // consume the next one
+ consumeChar(c);
+ // reset the index
+ index = 0u;
+ }
+ else if(c=='\r' && !index)
+ {
+ // consume the \n
+ consumeChar(c);
+ break;
+ }
+ else
+ {
+ line[index++] = c;
+ }
+ }
+
+ if(header->transferCoding == HTTP_TRANSFER_CHUNKED)
+ {
+ // read the first chunk
+ header->contentLengthOrChunk = 0;
+
+ client->state = HTTP_READING_CHUNK_VALUE;
+ readChunkLine(client);
+
+ }
+ else
+ client->state = HTTP_READING_BODY;
+
+ dbg("End of header\n");
+ return HTTP_RETURN_OK;
+
+}
+
+// an async read of the chunk part, since in theory a chunk can be split in 2 packets
+int readChunkLine(struct pico_http_client * client)
+{
+ char c = 0;
+
+ if(client->header->contentLengthOrChunk==0 && client->state == HTTP_READING_BODY)
+ {
+ client->state = HTTP_READING_CHUNK_VALUE;
+ }
+
+ if(client->state == HTTP_READING_CHUNK_VALUE)
+ {
+ while(consumeChar(c)>0 && c!='\r' && c!=';')
+ {
+ if(is_hex_digit(c))
+ client->header->contentLengthOrChunk = (client->header->contentLengthOrChunk << 4u) + hex_digit_to_dec(c);
+ else
+ {
+ pico_err = PICO_ERR_EINVAL;
+ // something went wrong
+ return HTTP_RETURN_ERROR;
+ }
+ }
+
+ if(c=='\r' || c==';') client->state = HTTP_READING_CHUNK_TRAIL;
+ }
+
+ if(client->state == HTTP_READING_CHUNK_TRAIL)
+ {
+
+ while(consumeChar(c)>0 && c!='\n');
+
+ if(c=='\n') client->state = HTTP_READING_BODY;
+ }
+
+ return HTTP_RETURN_OK;
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_client.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,49 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Author: Andrei Carp <andrei.carp@tass.be>
+*********************************************************************/
+
+
+#ifndef PICO_HTTP_CLIENT_H_
+#define PICO_HTTP_CLIENT_H_
+
+#include "pico_http_util.h"
+
+/*
+ * Transfer encodings
+ */
+#define HTTP_TRANSFER_CHUNKED 1u
+#define HTTP_TRANSFER_FULL 0u
+
+/*
+ * Parameters for the send header function
+ */
+#define HTTP_HEADER_RAW 0u
+#define HTTP_HEADER_DEFAULT 1u
+
+/*
+ * Data types
+ */
+
+struct pico_http_header
+{
+ uint16_t responseCode; // http response
+ char * location; // if redirect is reported
+ uint32_t contentLengthOrChunk; // size of the message
+ uint8_t transferCoding; // chunked or full
+
+};
+
+int pico_http_client_open(char * uri, void (*wakeup)(uint16_t ev, uint16_t conn));
+int pico_http_client_sendHeader(uint16_t conn, char * header, int hdr);
+
+struct pico_http_header * pico_http_client_readHeader(uint16_t conn);
+struct pico_http_uri * pico_http_client_readUriData(uint16_t conn);
+char * pico_http_client_buildHeader(const struct pico_http_uri * uriData);
+
+int pico_http_client_readData(uint16_t conn, char * data, uint16_t size);
+int pico_http_client_close(uint16_t conn);
+
+#endif /* PICO_HTTP_CLIENT_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_server.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,636 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Author: Andrei Carp <andrei.carp@tass.be>
+*********************************************************************/
+
+#include "pico_stack.h"
+#include "pico_http_server.h"
+#include "pico_tcp.h"
+#include "pico_tree.h"
+#include "pico_socket.h"
+
+#ifdef PICO_SUPPORT_HTTP_SERVER
+
+#define BACKLOG 10
+
+#define HTTP_SERVER_CLOSED 0
+#define HTTP_SERVER_LISTEN 1
+
+#define HTTP_HEADER_MAX_LINE 256u
+
+#define consumeChar(c) (pico_socket_read(client->sck,&c,1u))
+
+static const char returnOkHeader[] =
+"HTTP/1.1 200 OK\r\n\
+Host: localhost\r\n\
+Transfer-Encoding: chunked\r\n\
+Connection: close\r\n\
+\r\n";
+
+static const char returnFailHeader[] =
+"HTTP/1.1 404 Not Found\r\n\
+Host: localhost\r\n\
+Connection: close\r\n\
+\r\n\
+<html><body>The resource you requested cannot be found !</body></html>";
+
+static const char errorHeader[] =
+"HTTP/1.1 400 Bad Request\r\n\
+Host: localhost\r\n\
+Connection: close\r\n\
+\r\n\
+<html><body>There was a problem with your request !</body></html>";
+
+struct httpServer
+{
+ uint16_t state;
+ struct pico_socket * sck;
+ uint16_t port;
+ void (*wakeup)(uint16_t ev, uint16_t param);
+ uint8_t accepted;
+};
+
+struct httpClient
+{
+ uint16_t connectionID;
+ struct pico_socket * sck;
+ void * buffer;
+ uint16_t bufferSize;
+ uint16_t bufferSent;
+ char * resource;
+ uint16_t state;
+};
+
+/* Local states for clients */
+#define HTTP_WAIT_HDR 0
+#define HTTP_WAIT_EOF_HDR 1
+#define HTTP_EOF_HDR 2
+#define HTTP_WAIT_RESPONSE 3
+#define HTTP_WAIT_DATA 4
+#define HTTP_SENDING_DATA 5
+#define HTTP_ERROR 6
+#define HTTP_CLOSED 7
+
+static struct httpServer server = {0};
+
+/*
+ * Private functions
+ */
+static int parseRequest(struct httpClient * client);
+static int readRemainingHeader(struct httpClient * client);
+static void sendData(struct httpClient * client);
+static inline int readData(struct httpClient * client); // used only in a place
+static inline struct httpClient * findClient(uint16_t conn);
+
+static int compareClients(void * ka, void * kb)
+{
+ return ((struct httpClient *)ka)->connectionID - ((struct httpClient *)kb)->connectionID;
+}
+
+PICO_TREE_DECLARE(pico_http_clients,compareClients);
+
+void httpServerCbk(uint16_t ev, struct pico_socket *s)
+{
+ struct pico_tree_node * index;
+ struct httpClient * client = NULL;
+ uint8_t serverEvent = FALSE;
+
+ // determine the client for the socket
+ if( s == server.sck)
+ {
+ serverEvent = TRUE;
+ }
+ else
+ {
+ pico_tree_foreach(index,&pico_http_clients)
+ {
+ client = index->keyValue;
+ if(client->sck == s) break;
+ client = NULL;
+ }
+ }
+
+ if(!client && !serverEvent)
+ {
+ return;
+ }
+
+ if (ev & PICO_SOCK_EV_RD)
+ {
+
+ if(readData(client) == HTTP_RETURN_ERROR)
+ {
+ // send out error
+ client->state = HTTP_ERROR;
+ pico_socket_write(client->sck,(const char *)errorHeader,sizeof(errorHeader)-1);
+ server.wakeup(EV_HTTP_ERROR,client->connectionID);
+ }
+ }
+
+ if(ev & PICO_SOCK_EV_WR)
+ {
+ if(client->state == HTTP_SENDING_DATA)
+ {
+ sendData(client);
+ }
+ }
+
+ if(ev & PICO_SOCK_EV_CONN)
+ {
+ server.accepted = FALSE;
+ server.wakeup(EV_HTTP_CON,HTTP_SERVER_ID);
+ if(!server.accepted)
+ {
+ pico_socket_close(s); // reject socket
+ }
+ }
+
+ if( (ev & PICO_SOCK_EV_CLOSE) || (ev & PICO_SOCK_EV_FIN) )
+ {
+ server.wakeup(EV_HTTP_CLOSE,(serverEvent ? HTTP_SERVER_ID : client->connectionID));
+ }
+
+ if(ev & PICO_SOCK_EV_ERR)
+ {
+ server.wakeup(EV_HTTP_ERROR,(serverEvent ? HTTP_SERVER_ID : client->connectionID));
+ }
+}
+
+/*
+ * API for starting the server. If 0 is passed as a port, the port 80
+ * will be used.
+ */
+int pico_http_server_start(uint16_t port, void (*wakeup)(uint16_t ev, uint16_t conn))
+{
+ struct pico_ip4 anything = {0};
+
+ server.port = port ? short_be(port) : short_be(80u);
+
+ if(!wakeup)
+ {
+ pico_err = PICO_ERR_EINVAL;
+ return HTTP_RETURN_ERROR;
+ }
+
+ server.sck = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_TCP, &httpServerCbk);
+
+ if(!server.sck)
+ {
+ pico_err = PICO_ERR_EFAULT;
+ return HTTP_RETURN_ERROR;
+ }
+
+ if(pico_socket_bind(server.sck , &anything, &server.port)!=0)
+ {
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return HTTP_RETURN_ERROR;
+ }
+
+ if (pico_socket_listen(server.sck, BACKLOG) != 0)
+ {
+ pico_err = PICO_ERR_EADDRINUSE;
+ return HTTP_RETURN_ERROR;
+ }
+ server.wakeup = wakeup;
+ server.state = HTTP_SERVER_LISTEN;
+ return HTTP_RETURN_OK;
+}
+
+/*
+ * API for accepting new connections. This function should be
+ * called when the event EV_HTTP_CON is triggered, if not called
+ * when noticed the connection will be considered rejected and the
+ * socket will be dropped.
+ *
+ * Returns the ID of the new connection or a negative value if error.
+ */
+int pico_http_server_accept(void)
+{
+ struct pico_ip4 orig;
+ struct httpClient * client;
+ uint16_t port;
+
+ client = pico_zalloc(sizeof(struct httpClient));
+ if(!client)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ return HTTP_RETURN_ERROR;
+ }
+
+ client->sck = pico_socket_accept(server.sck,&orig,&port);
+
+ if(!client->sck)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ pico_free(client);
+ return HTTP_RETURN_ERROR;
+ }
+
+ server.accepted = TRUE;
+ // buffer used for async sending
+ client->state = HTTP_WAIT_HDR;
+ client->buffer = NULL;
+ client->bufferSize = 0;
+ client->connectionID = pico_rand() & 0x7FFF;
+
+ //add element to the tree, if duplicate because the rand
+ //regenerate
+ while(pico_tree_insert(&pico_http_clients,client)!=NULL)
+ client->connectionID = pico_rand() & 0x7FFF;
+
+ return client->connectionID;
+}
+
+/*
+ * Function used for getting the resource asked by the
+ * client. It is useful after the request header (EV_HTTP_REQ)
+ * from client was received, otherwise NULL is returned.
+ */
+char * pico_http_getResource(uint16_t conn)
+{
+ struct httpClient * client = findClient(conn);
+
+ if(!client)
+ return NULL;
+ else
+ return client->resource;
+}
+
+/*
+ * After the resource was asked by the client (EV_HTTP_REQ)
+ * before doing anything else, the server has to let know
+ * the client if the resource can be provided or not.
+ *
+ * This is controlled via the code parameter which can
+ * have two values :
+ *
+ * HTTP_RESOURCE_FOUND or HTTP_RESOURCE_NOT_FOUND
+ *
+ * If a resource is reported not found the 404 header will be sent and the connection
+ * will be closed , otherwise the 200 header is sent and the user should
+ * immediately submit data.
+ *
+ */
+int pico_http_respond(uint16_t conn, uint16_t code)
+{
+ struct httpClient * client = findClient(conn);
+
+ if(!client)
+ {
+ dbg("Client not found !\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ if(client->state == HTTP_WAIT_RESPONSE)
+ {
+ if(code == HTTP_RESOURCE_FOUND)
+ {
+ client->state = HTTP_WAIT_DATA;
+ return pico_socket_write(client->sck,(const char *)returnOkHeader,sizeof(returnOkHeader)-1);//remove \0
+ }
+ else
+ {
+ int length;
+
+ length = pico_socket_write(client->sck,(const char *)returnFailHeader,sizeof(returnFailHeader)-1);//remove \0
+ pico_socket_close(client->sck);
+ client->state = HTTP_CLOSED;
+ return length;
+
+ }
+ }
+ else
+ {
+ dbg("Bad state for the client \n");
+ return HTTP_RETURN_ERROR;
+ }
+
+}
+
+/*
+ * API used to submit data to the client.
+ * Server sends data only using Transfer-Encoding: chunked.
+ *
+ * With this function the user will submit a data chunk to
+ * be sent.
+ * The function will send the chunk size in hex and the rest will
+ * be sent using WR event from sockets.
+ * After each transmision EV_HTTP_PROGRESS is called and at the
+ * end of the chunk EV_HTTP_SENT is called.
+ *
+ * To let the client know this is the last chunk, the user
+ * should pass a NULL buffer.
+ */
+int pico_http_submitData(uint16_t conn, void * buffer, int len)
+{
+
+ struct httpClient * client = findClient(conn);
+ char chunkStr[10];
+ int chunkCount;
+
+ if(client->state != HTTP_WAIT_DATA)
+ {
+ dbg("Client is in a different state than accepted\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ if(client->buffer)
+ {
+ dbg("Already a buffer submited\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ if(!client)
+ {
+ dbg("Wrong connection ID\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ if(!buffer)
+ {
+ len = 0;
+ }
+
+ if(len > 0)
+ {
+ client->buffer = pico_zalloc(len);
+ if(!client->buffer)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ return HTTP_RETURN_ERROR;
+ }
+ // taking over the buffer
+ memcpy(client->buffer,buffer,len);
+ }
+ else
+ client->buffer = NULL;
+
+
+ client->bufferSize = len;
+ client->bufferSent = 0;
+
+ // create the chunk size and send it
+ if(len > 0)
+ {
+ client->state = HTTP_SENDING_DATA;
+ chunkCount = pico_itoaHex(client->bufferSize,chunkStr);
+ chunkStr[chunkCount++] = '\r';
+ chunkStr[chunkCount++] = '\n';
+ pico_socket_write(client->sck,chunkStr,chunkCount);
+ }
+ else if(len == 0)
+ {
+ dbg("->\n");
+ // end of transmision
+ pico_socket_write(client->sck,"0\r\n\r\n",5u);
+ // nothing left, close the client
+ pico_socket_close(client->sck);
+ client->state = HTTP_CLOSED;
+ }
+
+ return HTTP_RETURN_OK;
+}
+
+/*
+ * When EV_HTTP_PROGRESS is triggered you can use this
+ * function to check the state of the chunk.
+ */
+
+int pico_http_getProgress(uint16_t conn, uint16_t * sent, uint16_t *total)
+{
+ struct httpClient * client = findClient(conn);
+
+ if(!client)
+ {
+ dbg("Wrong connection id !\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ *sent = client->bufferSent;
+ *total = client->bufferSize;
+
+ return HTTP_RETURN_OK;
+}
+
+/*
+ * This API can be used to close either a client
+ * or the server ( if you pass HTTP_SERVER_ID as a connection ID).
+ */
+int pico_http_close(uint16_t conn)
+{
+ // close the server
+ if(conn == HTTP_SERVER_ID)
+ {
+ if(server.state == HTTP_SERVER_LISTEN)
+ {
+ struct pico_tree_node * index, * tmp;
+ // close the server
+ pico_socket_close(server.sck);
+ server.sck = NULL;
+
+ // destroy the tree
+ pico_tree_foreach_safe(index,&pico_http_clients,tmp)
+ {
+ struct httpClient * client = index->keyValue;
+
+ if(client->resource)
+ pico_free(client->resource);
+
+ pico_socket_close(client->sck);
+ pico_tree_delete(&pico_http_clients,client);
+ }
+
+ server.state = HTTP_SERVER_CLOSED;
+ return HTTP_RETURN_OK;
+ }
+ else // nothing to close
+ return HTTP_RETURN_ERROR;
+ } // close a connection in this case
+ else
+ {
+
+ struct httpClient * client = findClient(conn);
+
+ if(!client)
+ {
+ dbg("Client not found..\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ pico_tree_delete(&pico_http_clients,client);
+
+ if(client->resource)
+ pico_free(client->resource);
+
+ if(client->buffer)
+ pico_free(client->buffer);
+
+ if(client->state != HTTP_CLOSED || !client->sck)
+ pico_socket_close(client->sck);
+
+ pico_free(client);
+ return HTTP_RETURN_OK;
+ }
+}
+
+// check the integrity of the request
+int parseRequest(struct httpClient * client)
+{
+ char c;
+ //read first line
+ consumeChar(c);
+ if(c == 'G')
+ { // possible GET
+
+ char line[HTTP_HEADER_MAX_LINE];
+ uint32_t index = 0;
+
+ line[index] = c;
+
+ // consume the full line
+ while(consumeChar(c)>0) // read char by char only the first line
+ {
+ line[++index] = c;
+ if(c == '\n')
+ break;
+
+ if(index >= HTTP_HEADER_MAX_LINE)
+ {
+ dbg("Size exceeded \n");
+ return HTTP_RETURN_ERROR;
+ }
+ }
+
+ // extract the function and the resource
+ if(memcmp(line,"GET",3u) || line[3u]!=' ' || index < 10u || line[index] !='\n')
+ {
+ dbg("Wrong command or wrong ending\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ // start reading the resource
+ index = 4u; // go after ' '
+ while(line[index]!=' ')
+ {
+ if(line[index]=='\n') // no terminator ' '
+ {
+ dbg("No terminator...\n");
+ return HTTP_RETURN_ERROR;
+ }
+
+ index++;
+ }
+
+ client->resource = pico_zalloc(index - 3u);// allocate without the GET in front + 1 which is \0
+
+ if(!client)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ return HTTP_RETURN_ERROR;
+ }
+
+ // copy the resource
+ memcpy(client->resource,line+4u,index-4u);// copy without the \0 which was already set by pico_zalloc
+
+ client->state = HTTP_WAIT_EOF_HDR;
+ return HTTP_RETURN_OK;
+
+ }
+
+ return HTTP_RETURN_ERROR;
+}
+
+
+
+int readRemainingHeader(struct httpClient * client)
+{
+ char line[100];
+ int count = 0;
+ int len;
+
+ while( (len = pico_socket_read(client->sck,line,100u)) > 0)
+ {
+ char c;
+ int index = 0;
+ // parse the response
+ while(index < len)
+ {
+ c = line[index++];
+ if(c!='\r' && c!='\n')
+ count++;
+ if(c=='\n')
+ {
+ if(!count)
+ {
+ client->state = HTTP_EOF_HDR;
+ dbg("End of header !\n");
+ break;
+ }
+ count = 0;
+
+ }
+ }
+ }
+
+ return HTTP_RETURN_OK;
+}
+
+void sendData(struct httpClient * client)
+{
+ int length;
+ while( client->bufferSent < client->bufferSize &&
+ (length = pico_socket_write(client->sck,client->buffer+client->bufferSent,client->bufferSize-client->bufferSent)) > 0 )
+ {
+ client->bufferSent += length;
+ server.wakeup(EV_HTTP_PROGRESS,client->connectionID);
+ }
+
+ if(client->bufferSent == client->bufferSize && client->bufferSize)
+ {
+ //send chunk trail
+ if(pico_socket_write(client->sck,"\r\n",2) > 0)
+ {
+ client->state = HTTP_WAIT_DATA;
+ //free the buffer
+ pico_free(client->buffer);
+ client->buffer = NULL;
+ server.wakeup(EV_HTTP_SENT,client->connectionID);
+ }
+ }
+
+}
+
+int readData(struct httpClient * client)
+{
+ if(client->state == HTTP_WAIT_HDR)
+ {
+ if(parseRequest(client)<0 || readRemainingHeader(client)<0)
+ {
+ return HTTP_RETURN_ERROR;
+ }
+ } // continue with this in case the header comes line by line not a big chunk
+ else if(client->state == HTTP_WAIT_EOF_HDR)
+ {
+ if(readRemainingHeader(client)<0 )
+ return HTTP_RETURN_ERROR;
+ }
+
+ if(client->state == HTTP_EOF_HDR)
+ {
+ client->state = HTTP_WAIT_RESPONSE;
+ pico_socket_shutdown(client->sck,PICO_SHUT_RD);
+ server.wakeup(EV_HTTP_REQ,client->connectionID);
+ }
+
+ return HTTP_RETURN_OK;
+}
+
+struct httpClient * findClient(uint16_t conn)
+{
+ struct httpClient dummy = {.connectionID = conn};
+
+ return pico_tree_findKey(&pico_http_clients,&dummy);
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_http_server.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,40 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +Author: Andrei Carp <andrei.carp@tass.be> +*********************************************************************/ + +#ifndef PICO_HTTP_SERVER_H_ +#define PICO_HTTP_SERVER_H_ + +#include <stdint.h> +#include "pico_http_util.h" + +// Response codes +#define HTTP_RESOURCE_FOUND 0 +#define HTTP_RESOURCE_NOT_FOUND 1 + +// Generic id for the server +#define HTTP_SERVER_ID 0 + +/* + * Server functions + */ +int pico_http_server_start(uint16_t port, void (*wakeup)(uint16_t ev, uint16_t conn)); +int pico_http_server_accept(void); + +/* + * Client functions + */ +char * pico_http_getResource(uint16_t conn); +int pico_http_getProgress(uint16_t conn, uint16_t * sent, uint16_t *total); + +/* + * Handshake and data functions + */ +int pico_http_respond(uint16_t conn, uint16_t code); +int pico_http_submitData(uint16_t conn, void * buffer, int len); +int pico_http_close(uint16_t conn); + +#endif /* PICO_HTTP_SERVER_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_util.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,186 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Author: Andrei Carp <andrei.carp@tass.be>
+*********************************************************************/
+
+#include <stdint.h>
+#include "pico_config.h"
+#include "pico_stack.h"
+#include "pico_protocol.h"
+#include "pico_http_util.h"
+
+#define TRUE 1
+#define FALSE 0
+
+#define HTTP_PROTO_TOK "http://"
+#define HTTP_PROTO_LEN 7u
+
+#if defined PICO_SUPPORT_HTTP_CLIENT || defined PICO_SUPPORT_HTTP_SERVER
+
+int pico_itoaHex(uint16_t port, char * ptr)
+{
+ int size = 0;
+ int index;
+
+ // transform to from number to string [ in backwards ]
+ while(port)
+ {
+ ptr[size] = ((port & 0xF) < 10) ? ((port & 0xF) + '0') : ((port & 0xF) - 10 + 'a');
+ port = port>>4u; //divide by 16
+ size++;
+ }
+
+ // invert positions
+ for(index=0 ;index < size>>1u ;index++)
+ {
+ char c = ptr[index];
+ ptr[index] = ptr[size-index-1];
+ ptr[size-index-1] = c;
+ }
+ ptr[size] = '\0';
+ return size;
+}
+
+int pico_itoa(uint16_t port, char * ptr)
+{
+ int size = 0;
+ int index;
+
+ // transform to from number to string [ in backwards ]
+ while(port)
+ {
+ ptr[size] = port%10 + '0';
+ port = port/10;
+ size++;
+ }
+
+ // invert positions
+ for(index=0 ;index < size>>1u ;index++)
+ {
+ char c = ptr[index];
+ ptr[index] = ptr[size-index-1];
+ ptr[size-index-1] = c;
+ }
+ ptr[size] = '\0';
+ return size;
+}
+
+
+int pico_processURI(const char * uri, struct pico_http_uri * urikey)
+{
+
+ uint16_t lastIndex = 0, index;
+
+ if(!uri || !urikey || uri[0] == '/')
+ {
+ pico_err = PICO_ERR_EINVAL;
+ goto error;
+ }
+
+ // detect protocol => search for "://"
+ if(memcmp(uri,HTTP_PROTO_TOK,HTTP_PROTO_LEN) == 0) // could be optimized
+ { // protocol identified, it is http
+ urikey->protoHttp = TRUE;
+ lastIndex = HTTP_PROTO_LEN;
+ }
+ else
+ {
+ if(strstr(uri,"://")) // different protocol specified
+ {
+ urikey->protoHttp = FALSE;
+ goto error;
+ }
+ // no protocol specified, assuming by default it's http
+ urikey->protoHttp = TRUE;
+ }
+
+ // detect hostname
+ index = lastIndex;
+ while(uri[index] && uri[index]!='/' && uri[index]!=':') index++;
+
+ if(index == lastIndex)
+ {
+ // wrong format
+ urikey->host = urikey->resource = NULL;
+ urikey->port = urikey->protoHttp = 0u;
+
+ goto error;
+ }
+ else
+ {
+ // extract host
+ urikey->host = (char *)pico_zalloc(index-lastIndex+1);
+
+ if(!urikey->host)
+ {
+ // no memory
+ goto error;
+ }
+ memcpy(urikey->host,uri+lastIndex,index-lastIndex);
+ }
+
+ if(!uri[index])
+ {
+ // nothing specified
+ urikey->port = 80u;
+ urikey->resource = pico_zalloc(2u);
+ urikey->resource[0] = '/';
+ return HTTP_RETURN_OK;
+ }
+ else if(uri[index] == '/')
+ {
+ urikey->port = 80u;
+ }
+ else if(uri[index] == ':')
+ {
+ urikey->port = 0u;
+ index++;
+ while(uri[index] && uri[index]!='/')
+ {
+ // should check if every component is a digit
+ urikey->port = urikey->port*10 + (uri[index] - '0');
+ index++;
+ }
+ }
+
+ // extract resource
+ if(!uri[index])
+ {
+ urikey->resource = pico_zalloc(2u);
+ urikey->resource[0] = '/';
+ }
+ else
+ {
+ lastIndex = index;
+ while(uri[index] && uri[index]!='?' && uri[index]!='&' && uri[index]!='#') index++;
+ urikey->resource = (char *)pico_zalloc(index-lastIndex+1);
+
+ if(!urikey->resource)
+ {
+ // no memory
+ pico_err = PICO_ERR_ENOMEM;
+ goto error;
+ }
+
+ memcpy(urikey->resource,uri+lastIndex,index-lastIndex);
+ }
+
+ return HTTP_RETURN_OK;
+
+ error :
+ if(urikey->resource)
+ {
+ pico_free(urikey->resource);
+ urikey->resource = NULL;
+ }
+ if(urikey->host)
+ {
+ pico_free(urikey->host);
+ urikey->host = NULL;
+ }
+
+ return HTTP_RETURN_ERROR;
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_util.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,117 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Author: Andrei Carp <andrei.carp@tass.be>
+*********************************************************************/
+
+#ifndef PICO_HTTP_UTIL_H_
+#define PICO_HTTP_UTIL_H_
+
+/* Informational reponses */
+#define HTTP_CONTINUE 100u
+#define HTTP_SWITCHING_PROTOCOLS 101u
+#define HTTP_PROCESSING 102u
+
+/* Success */
+#define HTTP_OK 200u
+#define HTTP_CREATED 201u
+#define HTTP_ACCEPTED 202u
+#define HTTP_NON_AUTH_INFO 203u
+#define HTTP_NO_CONTENT 204u
+#define HTTP_RESET_CONTENT 205u
+#define HTTP_PARTIAL_CONTENT 206u
+#define HTTP_MULTI_STATUS 207u
+#define HTTP_ALREADY_REPORTED 208u
+#define HTTP_LOW_SPACE 250u
+#define HTTP_IM_SPACE 226u
+
+/* Redirection */
+#define HTTP_MULTI_CHOICE 300u
+#define HTTP_MOVED_PERMANENT 301u
+#define HTTP_FOUND 302u
+#define HTTP_SEE_OTHER 303u
+#define HTTP_NOT_MODIFIED 304u
+#define HTTP_USE_PROXY 305u
+#define HTTP_SWITCH_PROXY 306u
+#define HTTP_TEMP_REDIRECT 307u
+#define HTTP_PERM_REDIRECT 308u
+
+/* Client error */
+#define HTTP_BAD_REQUEST 400u
+#define HTTP_UNAUTH 401u
+#define HTTP_PAYMENT_REQ 402u
+#define HTTP_FORBIDDEN 403u
+#define HTTP_NOT_FOUND 404u
+#define HTTP_METH_NOT_ALLOWED 405u
+#define HTTP_NOT_ACCEPTABLE 406u
+#define HTTP_PROXY_AUTH_REQ 407u
+#define HTTP_REQ_TIMEOUT 408u
+#define HTTP_CONFLICT 409u
+#define HTTP_GONE 410u
+#define HTTP_LEN_REQ 411u
+#define HTTP_PRECONDITION_FAIL 412u
+#define HTTP_REQ_ENT_LARGE 413u
+#define HTTP_URI_TOO_LONG 414u
+#define HTTP_UNSUPORTED_MEDIA 415u
+#define HTTP_REQ_RANGE_NOK 416u
+#define HTTP_EXPECT_FAILED 417u
+#define HTTP_TEAPOT 418u
+#define HTTP_UNPROC_ENTITY 422u
+#define HTTP_LOCKED 423u
+#define HTTP_METHOD_FAIL 424u
+#define HTTP_UNORDERED 425u
+#define HTTP_UPGRADE_REQ 426u
+#define HTTP_PRECOND_REQ 428u
+#define HTTP_TOO_MANY_REQ 429u
+#define HTTP_HEDER_FIELD_LARGE 431u
+
+/* Server error */
+#define HTTP_INTERNAL_SERVER_ERR 500u
+#define HTTP_NOT_IMPLEMENTED 501u
+#define HTTP_BAD_GATEWAY 502u
+#define HTTP_SERVICE_UNAVAILABLE 503u
+#define HTTP_GATEWAY_TIMEOUT 504u
+#define HTTP_NOT_SUPPORTED 505u
+#define HTTP_SERV_LOW_STORAGE 507u
+#define HTTP_LOOP_DETECTED 508u
+#define HTTP_NOT_EXTENDED 510u
+#define HTTP_NETWORK_AUTH 511u
+#define HTTP_PERMISSION_DENIED 550u
+
+/* Returns used */
+#define HTTP_RETURN_ERROR -1
+#define HTTP_RETURN_OK 0
+
+/* List of events - shared between client and server */
+#define EV_HTTP_CON 1u
+#define EV_HTTP_REQ 2u
+#define EV_HTTP_PROGRESS 4u
+#define EV_HTTP_SENT 8u
+#define EV_HTTP_CLOSE 16u
+#define EV_HTTP_ERROR 32u
+#define EV_HTTP_BODY 64u
+#define EV_HTTP_DNS 128u
+
+#ifndef TRUE
+ #define TRUE 1
+#endif
+
+#ifndef FALSE
+ #define FALSE 0
+#endif
+
+struct pico_http_uri
+{
+ uint8_t protoHttp; // is the protocol Http ?
+ char * host; // hostname
+ uint16_t port; // port if specified
+ char * resource; // resource , ignoring the other possible parameters
+};
+
+// used for chunks
+int pico_itoaHex(uint16_t port, char * ptr);
+int pico_itoa(uint16_t port, char * ptr);
+int pico_processURI(const char * uri, struct pico_http_uri * urikey);
+
+#endif /* PICO_HTTP_UTIL_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_icmp4.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,314 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_icmp4.h"
+#include "pico_config.h"
+#include "pico_ipv4.h"
+#include "pico_eth.h"
+#include "pico_device.h"
+#include "pico_stack.h"
+#include "pico_tree.h"
+
+/* Queues */
+static struct pico_queue icmp_in = {0};
+static struct pico_queue icmp_out = {0};
+
+
+/* Functions */
+
+static int pico_icmp4_checksum(struct pico_frame *f)
+{
+ struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
+ if (!hdr) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_checksum(hdr, f->transport_len));
+ return 0;
+}
+
+#ifdef PICO_SUPPORT_PING
+static void ping_recv_reply(struct pico_frame *f);
+#endif
+
+static int pico_icmp4_process_in(struct pico_protocol *self, struct pico_frame *f)
+{
+ struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
+ IGNORE_PARAMETER(self);
+
+ if (hdr->type == PICO_ICMP_ECHO) {
+ hdr->type = PICO_ICMP_ECHOREPLY;
+ /* outgoing frames require a f->len without the ethernet header len */
+ if (f->dev->eth)
+ f->len -= PICO_SIZE_ETHHDR;
+ pico_icmp4_checksum(f);
+ pico_ipv4_rebound(f);
+ } else if (hdr->type == PICO_ICMP_UNREACH) {
+ f->net_hdr = f->transport_hdr + PICO_ICMPHDR_UN_SIZE;
+ pico_ipv4_unreachable(f, hdr->code);
+ } else if (hdr->type == PICO_ICMP_ECHOREPLY) {
+#ifdef PICO_SUPPORT_PING
+ ping_recv_reply(f);
+#endif
+ pico_frame_discard(f);
+ } else {
+ pico_frame_discard(f);
+ }
+ return 0;
+}
+
+static int pico_icmp4_process_out(struct pico_protocol *self, struct pico_frame *f)
+{
+ IGNORE_PARAMETER(self);
+ IGNORE_PARAMETER(f);
+ dbg("Called %s\n", __FUNCTION__);
+ return 0;
+}
+
+/* Interface: protocol definition */
+struct pico_protocol pico_proto_icmp4 = {
+ .name = "icmp4",
+ .proto_number = PICO_PROTO_ICMP4,
+ .layer = PICO_LAYER_TRANSPORT,
+ .process_in = pico_icmp4_process_in,
+ .process_out = pico_icmp4_process_out,
+ .q_in = &icmp_in,
+ .q_out = &icmp_out,
+};
+
+static int pico_icmp4_notify(struct pico_frame *f, uint8_t type, uint8_t code)
+{
+ struct pico_frame *reply;
+ struct pico_icmp4_hdr *hdr;
+ struct pico_ipv4_hdr *info;
+ if (f == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ reply = pico_proto_ipv4.alloc(&pico_proto_ipv4, 8 + sizeof(struct pico_ipv4_hdr) + PICO_ICMPHDR_UN_SIZE);
+ info = (struct pico_ipv4_hdr*)(f->net_hdr);
+ hdr = (struct pico_icmp4_hdr *) reply->transport_hdr;
+ hdr->type = type;
+ hdr->code = code;
+ hdr->hun.ih_pmtu.ipm_nmtu = short_be(1500);
+ hdr->hun.ih_pmtu.ipm_void = 0;
+ reply->transport_len = 8 + sizeof(struct pico_ipv4_hdr) + PICO_ICMPHDR_UN_SIZE;
+ reply->payload = reply->transport_hdr + PICO_ICMPHDR_UN_SIZE;
+ memcpy(reply->payload, f->net_hdr, 8 + sizeof(struct pico_ipv4_hdr));
+ pico_icmp4_checksum(reply);
+ pico_ipv4_frame_push(reply, &info->src, PICO_PROTO_ICMP4);
+ return 0;
+}
+
+int pico_icmp4_port_unreachable(struct pico_frame *f)
+{
+ /*Parameter check executed in pico_icmp4_notify*/
+ return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PORT);
+}
+
+int pico_icmp4_proto_unreachable(struct pico_frame *f)
+{
+ /*Parameter check executed in pico_icmp4_notify*/
+ return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_PROTOCOL);
+}
+
+int pico_icmp4_dest_unreachable(struct pico_frame *f)
+{
+ /*Parameter check executed in pico_icmp4_notify*/
+ return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_HOST);
+}
+
+int pico_icmp4_ttl_expired(struct pico_frame *f)
+{
+ /*Parameter check executed in pico_icmp4_notify*/
+ return pico_icmp4_notify(f, PICO_ICMP_TIME_EXCEEDED, PICO_ICMP_TIMXCEED_INTRANS);
+}
+
+int pico_icmp4_packet_filtered(struct pico_frame *f)
+{
+ /*Parameter check executed in pico_icmp4_notify*/
+ /*Packet Filtered: type 3, code 13 (Communication Administratively Prohibited)*/
+ return pico_icmp4_notify(f, PICO_ICMP_UNREACH, PICO_ICMP_UNREACH_FILTER_PROHIB);
+}
+
+/***********************/
+/* Ping implementation */
+/***********************/
+/***********************/
+/***********************/
+/***********************/
+
+
+#ifdef PICO_SUPPORT_PING
+
+
+struct pico_icmp4_ping_cookie
+{
+ struct pico_ip4 dst;
+ uint16_t err;
+ uint16_t id;
+ uint16_t seq;
+ uint16_t size;
+ int count;
+ unsigned long timestamp;
+ int interval;
+ int timeout;
+ void (*cb)(struct pico_icmp4_stats*);
+
+};
+
+static int cookie_compare(void *ka, void *kb)
+{
+ struct pico_icmp4_ping_cookie *a = ka, *b = kb;
+ if (a->id < b->id)
+ return -1;
+ if (a->id > b->id)
+ return 1;
+ return (a->seq - b->seq);
+}
+
+PICO_TREE_DECLARE(Pings,cookie_compare);
+
+static int pico_icmp4_send_echo(struct pico_icmp4_ping_cookie *cookie)
+{
+ struct pico_frame *echo = pico_proto_ipv4.alloc(&pico_proto_ipv4, PICO_ICMPHDR_UN_SIZE + cookie->size);
+ struct pico_icmp4_hdr *hdr;
+
+ hdr = (struct pico_icmp4_hdr *) echo->transport_hdr;
+
+ hdr->type = PICO_ICMP_ECHO;
+ hdr->code = 0;
+ hdr->hun.ih_idseq.idseq_id = short_be(cookie->id);
+ hdr->hun.ih_idseq.idseq_seq = short_be(cookie->seq);
+ echo->transport_len = PICO_ICMPHDR_UN_SIZE + cookie->size;
+ echo->payload = echo->transport_hdr + PICO_ICMPHDR_UN_SIZE;
+ echo->payload_len = cookie->size;
+ /* XXX: Fill payload */
+ pico_icmp4_checksum(echo);
+ pico_ipv4_frame_push(echo, &cookie->dst, PICO_PROTO_ICMP4);
+ return 0;
+}
+
+
+static void ping_timeout(unsigned long now, void *arg)
+{
+ struct pico_icmp4_ping_cookie *cookie = (struct pico_icmp4_ping_cookie *)arg;
+ IGNORE_PARAMETER(now);
+
+ if(pico_tree_findKey(&Pings,cookie)){
+ if (cookie->err == PICO_PING_ERR_PENDING) {
+ struct pico_icmp4_stats stats;
+ stats.dst = cookie->dst;
+ stats.seq = cookie->seq;
+ stats.time = 0;
+ stats.size = cookie->size;
+ stats.err = PICO_PING_ERR_TIMEOUT;
+ dbg(" ---- Ping timeout!!!\n");
+ cookie->cb(&stats);
+ }
+
+ pico_tree_delete(&Pings,cookie);
+ pico_free(cookie);
+ }
+}
+
+static void next_ping(unsigned long now, void *arg);
+static inline void send_ping(struct pico_icmp4_ping_cookie *cookie)
+{
+ pico_icmp4_send_echo(cookie);
+ cookie->timestamp = pico_tick;
+ pico_timer_add(cookie->timeout, ping_timeout, cookie);
+ if (cookie->seq < cookie->count)
+ pico_timer_add(cookie->interval, next_ping, cookie);
+}
+
+static void next_ping(unsigned long now, void *arg)
+{
+ struct pico_icmp4_ping_cookie *newcookie, *cookie = (struct pico_icmp4_ping_cookie *)arg;
+ IGNORE_PARAMETER(now);
+
+ if(pico_tree_findKey(&Pings,cookie)){
+ if (cookie->seq < cookie->count) {
+ newcookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
+ if (!newcookie)
+ return;
+ memcpy(newcookie, cookie, sizeof(struct pico_icmp4_ping_cookie));
+ newcookie->seq++;
+
+ pico_tree_insert(&Pings,newcookie);
+ send_ping(newcookie);
+ }
+ }
+}
+
+
+static void ping_recv_reply(struct pico_frame *f)
+{
+ struct pico_icmp4_ping_cookie test, *cookie;
+ struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
+ test.id = short_be(hdr->hun.ih_idseq.idseq_id );
+ test.seq = short_be(hdr->hun.ih_idseq.idseq_seq);
+
+ cookie = pico_tree_findKey(&Pings, &test);
+ if (cookie) {
+ struct pico_icmp4_stats stats;
+ cookie->err = PICO_PING_ERR_REPLIED;
+ stats.dst = cookie->dst;
+ stats.seq = cookie->seq;
+ stats.size = cookie->size;
+ stats.time = pico_tick - cookie->timestamp;
+ stats.err = cookie->err;
+ stats.ttl = ((struct pico_ipv4_hdr *)f->net_hdr)->ttl;
+ if(cookie->cb != NULL)
+ cookie->cb(&stats);
+ } else {
+ dbg("Reply for seq=%d, not found.\n", test.seq);
+ }
+}
+
+int pico_icmp4_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *))
+{
+ static uint16_t next_id = 0x91c0;
+ struct pico_icmp4_ping_cookie *cookie;
+
+ if((dst == NULL) || (interval == 0) || (timeout == 0) || (count == 0)){
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ cookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
+ if (!cookie) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+
+ if (pico_string_to_ipv4(dst, &cookie->dst.addr) < 0) {
+ pico_err = PICO_ERR_EINVAL;
+ pico_free(cookie);
+ return -1;
+ }
+ cookie->seq = 1;
+ cookie->id = next_id++;
+ cookie->err = PICO_PING_ERR_PENDING;
+ cookie->size = size;
+ cookie->interval = interval;
+ cookie->timeout = timeout;
+ cookie->cb = cb;
+ cookie->count = count;
+
+ pico_tree_insert(&Pings,cookie);
+ send_ping(cookie);
+
+ return 0;
+
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_icmp4.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,149 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_ICMP4
+#define _INCLUDE_PICO_ICMP4
+#include "pico_addressing.h"
+#include "pico_protocol.h"
+
+extern struct pico_protocol pico_proto_icmp4;
+
+struct __attribute__((packed)) pico_icmp4_hdr {
+ uint8_t type;
+ uint8_t code;
+ uint16_t crc;
+
+ /* hun */
+ union {
+ uint8_t ih_pptr;
+ struct pico_ip4 ih_gwaddr;
+ struct {
+ uint16_t idseq_id;
+ uint16_t idseq_seq;
+ } ih_idseq;
+ uint32_t ih_void;
+ struct {
+ uint16_t ipm_void;
+ uint16_t ipm_nmtu;
+ } ih_pmtu;
+ struct {
+ uint8_t rta_numgw;
+ uint8_t rta_wpa;
+ uint16_t rta_lifetime;
+ } ih_rta;
+ } hun;
+
+ /* dun */
+ union {
+ struct {
+ uint32_t ts_otime;
+ uint32_t ts_rtime;
+ uint32_t ts_ttime;
+ } id_ts;
+ struct {
+ uint32_t ip_options;
+ uint32_t ip_data_hi;
+ uint32_t ip_data_lo;
+ } id_ip;
+ struct {
+ uint32_t ira_addr;
+ uint32_t ira_pref;
+ } id_ra;
+ uint32_t id_mask;
+ uint8_t id_data[1];
+ } dun;
+};
+
+#define PICO_ICMPHDR_DRY_SIZE 4
+#define PICO_ICMPHDR_UN_SIZE 8
+
+#define PICO_ICMP_ECHOREPLY 0
+#define PICO_ICMP_DEST_UNREACH 3
+#define PICO_ICMP_SOURCE_QUENCH 4
+#define PICO_ICMP_REDIRECT 5
+#define PICO_ICMP_ECHO 8
+#define PICO_ICMP_TIME_EXCEEDED 11
+#define PICO_ICMP_PARAMETERPROB 12
+#define PICO_ICMP_TIMESTAMP 13
+#define PICO_ICMP_TIMESTAMPREPLY 14
+#define PICO_ICMP_INFO_REQUEST 15
+#define PICO_ICMP_INFO_REPLY 16
+#define PICO_ICMP_ADDRESS 17
+#define PICO_ICMP_ADDRESSREPLY 18
+
+
+#define PICO_ICMP_UNREACH 3
+#define PICO_ICMP_SOURCEQUENCH 4
+#define PICO_ICMP_ROUTERADVERT 9
+#define PICO_ICMP_ROUTERSOLICIT 10
+#define PICO_ICMP_TIMXCEED 11
+#define PICO_ICMP_PARAMPROB 12
+#define PICO_ICMP_TSTAMP 13
+#define PICO_ICMP_TSTAMPREPLY 14
+#define PICO_ICMP_IREQ 15
+#define PICO_ICMP_IREQREPLY 16
+#define PICO_ICMP_MASKREQ 17
+#define PICO_ICMP_MASKREPLY 18
+
+#define PICO_ICMP_MAXTYPE 18
+
+
+#define PICO_ICMP_UNREACH_NET 0
+#define PICO_ICMP_UNREACH_HOST 1
+#define PICO_ICMP_UNREACH_PROTOCOL 2
+#define PICO_ICMP_UNREACH_PORT 3
+#define PICO_ICMP_UNREACH_NEEDFRAG 4
+#define PICO_ICMP_UNREACH_SRCFAIL 5
+#define PICO_ICMP_UNREACH_NET_UNKNOWN 6
+#define PICO_ICMP_UNREACH_HOST_UNKNOWN 7
+#define PICO_ICMP_UNREACH_ISOLATED 8
+#define PICO_ICMP_UNREACH_NET_PROHIB 9
+#define PICO_ICMP_UNREACH_HOST_PROHIB 10
+#define PICO_ICMP_UNREACH_TOSNET 11
+#define PICO_ICMP_UNREACH_TOSHOST 12
+#define PICO_ICMP_UNREACH_FILTER_PROHIB 13
+#define PICO_ICMP_UNREACH_HOST_PRECEDENCE 14
+#define PICO_ICMP_UNREACH_PRECEDENCE_CUTOFF 15
+
+
+#define PICO_ICMP_REDIRECT_NET 0
+#define PICO_ICMP_REDIRECT_HOST 1
+#define PICO_ICMP_REDIRECT_TOSNET 2
+#define PICO_ICMP_REDIRECT_TOSHOST 3
+
+
+#define PICO_ICMP_TIMXCEED_INTRANS 0
+#define PICO_ICMP_TIMXCEED_REASS 1
+
+
+#define PICO_ICMP_PARAMPROB_OPTABSENT 1
+
+#define PICO_SIZE_ICMP4HDR ((sizeof(struct pico_icmp4_hdr)))
+
+struct pico_icmp4_stats
+{
+ struct pico_ip4 dst;
+ unsigned long size;
+ unsigned long seq;
+ unsigned long time;
+ unsigned long ttl;
+ int err;
+};
+
+int pico_icmp4_port_unreachable(struct pico_frame *f);
+int pico_icmp4_proto_unreachable(struct pico_frame *f);
+int pico_icmp4_dest_unreachable(struct pico_frame *f);
+int pico_icmp4_ttl_expired(struct pico_frame *f);
+int pico_icmp4_packet_filtered(struct pico_frame *f);
+
+int pico_icmp4_ping(char *dst, int count, int interval, int timeout, int size, void (*cb)(struct pico_icmp4_stats *));
+#define PICO_PING_ERR_REPLIED 0
+#define PICO_PING_ERR_TIMEOUT 1
+#define PICO_PING_ERR_UNREACH 2
+#define PICO_PING_ERR_PENDING 0xFFFF
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_igmp.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,1124 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+RFC 1112, 2236, 3376, 3569, 3678, 4607
+
+Authors: Kristof Roelants (IGMPv3), Simon Maes, Brecht Van Cauwenberghe
+*********************************************************************/
+
+#include "pico_stack.h"
+#include "pico_ipv4.h"
+#include "pico_igmp.h"
+#include "pico_config.h"
+#include "pico_eth.h"
+#include "pico_addressing.h"
+#include "pico_frame.h"
+#include "pico_tree.h"
+#include "pico_device.h"
+#include "pico_socket.h"
+
+#define igmp_dbg(...) do{}while(0)
+//#define igmp_dbg dbg
+
+/* membership states */
+#define IGMP_STATE_NON_MEMBER (0x0)
+#define IGMP_STATE_DELAYING_MEMBER (0x1)
+#define IGMP_STATE_IDLE_MEMBER (0x2)
+
+/* events */
+#define IGMP_EVENT_DELETE_GROUP (0x0)
+#define IGMP_EVENT_CREATE_GROUP (0x1)
+#define IGMP_EVENT_UPDATE_GROUP (0x2)
+#define IGMP_EVENT_QUERY_RECV (0x3)
+#define IGMP_EVENT_REPORT_RECV (0x4)
+#define IGMP_EVENT_TIMER_EXPIRED (0x5)
+
+/* message types */
+#define IGMP_TYPE_MEM_QUERY (0x11)
+#define IGMP_TYPE_MEM_REPORT_V1 (0x12)
+#define IGMP_TYPE_MEM_REPORT_V2 (0x16)
+#define IGMP_TYPE_LEAVE_GROUP (0x17)
+#define IGMP_TYPE_MEM_REPORT_V3 (0x22)
+
+/* group record types */
+#define IGMP_MODE_IS_INCLUDE (1)
+#define IGMP_MODE_IS_EXCLUDE (2)
+#define IGMP_CHANGE_TO_INCLUDE_MODE (3)
+#define IGMP_CHANGE_TO_EXCLUDE_MODE (4)
+#define IGMP_ALLOW_NEW_SOURCES (5)
+#define IGMP_BLOCK_OLD_SOURCES (6)
+
+/* host flag */
+#define IGMP_HOST_LAST (0x1)
+#define IGMP_HOST_NOT_LAST (0x0)
+
+/* list of timers, counters and their default values */
+#define IGMP_ROBUSTNESS (2)
+#define IGMP_QUERY_INTERVAL (125) /* secs */
+#define IGMP_QUERY_RESPONSE_INTERVAL (10) /* secs */
+#define IGMP_STARTUP_QUERY_INTERVAL (IGMPV3_QUERY_INTERVAL / 4)
+#define IGMP_STARTUP_QUERY_COUNT (IGMPV3_ROBUSTNESS)
+#define IGMP_LAST_MEMBER_QUERY_INTERVAL (1) /* secs */
+#define IGMP_LAST_MEMBER_QUERY_COUNT (IGMPV3_ROBUSTNESS)
+#define IGMP_UNSOLICITED_REPORT_INTERVAL (1) /* secs */
+#define IGMP_DEFAULT_MAX_RESPONSE_TIME (100)
+
+/* custom timers types */
+#define IGMP_TIMER_GROUP_REPORT (1)
+#define IGMP_TIMER_V1_QUERIER (2)
+#define IGMP_TIMER_V2_QUERIER (3)
+
+/* IGMP groups */
+#define IGMP_ALL_HOST_GROUP long_be(0xE0000001) /* 224.0.0.1 */
+#define IGMP_ALL_ROUTER_GROUP long_be(0xE0000002) /* 224.0.0.2 */
+#define IGMPV3_ALL_ROUTER_GROUP long_be(0xE0000016) /* 224.0.0.22 */
+
+/* misc */
+#define IGMP_TIMER_STOPPED (1)
+#define IP_OPTION_ROUTER_ALERT_LEN (4)
+#define IGMP_MAX_GROUPS (32) /* max 255 */
+
+struct __attribute__((packed)) igmp_message {
+ uint8_t type;
+ uint8_t max_resp_time;
+ uint16_t crc;
+ uint32_t mcast_group;
+};
+
+struct __attribute__((packed)) igmpv3_query {
+ uint8_t type;
+ uint8_t max_resp_time;
+ uint16_t crc;
+ uint32_t mcast_group;
+ uint8_t rsq;
+ uint8_t qqic;
+ uint16_t sources;
+ uint32_t source_addr[0];
+};
+
+struct __attribute__((packed)) igmpv3_group_record {
+ uint8_t type;
+ uint8_t aux;
+ uint16_t sources;
+ uint32_t mcast_group;
+ uint32_t source_addr[0];
+};
+
+struct __attribute__((packed)) igmpv3_report {
+ uint8_t type;
+ uint8_t res0;
+ uint16_t crc;
+ uint16_t res1;
+ uint16_t groups;
+ struct igmpv3_group_record record[0];
+};
+
+struct igmp_parameters {
+ uint8_t event;
+ uint8_t state;
+ uint8_t last_host;
+ uint8_t filter_mode;
+ uint8_t max_resp_time;
+ struct pico_ip4 mcast_link;
+ struct pico_ip4 mcast_group;
+ struct pico_tree *MCASTFilter;
+ struct pico_frame *f;
+};
+
+struct igmp_timer {
+ uint8_t type;
+ uint8_t stopped;
+ unsigned long start;
+ unsigned long delay;
+ struct pico_ip4 mcast_link;
+ struct pico_ip4 mcast_group;
+ struct pico_frame *f;
+ void (*callback)(struct igmp_timer *t);
+};
+
+/* queues */
+static struct pico_queue igmp_in = {0};
+static struct pico_queue igmp_out = {0};
+
+/* finite state machine caller */
+static int pico_igmp_process_event(struct igmp_parameters *p);
+
+/* state callback prototype */
+typedef int (*callback)(struct igmp_parameters *);
+
+/* redblack trees */
+static int igmp_timer_cmp(void *ka, void *kb)
+{
+ struct igmp_timer *a = ka, *b =kb;
+ if (a->type < b->type)
+ return -1;
+ if (a->type > b->type)
+ return 1;
+ if (a->mcast_group.addr < b->mcast_group.addr)
+ return -1;
+ if (a->mcast_group.addr > b->mcast_group.addr)
+ return 1;
+ if (a->mcast_link.addr < b->mcast_link.addr)
+ return -1;
+ if (a->mcast_link.addr > b->mcast_link.addr)
+ return 1;
+ return 0;
+}
+PICO_TREE_DECLARE(IGMPTimers, igmp_timer_cmp);
+
+static int igmp_parameters_cmp(void *ka, void *kb)
+{
+ struct igmp_parameters *a = ka, *b = kb;
+ if (a->mcast_group.addr < b->mcast_group.addr)
+ return -1;
+ if (a->mcast_group.addr > b->mcast_group.addr)
+ return 1;
+ if (a->mcast_link.addr < b->mcast_link.addr)
+ return -1;
+ if (a->mcast_link.addr > b->mcast_link.addr)
+ return 1;
+ return 0;
+}
+PICO_TREE_DECLARE(IGMPParameters, igmp_parameters_cmp);
+
+static int igmp_sources_cmp(void *ka, void *kb)
+{
+ struct pico_ip4 *a = ka, *b = kb;
+ if (a->addr < b->addr)
+ return -1;
+ if (a->addr > b->addr)
+ return 1;
+ return 0;
+}
+PICO_TREE_DECLARE(IGMPAllow, igmp_sources_cmp);
+PICO_TREE_DECLARE(IGMPBlock, igmp_sources_cmp);
+
+static struct igmp_parameters *pico_igmp_find_parameter(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group)
+{
+ struct igmp_parameters test = {0};
+ test.mcast_link.addr = mcast_link->addr;
+ test.mcast_group.addr = mcast_group->addr;
+ return pico_tree_findKey(&IGMPParameters, &test);
+}
+
+static int pico_igmp_delete_parameter(struct igmp_parameters *p)
+{
+ if (pico_tree_delete(&IGMPParameters, p))
+ pico_free(p);
+ else
+ return -1;
+
+ return 0;
+}
+
+static void pico_igmp_timer_expired(unsigned long now, void *arg)
+{
+ struct igmp_timer *t = NULL, *timer = NULL, test = {0};
+
+ IGNORE_PARAMETER(now);
+ t = (struct igmp_timer *)arg;
+ test.type = t->type;
+ test.mcast_link = t->mcast_link;
+ test.mcast_group = t->mcast_group;
+ igmp_dbg("IGMP: timer expired for %08X link %08X type %u, delay %lu\n", t->mcast_group.addr, t->mcast_link.addr, t->type, t->delay);
+ timer = pico_tree_findKey(&IGMPTimers, &test);
+ if (!timer) {
+ return;
+ }
+ if (timer->stopped == IGMP_TIMER_STOPPED) {
+ pico_free(t);
+ return;
+ }
+ if (timer->start + timer->delay < PICO_TIME_MS()) {
+ pico_tree_delete(&IGMPTimers, timer);
+ if (timer->callback)
+ timer->callback(timer);
+ pico_free(timer);
+ } else {
+ igmp_dbg("IGMP: restart timer for %08X, delay %lu, new delay %lu\n", t->mcast_group.addr, t->delay, (timer->start + timer->delay) - PICO_TIME_MS());
+ pico_timer_add((timer->start + timer->delay) - PICO_TIME_MS(), &pico_igmp_timer_expired, timer);
+ }
+ return;
+}
+
+static int pico_igmp_timer_reset(struct igmp_timer *t)
+{
+ struct igmp_timer *timer = NULL, test = {0};
+
+ igmp_dbg("IGMP: reset timer for %08X, delay %lu\n", t->mcast_group.addr, t->delay);
+ test.type = t->type;
+ test.mcast_link = t->mcast_link;
+ test.mcast_group = t->mcast_group;
+ timer = pico_tree_findKey(&IGMPTimers, &test);
+ if (!timer)
+ return -1;
+
+ *timer = *t;
+ timer->start = PICO_TIME_MS();
+ return 0;
+}
+
+static int pico_igmp_timer_start(struct igmp_timer *t)
+{
+ struct igmp_timer *timer = NULL, test = {0};
+
+ igmp_dbg("IGMP: start timer for %08X link %08X type %u, delay %lu\n", t->mcast_group.addr, t->mcast_link.addr, t->type, t->delay);
+ test.type = t->type;
+ test.mcast_link = t->mcast_link;
+ test.mcast_group = t->mcast_group;
+ timer = pico_tree_findKey(&IGMPTimers, &test);
+ if (timer)
+ return pico_igmp_timer_reset(t);
+
+ timer = pico_zalloc(sizeof(struct igmp_timer));
+ if (!timer) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ *timer = *t;
+ timer->start = PICO_TIME_MS();
+
+ pico_tree_insert(&IGMPTimers, timer);
+ pico_timer_add(timer->delay, &pico_igmp_timer_expired, timer);
+ return 0;
+}
+
+static int pico_igmp_timer_stop(struct igmp_timer *t)
+{
+ struct igmp_timer *timer = NULL, test = {0};
+
+ test.type = t->type;
+ test.mcast_link = t->mcast_link;
+ test.mcast_group = t->mcast_group;
+ timer = pico_tree_findKey(&IGMPTimers, &test);
+ if (!timer)
+ return 0;
+
+ igmp_dbg("IGMP: stop timer for %08X, delay %lu\n", timer->mcast_group.addr, timer->delay);
+ timer->stopped = IGMP_TIMER_STOPPED;
+ return 0;
+}
+
+static int pico_igmp_timer_is_running(struct igmp_timer *t)
+{
+ struct igmp_timer *timer = NULL, test = {0};
+
+ test.type = t->type;
+ test.mcast_link = t->mcast_link;
+ test.mcast_group = t->mcast_group;
+ timer = pico_tree_findKey(&IGMPTimers, &test);
+ if (timer)
+ return 1;
+ return 0;
+}
+
+static struct igmp_timer *pico_igmp_find_timer(uint8_t type, struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group)
+{
+ struct igmp_timer test = {0};
+
+ test.type = type;
+ test.mcast_link = *mcast_link;
+ test.mcast_group = *mcast_group;
+ return pico_tree_findKey(&IGMPTimers, &test);
+}
+
+static void pico_igmp_report_expired(struct igmp_timer *t)
+{
+ struct igmp_parameters *p = NULL;
+
+ p = pico_igmp_find_parameter(&t->mcast_link, &t->mcast_group);
+ if (!p)
+ return;
+
+ p->event = IGMP_EVENT_TIMER_EXPIRED;
+ pico_igmp_process_event(p);
+}
+
+static void pico_igmp_v2querier_expired(struct igmp_timer *t)
+{
+ struct pico_ipv4_link *link = NULL;
+ struct pico_tree_node *index = NULL, *_tmp = NULL;
+
+ link = pico_ipv4_link_by_dev(t->f->dev);
+ if (!link)
+ return;
+
+ /* When changing compatibility mode, cancel all pending response
+ * and retransmission timers.
+ */
+ pico_tree_foreach_safe(index, &IGMPTimers, _tmp)
+ {
+ ((struct igmp_timer *)index->keyValue)->stopped = IGMP_TIMER_STOPPED;
+ pico_tree_delete(&IGMPTimers, index->keyValue);
+ }
+ igmp_dbg("IGMP: switch to compatibility mode IGMPv3\n");
+ link->mcast_compatibility = PICO_IGMPV3;
+ return;
+}
+
+static int pico_igmp_is_checksum_valid(struct pico_frame *f)
+{
+ struct pico_ipv4_hdr *hdr = NULL;
+ uint8_t ihl = 24, datalen = 0;
+
+ hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ ihl = (hdr->vhl & 0x0F) * 4; /* IHL is in 32bit words */
+ datalen = short_be(hdr->len) - ihl;
+
+ if (short_be(pico_checksum(f->transport_hdr, datalen)) == 0)
+ return 1;
+ igmp_dbg("IGMP: invalid checksum\n");
+ return 0;
+}
+
+/* RFC 3376 $7.1 */
+static int pico_igmp_compatibility_mode(struct pico_frame *f)
+{
+ struct pico_ipv4_hdr *hdr = NULL;
+ struct pico_ipv4_link *link = NULL;
+ struct pico_tree_node *index = NULL, *_tmp = NULL;
+ struct igmp_timer t = {0};
+ uint8_t ihl = 24, datalen = 0;
+
+ link = pico_ipv4_link_by_dev(f->dev);
+ if (!link)
+ return -1;
+
+ hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ ihl = (hdr->vhl & 0x0F) * 4; /* IHL is in 32bit words */
+ datalen = short_be(hdr->len) - ihl;
+ igmp_dbg("IGMP: IHL = %u, LEN = %u, OCTETS = %u\n", ihl, short_be(hdr->len), datalen);
+
+ if (datalen > 12) {
+ /* IGMPv3 query */
+ t.type = IGMP_TIMER_V2_QUERIER;
+ if (pico_igmp_timer_is_running(&t)) { /* IGMPv2 querier present timer still running */
+ return -1;
+ } else {
+ link->mcast_compatibility = PICO_IGMPV3;
+ return 0;
+ }
+ } else if (datalen == 8) {
+ struct igmp_message *query = (struct igmp_message *)f->transport_hdr;
+ if (query->max_resp_time != 0) {
+ /* IGMPv2 query */
+ /* When changing compatibility mode, cancel all pending response
+ * and retransmission timers.
+ */
+ pico_tree_foreach_safe(index, &IGMPTimers, _tmp)
+ {
+ ((struct igmp_timer *)index->keyValue)->stopped = IGMP_TIMER_STOPPED;
+ pico_tree_delete(&IGMPTimers, index->keyValue);
+ }
+ igmp_dbg("IGMP: switch to compatibility mode IGMPv2\n");
+ link->mcast_compatibility = PICO_IGMPV2;
+ t.type = IGMP_TIMER_V2_QUERIER;
+ t.delay = ((IGMP_ROBUSTNESS * link->mcast_last_query_interval) + IGMP_QUERY_RESPONSE_INTERVAL) * 1000;
+ t.f = f;
+ t.callback = pico_igmp_v2querier_expired;
+ /* only one of this type of timer may exist! */
+ pico_igmp_timer_start(&t);
+ } else {
+ /* IGMPv1 query, not supported */
+ return -1;
+ }
+ } else {
+ /* invalid query, silently ignored */
+ return -1;
+ }
+ return 0;
+}
+
+static struct igmp_parameters *pico_igmp_analyse_packet(struct pico_frame *f)
+{
+ struct igmp_message *message = NULL;
+ struct igmp_parameters *p = NULL;
+ struct pico_ipv4_link *link = NULL;
+ struct pico_ip4 mcast_group = {0};
+
+ link = pico_ipv4_link_by_dev(f->dev);
+ if (!link)
+ return NULL;
+
+ /* IGMPv2 and IGMPv3 have a similar structure for the first 8 bytes */
+ message = (struct igmp_message *)f->transport_hdr;
+ mcast_group.addr = message->mcast_group;
+ p = pico_igmp_find_parameter(&link->address, &mcast_group);
+ if (!p && mcast_group.addr == 0) { /* general query */
+ p = pico_zalloc(sizeof(struct igmp_parameters));
+ if (!p)
+ return NULL;
+ p->state = IGMP_STATE_NON_MEMBER;
+ p->mcast_link.addr = link->address.addr;
+ p->mcast_group.addr = mcast_group.addr;
+ pico_tree_insert(&IGMPParameters, p);
+ } else if (!p) {
+ return NULL;
+ }
+
+ switch (message->type) {
+ case IGMP_TYPE_MEM_QUERY:
+ p->event = IGMP_EVENT_QUERY_RECV;
+ break;
+ case IGMP_TYPE_MEM_REPORT_V1:
+ p->event = IGMP_EVENT_REPORT_RECV;
+ break;
+ case IGMP_TYPE_MEM_REPORT_V2:
+ p->event = IGMP_EVENT_REPORT_RECV;
+ break;
+ case IGMP_TYPE_MEM_REPORT_V3:
+ p->event = IGMP_EVENT_REPORT_RECV;
+ break;
+ default:
+ return NULL;
+ }
+ p->max_resp_time = message->max_resp_time; /* if IGMPv3 report this will be 0 (res0 field) */
+ p->f = f;
+
+ return p;
+}
+
+static int pico_igmp_process_in(struct pico_protocol *self, struct pico_frame *f)
+{
+ struct igmp_parameters *p = NULL;
+ IGNORE_PARAMETER(self);
+
+ if (!pico_igmp_is_checksum_valid(f))
+ goto out;
+ if (pico_igmp_compatibility_mode(f) < 0)
+ goto out;
+ p = pico_igmp_analyse_packet(f);
+ if (!p)
+ goto out;
+
+ return pico_igmp_process_event(p);
+
+ out:
+ pico_frame_discard(f);
+ return 0;
+}
+
+static int pico_igmp_process_out(struct pico_protocol *self, struct pico_frame *f) {
+ /* packets are directly transferred to the IP layer by calling pico_ipv4_frame_push */
+ IGNORE_PARAMETER(self);
+ IGNORE_PARAMETER(f);
+ return 0;
+}
+
+/* Interface: protocol definition */
+struct pico_protocol pico_proto_igmp = {
+ .name = "igmp",
+ .proto_number = PICO_PROTO_IGMP,
+ .layer = PICO_LAYER_TRANSPORT,
+ .process_in = pico_igmp_process_in,
+ .process_out = pico_igmp_process_out,
+ .q_in = &igmp_in,
+ .q_out = &igmp_out,
+};
+
+int pico_igmp_state_change(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t filter_mode, struct pico_tree *MCASTFilter, uint8_t state)
+{
+ struct igmp_parameters *p = NULL;
+
+ if (mcast_group->addr == IGMP_ALL_HOST_GROUP)
+ return 0;
+
+ p = pico_igmp_find_parameter(mcast_link, mcast_group);
+ if (!p && state == PICO_IGMP_STATE_CREATE) {
+ p = pico_zalloc(sizeof(struct igmp_parameters));
+ if (!p) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ p->state = IGMP_STATE_NON_MEMBER;
+ p->mcast_link = *mcast_link;
+ p->mcast_group = *mcast_group;
+ pico_tree_insert(&IGMPParameters, p);
+ } else if (!p) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ switch (state) {
+ case PICO_IGMP_STATE_CREATE:
+ p->event = IGMP_EVENT_CREATE_GROUP;
+ break;
+
+ case PICO_IGMP_STATE_UPDATE:
+ p->event = IGMP_EVENT_UPDATE_GROUP;
+ break;
+
+ case PICO_IGMP_STATE_DELETE:
+ p->event = IGMP_EVENT_DELETE_GROUP;
+ break;
+
+ default:
+ return -1;
+ }
+ p->filter_mode = filter_mode;
+ p->MCASTFilter = MCASTFilter;
+
+ return pico_igmp_process_event(p);
+}
+
+static int pico_igmp_send_report(struct igmp_parameters *p, struct pico_frame *f)
+{
+ struct pico_ip4 dst = {0};
+ struct pico_ip4 mcast_group = {0};
+ struct pico_ipv4_link *link = NULL;
+
+ link = pico_ipv4_link_get(&p->mcast_link);
+ if (!link)
+ return -1;
+
+ mcast_group.addr = p->mcast_group.addr;
+ switch (link->mcast_compatibility) {
+ case PICO_IGMPV2:
+ if (p->event == IGMP_EVENT_DELETE_GROUP)
+ dst.addr = IGMP_ALL_ROUTER_GROUP;
+ else
+ dst.addr = mcast_group.addr;
+ break;
+
+ case PICO_IGMPV3:
+ dst.addr = IGMPV3_ALL_ROUTER_GROUP;
+ break;
+
+ default:
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+ }
+
+ igmp_dbg("IGMP: send membership report on group %08X to %08X\n", mcast_group.addr, dst.addr);
+ pico_ipv4_frame_push(f, &dst, PICO_PROTO_IGMP);
+ return 0;
+}
+
+static int pico_igmp_generate_report(struct igmp_parameters *p)
+{
+ struct pico_ipv4_link *link = NULL;
+ int i = 0;
+
+ link = pico_ipv4_link_get(&p->mcast_link);
+ if (!link) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ switch (link->mcast_compatibility) {
+ case PICO_IGMPV1:
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+
+ case PICO_IGMPV2:
+ {
+ struct igmp_message *report = NULL;
+ uint8_t report_type = IGMP_TYPE_MEM_REPORT_V2;
+ if (p->event == IGMP_EVENT_DELETE_GROUP)
+ report_type = IGMP_TYPE_LEAVE_GROUP;
+
+ p->f = pico_proto_ipv4.alloc(&pico_proto_ipv4, IP_OPTION_ROUTER_ALERT_LEN + sizeof(struct igmp_message));
+ p->f->net_len += IP_OPTION_ROUTER_ALERT_LEN;
+ p->f->transport_hdr += IP_OPTION_ROUTER_ALERT_LEN;
+ p->f->transport_len -= IP_OPTION_ROUTER_ALERT_LEN;
+ p->f->dev = pico_ipv4_link_find(&p->mcast_link);
+ /* p->f->len is correctly set by alloc */
+
+ report = (struct igmp_message *)p->f->transport_hdr;
+ report->type = report_type;
+ report->max_resp_time = IGMP_DEFAULT_MAX_RESPONSE_TIME;
+ report->mcast_group = p->mcast_group.addr;
+
+ report->crc = 0;
+ report->crc = short_be(pico_checksum(report, sizeof(struct igmp_message)));
+ break;
+ }
+ case PICO_IGMPV3:
+ {
+ struct igmpv3_report *report = NULL;
+ struct igmpv3_group_record *record = NULL;
+ struct pico_mcast_group *g = NULL, test = {0};
+ struct pico_tree_node *index = NULL, *_tmp = NULL;
+ struct pico_tree *IGMPFilter = NULL;
+ struct pico_ip4 *source = NULL;
+ uint8_t record_type = 0;
+ uint8_t sources = 0;
+ int len = 0;
+
+ test.mcast_addr = p->mcast_group;
+ g = pico_tree_findKey(link->MCASTGroups, &test);
+ if (!g) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if (p->event == IGMP_EVENT_DELETE_GROUP) { /* "non-existent" state of filter mode INCLUDE and empty source list */
+ p->filter_mode = PICO_IP_MULTICAST_INCLUDE;
+ p->MCASTFilter = NULL;
+ }
+
+ /* cleanup filters */
+ pico_tree_foreach_safe(index, &IGMPAllow, _tmp)
+ {
+ pico_tree_delete(&IGMPAllow, index->keyValue);
+ }
+ pico_tree_foreach_safe(index, &IGMPBlock, _tmp)
+ {
+ pico_tree_delete(&IGMPBlock, index->keyValue);
+ }
+
+ switch (g->filter_mode) {
+
+ case PICO_IP_MULTICAST_INCLUDE:
+ switch (p->filter_mode) {
+ case PICO_IP_MULTICAST_INCLUDE:
+ if (p->event == IGMP_EVENT_DELETE_GROUP) { /* all ADD_SOURCE_MEMBERSHIP had an equivalent DROP_SOURCE_MEMBERSHIP */
+ /* TO_IN (B) */
+ record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
+ IGMPFilter = &IGMPAllow;
+ if (p->MCASTFilter) {
+ pico_tree_foreach(index, p->MCASTFilter) /* B */
+ {
+ pico_tree_insert(&IGMPAllow, index->keyValue);
+ sources++;
+ }
+ } /* else { IGMPAllow stays empty } */
+ break;
+ }
+
+ /* ALLOW (B-A) */
+ /* if event is CREATE A will be empty, thus only ALLOW (B-A) has sense */
+ if (p->event == IGMP_EVENT_CREATE_GROUP) /* first ADD_SOURCE_MEMBERSHIP */
+ record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
+ else
+ record_type = IGMP_ALLOW_NEW_SOURCES;
+ IGMPFilter = &IGMPAllow;
+ pico_tree_foreach(index, p->MCASTFilter) /* B */
+ {
+ pico_tree_insert(&IGMPAllow, index->keyValue);
+ sources++;
+ }
+ pico_tree_foreach(index, &g->MCASTSources) /* A */
+ {
+ source = pico_tree_findKey(&IGMPAllow, index->keyValue);
+ if (source) {
+ pico_tree_delete(&IGMPAllow, source);
+ sources--;
+ }
+ }
+ if (!pico_tree_empty(&IGMPAllow)) /* record type is ALLOW */
+ break;
+
+ /* BLOCK (A-B) */
+ record_type = IGMP_BLOCK_OLD_SOURCES;
+ IGMPFilter = &IGMPBlock;
+ pico_tree_foreach(index, &g->MCASTSources) /* A */
+ {
+ pico_tree_insert(&IGMPBlock, index->keyValue);
+ sources++;
+ }
+ pico_tree_foreach(index, p->MCASTFilter) /* B */
+ {
+ source = pico_tree_findKey(&IGMPBlock, index->keyValue);
+ if (source) {
+ pico_tree_delete(&IGMPBlock, source);
+ sources--;
+ }
+ }
+ if (!pico_tree_empty(&IGMPBlock)) /* record type is BLOCK */
+ break;
+
+ /* ALLOW (B-A) and BLOCK (A-B) are empty: do not send report (RFC 3376 $5.1) */
+ p->f = NULL;
+ return 0;
+
+ case PICO_IP_MULTICAST_EXCLUDE:
+ /* TO_EX (B) */
+ record_type = IGMP_CHANGE_TO_EXCLUDE_MODE;
+ IGMPFilter = &IGMPBlock;
+ pico_tree_foreach(index, p->MCASTFilter) /* B */
+ {
+ pico_tree_insert(&IGMPBlock, index->keyValue);
+ sources++;
+ }
+ break;
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ break;
+
+ case PICO_IP_MULTICAST_EXCLUDE:
+ switch (p->filter_mode) {
+ case PICO_IP_MULTICAST_INCLUDE:
+ /* TO_IN (B) */
+ record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
+ IGMPFilter = &IGMPAllow;
+ if (p->MCASTFilter) {
+ pico_tree_foreach(index, p->MCASTFilter) /* B */
+ {
+ pico_tree_insert(&IGMPAllow, index->keyValue);
+ sources++;
+ }
+ } /* else { IGMPAllow stays empty } */
+ break;
+
+ case PICO_IP_MULTICAST_EXCLUDE:
+ /* BLOCK (B-A) */
+ record_type = IGMP_BLOCK_OLD_SOURCES;
+ IGMPFilter = &IGMPBlock;
+ pico_tree_foreach(index, p->MCASTFilter)
+ {
+ pico_tree_insert(&IGMPBlock, index->keyValue);
+ sources++;
+ }
+ pico_tree_foreach(index, &g->MCASTSources) /* A */
+ {
+ source = pico_tree_findKey(&IGMPBlock, index->keyValue); /* B */
+ if (source) {
+ pico_tree_delete(&IGMPBlock, source);
+ sources--;
+ }
+ }
+ if (!pico_tree_empty(&IGMPBlock)) /* record type is BLOCK */
+ break;
+
+ /* ALLOW (A-B) */
+ record_type = IGMP_ALLOW_NEW_SOURCES;
+ IGMPFilter = &IGMPAllow;
+ pico_tree_foreach(index, &g->MCASTSources)
+ {
+ pico_tree_insert(&IGMPAllow, index->keyValue);
+ sources++;
+ }
+ pico_tree_foreach(index, p->MCASTFilter) /* B */
+ {
+ source = pico_tree_findKey(&IGMPAllow, index->keyValue); /* A */
+ if (source) {
+ pico_tree_delete(&IGMPAllow, source);
+ sources--;
+ }
+ }
+ if (!pico_tree_empty(&IGMPAllow)) /* record type is ALLOW */
+ break;
+
+ /* BLOCK (B-A) and ALLOW (A-B) are empty: do not send report (RFC 3376 $5.1) */
+ p->f = NULL;
+ return 0;
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ break;
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ len = sizeof(struct igmpv3_report) + sizeof(struct igmpv3_group_record) + (sources * sizeof(struct pico_ip4));
+ p->f = pico_proto_ipv4.alloc(&pico_proto_ipv4, IP_OPTION_ROUTER_ALERT_LEN + len);
+ p->f->net_len += IP_OPTION_ROUTER_ALERT_LEN;
+ p->f->transport_hdr += IP_OPTION_ROUTER_ALERT_LEN;
+ p->f->transport_len -= IP_OPTION_ROUTER_ALERT_LEN;
+ p->f->dev = pico_ipv4_link_find(&p->mcast_link);
+ /* p->f->len is correctly set by alloc */
+
+ report = (struct igmpv3_report *)p->f->transport_hdr;
+ report->type = IGMP_TYPE_MEM_REPORT_V3;
+ report->res0 = 0;
+ report->crc = 0;
+ report->res1 = 0;
+ report->groups = short_be(1);
+
+ record = &report->record[0];
+ record->type = record_type;
+ record->aux = 0;
+ record->sources = short_be(sources);
+ record->mcast_group = p->mcast_group.addr;
+ if (!pico_tree_empty(IGMPFilter)) {
+ i = 0;
+ pico_tree_foreach(index, IGMPFilter)
+ {
+ record->source_addr[i] = ((struct pico_ip4 *)index->keyValue)->addr;
+ i++;
+ }
+ }
+ report->crc = short_be(pico_checksum(report, len));
+ break;
+ }
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ return 0;
+}
+
+/* stop timer, send leave if flag set */
+static int stslifs(struct igmp_parameters *p)
+{
+ struct igmp_timer t = {0};
+
+ igmp_dbg("IGMP: event = leave group | action = stop timer, send leave if flag set\n");
+
+ t.type = IGMP_TIMER_GROUP_REPORT;
+ t.mcast_link = p->mcast_link;
+ t.mcast_group = p->mcast_group;
+ if (pico_igmp_timer_stop(&t) < 0)
+ return -1;
+
+ /* always send leave, even if not last host */
+ if (pico_igmp_send_report(p, p->f) < 0)
+ return -1;
+
+ pico_igmp_delete_parameter(p);
+ igmp_dbg("IGMP: new state = non-member\n");
+ return 0;
+}
+
+/* send report, set flag, start timer */
+static int srsfst(struct igmp_parameters *p)
+{
+ struct igmp_timer t = {0};
+ struct pico_frame *copy_frame = NULL;
+
+ igmp_dbg("IGMP: event = join group | action = send report, set flag, start timer\n");
+
+ p->last_host = IGMP_HOST_LAST;
+
+ if (pico_igmp_generate_report(p) < 0)
+ return -1;
+ if (!p->f)
+ return 0;
+ copy_frame = pico_frame_copy(p->f);
+ if (!copy_frame) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ if (pico_igmp_send_report(p, copy_frame) < 0)
+ return -1;
+
+ t.type = IGMP_TIMER_GROUP_REPORT;
+ t.mcast_link = p->mcast_link;
+ t.mcast_group = p->mcast_group;
+ t.delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000));
+ t.f = p->f;
+ t.callback = pico_igmp_report_expired;
+ pico_igmp_timer_start(&t);
+
+ p->state = IGMP_STATE_DELAYING_MEMBER;
+ igmp_dbg("IGMP: new state = delaying member\n");
+ return 0;
+}
+
+/* merge report, send report, reset timer (IGMPv3 only) */
+static int mrsrrt(struct igmp_parameters *p)
+{
+ struct igmp_timer *t = NULL;
+ struct pico_frame *copy_frame = NULL;
+ struct pico_ipv4_link *link = NULL;
+
+ igmp_dbg("IGMP: event = update group | action = merge report, send report, reset timer (IGMPv3 only)\n");
+
+ link = pico_ipv4_link_get(&p->mcast_link);
+ if (!link)
+ return -1;
+
+ if (link->mcast_compatibility != PICO_IGMPV3) {
+ igmp_dbg("IGMP: no IGMPv3 compatible router on network\n");
+ return -1;
+ }
+
+ /* XXX: merge with pending report rfc 3376 $5.1 */
+
+ copy_frame = pico_frame_copy(p->f);
+ if (!copy_frame)
+ return -1;
+ if (pico_igmp_send_report(p, copy_frame) < 0)
+ return -1;
+
+ t = pico_igmp_find_timer(IGMP_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group);
+ if (!t)
+ return -1;
+ t->delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000));
+ pico_igmp_timer_reset(t);
+
+ p->state = IGMP_STATE_DELAYING_MEMBER;
+ igmp_dbg("IGMP: new state = delaying member\n");
+ return 0;
+}
+
+/* send report, start timer (IGMPv3 only) */
+static int srst(struct igmp_parameters *p)
+{
+ struct igmp_timer t = {0};
+ struct pico_frame *copy_frame = NULL;
+ struct pico_ipv4_link *link = NULL;
+
+ igmp_dbg("IGMP: event = update group | action = send report, start timer (IGMPv3 only)\n");
+
+ link = pico_ipv4_link_get(&p->mcast_link);
+ if (!link)
+ return -1;
+
+ if (link->mcast_compatibility != PICO_IGMPV3) {
+ igmp_dbg("IGMP: no IGMPv3 compatible router on network\n");
+ return -1;
+ }
+
+ if (pico_igmp_generate_report(p) < 0)
+ return -1;
+ if (!p->f)
+ return 0;
+ copy_frame = pico_frame_copy(p->f);
+ if (!copy_frame)
+ return -1;
+ if (pico_igmp_send_report(p, copy_frame) < 0)
+ return -1;
+
+ t.type = IGMP_TIMER_GROUP_REPORT;
+ t.mcast_link = p->mcast_link;
+ t.mcast_group = p->mcast_group;
+ t.delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000));
+ t.f = p->f;
+ t.callback = pico_igmp_report_expired;
+ pico_igmp_timer_start(&t);
+
+ p->state = IGMP_STATE_DELAYING_MEMBER;
+ igmp_dbg("IGMP: new state = delaying member\n");
+ return 0;
+}
+
+/* send leave if flag set */
+static int slifs(struct igmp_parameters *p)
+{
+ igmp_dbg("IGMP: event = leave group | action = send leave if flag set\n");
+
+ /* always send leave, even if not last host */
+ if (pico_igmp_send_report(p, p->f) < 0)
+ return -1;
+
+ pico_igmp_delete_parameter(p);
+ igmp_dbg("IGMP: new state = non-member\n");
+ return 0;
+}
+
+/* start timer */
+static int st(struct igmp_parameters *p)
+{
+ struct igmp_timer t = {0};
+
+ igmp_dbg("IGMP: event = query received | action = start timer\n");
+
+ if (pico_igmp_generate_report(p) < 0)
+ return -1;
+ if (!p->f)
+ return -1;
+
+ t.type = IGMP_TIMER_GROUP_REPORT;
+ t.mcast_link = p->mcast_link;
+ t.mcast_group = p->mcast_group;
+ t.delay = (pico_rand() % (p->max_resp_time * 100));
+ t.f = p->f;
+ t.callback = pico_igmp_report_expired;
+ pico_igmp_timer_start(&t);
+
+ p->state = IGMP_STATE_DELAYING_MEMBER;
+ igmp_dbg("IGMP: new state = delaying member\n");
+ return 0;
+}
+
+/* stop timer, clear flag */
+static int stcl(struct igmp_parameters *p)
+{
+ struct igmp_timer t = {0};
+
+ igmp_dbg("IGMP: event = report received | action = stop timer, clear flag\n");
+
+ t.type = IGMP_TIMER_GROUP_REPORT;
+ t.mcast_link = p->mcast_link;
+ t.mcast_group = p->mcast_group;
+ if (pico_igmp_timer_stop(&t) < 0)
+ return -1;
+
+ p->last_host = IGMP_HOST_NOT_LAST;
+ p->state = IGMP_STATE_IDLE_MEMBER;
+ igmp_dbg("IGMP: new state = idle member\n");
+ return 0;
+}
+
+/* send report, set flag */
+static int srsf(struct igmp_parameters *p)
+{
+ igmp_dbg("IGMP: event = timer expired | action = send report, set flag\n");
+
+ if (pico_igmp_send_report(p, p->f) < 0)
+ return -1;
+
+ p->state = IGMP_STATE_IDLE_MEMBER;
+ igmp_dbg("IGMP: new state = idle member\n");
+ return 0;
+}
+
+/* reset timer if max response time < current timer */
+static int rtimrtct(struct igmp_parameters *p)
+{
+ struct igmp_timer *t = NULL;
+ unsigned long time_to_run = 0;
+
+ igmp_dbg("IGMP: event = query received | action = reset timer if max response time < current timer\n");
+
+ t = pico_igmp_find_timer(IGMP_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group);
+ if (!t)
+ return -1;
+
+ time_to_run = t->start + t->delay - PICO_TIME_MS();
+ if ((p->max_resp_time * 100) < time_to_run) { /* max_resp_time in units of 1/10 seconds */
+ t->delay = pico_rand() % (p->max_resp_time * 100);
+ pico_igmp_timer_reset(t);
+ }
+
+ p->state = IGMP_STATE_DELAYING_MEMBER;
+ igmp_dbg("IGMP: new state = delaying member\n");
+ return 0;
+}
+
+static int discard(struct igmp_parameters *p){
+ igmp_dbg("IGMP: ignore and discard frame\n");
+ pico_frame_discard(p->f);
+ return 0;
+}
+
+/* finite state machine table */
+const callback host_membership_diagram_table[3][6] =
+{ /* event |Delete Group |Create Group |Update Group |Query Received |Report Received |Timer Expired */
+/* state Non-Member */ { discard, srsfst, srsfst, discard, discard, discard },
+/* state Delaying Member */ { stslifs, mrsrrt, mrsrrt, rtimrtct, stcl, srsf },
+/* state Idle Member */ { slifs, srst, srst, st, discard, discard }
+};
+
+static int pico_igmp_process_event(struct igmp_parameters *p)
+{
+ struct pico_tree_node *index = NULL;
+ struct igmp_parameters *_p = NULL;
+
+ igmp_dbg("IGMP: process event on group address %08X\n", p->mcast_group.addr);
+ if (p->event == IGMP_EVENT_QUERY_RECV && p->mcast_group.addr == 0) { /* general query */
+ pico_tree_foreach(index, &IGMPParameters) {
+ _p = index->keyValue;
+ _p->max_resp_time = p->max_resp_time;
+ _p->event = IGMP_EVENT_QUERY_RECV;
+ igmp_dbg("IGMP: for each mcast_group = %08X | state = %u\n", _p->mcast_group.addr, _p->state);
+ host_membership_diagram_table[_p->state][_p->event](_p);
+ }
+ } else {
+ igmp_dbg("IGMP: state = %u (0: non-member - 1: delaying member - 2: idle member)\n", p->state);
+ host_membership_diagram_table[p->state][p->event](p);
+ }
+ return 0;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_igmp.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,26 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +. + +Authors: Kristof Roelants, Simon Maes, Brecht Van Cauwenberghe +*********************************************************************/ + +#ifndef _INCLUDE_PICO_IGMP +#define _INCLUDE_PICO_IGMP + +#define PICO_IGMPV1 1 +#define PICO_IGMPV2 2 +#define PICO_IGMPV3 3 + +#define PICO_IGMP_STATE_CREATE 1 +#define PICO_IGMP_STATE_UPDATE 2 +#define PICO_IGMP_STATE_DELETE 3 + +#define PICO_IGMP_QUERY_INTERVAL 125 + +extern struct pico_protocol pico_proto_igmp; + +int pico_igmp_state_change(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t filter_mode, struct pico_tree *MCASTFilter, uint8_t state); +#endif /* _INCLUDE_PICO_IGMP */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipfilter.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,274 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Simon Maes
+*********************************************************************/
+
+#include "pico_ipv4.h"
+#include "pico_config.h"
+#include "pico_icmp4.h"
+#include "pico_stack.h"
+#include "pico_eth.h"
+#include "pico_socket.h"
+#include "pico_device.h"
+#include "pico_ipfilter.h"
+#include "pico_tcp.h"
+#include "pico_udp.h"
+
+
+//#define ipf_dbg dbg
+#define ipf_dbg(...) do{}while(0)
+
+struct filter_node;
+typedef int (*func_pntr)(struct filter_node *filter, struct pico_frame *f);
+
+struct filter_node {
+ struct pico_device *fdev;
+ struct filter_node *next_filter;
+ uint32_t out_addr;
+ uint32_t out_addr_netmask;
+ uint32_t in_addr;
+ uint32_t in_addr_netmask;
+ uint16_t out_port;
+ uint16_t in_port;
+ uint8_t proto;
+ int8_t priority;
+ uint8_t tos;
+ uint8_t filter_id;
+ func_pntr function_ptr;
+};
+
+static struct filter_node *head = NULL;
+static struct filter_node *tail = NULL;
+
+/*======================== FUNCTION PNTRS ==========================*/
+
+static int fp_accept(struct filter_node *filter, struct pico_frame *f) {
+ IGNORE_PARAMETER(filter);
+ IGNORE_PARAMETER(f);
+ return 0;
+}
+
+static int fp_priority(struct filter_node *filter, struct pico_frame *f) {
+
+ //TODO do priority-stuff
+ IGNORE_PARAMETER(filter);
+ IGNORE_PARAMETER(f);
+ return 0;
+}
+
+static int fp_reject(struct filter_node *filter, struct pico_frame *f) {
+// TODO check first if sender is pico itself or not
+ IGNORE_PARAMETER(filter);
+ ipf_dbg("ipfilter> #reject\n");
+ pico_icmp4_packet_filtered(f);
+ pico_frame_discard(f);
+ return 1;
+}
+
+static int fp_drop(struct filter_node *filter, struct pico_frame *f) {
+ IGNORE_PARAMETER(filter);
+ ipf_dbg("ipfilter> # drop\n");
+ pico_frame_discard(f);
+ return 1;
+}
+
+/*============================ API CALLS ============================*/
+int pico_ipv4_filter_add(struct pico_device *dev, uint8_t proto, struct pico_ip4 *out_addr, struct pico_ip4 *out_addr_netmask, struct pico_ip4 *in_addr, struct pico_ip4 *in_addr_netmask, uint16_t out_port, uint16_t in_port, int8_t priority, uint8_t tos, enum filter_action action)
+{
+ static uint8_t filter_id = 0;
+ struct filter_node *new_filter;
+
+ if ( !(dev != NULL || proto != 0 || (out_addr != NULL && out_addr->addr != 0U) || (out_addr_netmask != NULL && out_addr_netmask->addr != 0U)|| (in_addr != NULL && in_addr->addr != 0U) || (in_addr_netmask != NULL && in_addr_netmask->addr != 0U)|| out_port != 0 || in_port !=0 || tos != 0 )) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if ( priority > 10 || priority < -10) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (action > 3) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ ipf_dbg("ipfilter> # adding filter\n");
+
+ new_filter = pico_zalloc(sizeof(struct filter_node));
+ if (!head) {
+ head = tail = new_filter;
+ } else {
+ tail->next_filter = new_filter;
+ tail = new_filter;
+ }
+
+ new_filter->fdev = dev;
+ new_filter->proto = proto;
+ if (out_addr != NULL)
+ new_filter->out_addr = out_addr->addr;
+ else
+ new_filter->out_addr = 0U;
+
+ if (out_addr_netmask != NULL)
+ new_filter->out_addr_netmask = out_addr_netmask->addr;
+ else
+ new_filter->out_addr_netmask = 0U;
+
+ if (in_addr != NULL)
+ new_filter->in_addr = in_addr->addr;
+ else
+ new_filter->in_addr = 0U;
+
+ if (in_addr_netmask != NULL)
+ new_filter->in_addr_netmask = in_addr_netmask->addr;
+ else
+ new_filter->in_addr_netmask = 0U;
+
+ new_filter->out_port = out_port;
+ new_filter->in_port = in_port;
+ new_filter->priority = priority;
+ new_filter->tos = tos;
+ new_filter->filter_id = filter_id++;
+
+ /*Define filterType_functionPointer here instead of in ipfilter-function, to prevent running multiple times through switch*/
+ switch (action) {
+ case FILTER_ACCEPT:
+ new_filter->function_ptr = fp_accept;
+ break;
+ case FILTER_PRIORITY:
+ new_filter->function_ptr = fp_priority;
+ break;
+ case FILTER_REJECT:
+ new_filter->function_ptr = fp_reject;
+ break;
+ case FILTER_DROP:
+ new_filter->function_ptr = fp_drop;
+ break;
+ default:
+ ipf_dbg("ipfilter> #unknown filter action\n");
+ break;
+ }
+ return new_filter->filter_id;
+}
+
+int pico_ipv4_filter_del(uint8_t filter_id)
+{
+ struct filter_node *work;
+ struct filter_node *prev;
+
+ if (!tail || !head) {
+ pico_err = PICO_ERR_EPERM;
+ return -1;
+ }
+
+ work = head;
+ if (work->filter_id == filter_id) {
+ /*delete filter_node from linked list*/
+ head = work->next_filter;
+ pico_free(work);
+ return 0;
+ }
+ prev = work;
+ work = work->next_filter;
+
+ while (1) {
+ if (work->filter_id == filter_id) {
+ if (work != tail) {
+ /*delete filter_node from linked list*/
+ prev->next_filter = work->next_filter;
+ pico_free(work);
+ return 0;
+ } else {
+ prev->next_filter = NULL;
+ pico_free(work);
+ return 0;
+ }
+ } else {
+ /*check next filter_node*/
+ prev = work;
+ work = work->next_filter;
+ if (work == tail) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+ }
+}
+
+/*================================== CORE FILTER FUNCTIONS ==================================*/
+int match_filter(struct filter_node *filter, struct pico_frame *f)
+{
+ struct filter_node temp;
+ struct pico_ipv4_hdr *ipv4_hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ struct pico_tcp_hdr *tcp_hdr;
+ struct pico_udp_hdr *udp_hdr;
+
+ if (!filter|| !f) {
+ ipf_dbg("ipfilter> ## nullpointer in match filter \n");
+ return -1;
+ }
+
+ temp.fdev = f->dev;
+ temp.out_addr = ipv4_hdr->dst.addr;
+ temp.in_addr = ipv4_hdr->src.addr;
+ if (ipv4_hdr->proto == PICO_PROTO_TCP ) {
+ tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ temp.out_port = short_be(tcp_hdr->trans.dport);
+ temp.in_port = short_be(tcp_hdr->trans.sport);
+ }else if (ipv4_hdr->proto == PICO_PROTO_UDP ) {
+ udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ temp.out_port = short_be(udp_hdr->trans.dport);
+ temp.in_port = short_be(udp_hdr->trans.sport);
+ } else {
+ temp.out_port = temp.in_port = 0;
+ }
+ temp.proto = ipv4_hdr->proto;
+ temp.priority = f->priority;
+ temp.tos = ipv4_hdr->tos;
+
+
+
+ if ( ((filter->fdev == NULL || filter->fdev == temp.fdev) && \
+ (filter->in_addr == 0 || ((filter->in_addr_netmask == 0) ? (filter->in_addr == temp.in_addr) : 1)) &&\
+ (filter->in_port == 0 || filter->in_port == temp.in_port) &&\
+ (filter->out_addr == 0 || ((filter->out_addr_netmask == 0) ? (filter->out_addr == temp.out_addr) : 1)) && \
+ (filter->out_port == 0 || filter->out_port == temp.out_port) && \
+ (filter->proto == 0 || filter->proto == temp.proto ) &&\
+ (filter->priority == 0 || filter->priority == temp.priority ) &&\
+ (filter->tos == 0 || filter->tos == temp.tos ) &&\
+ (filter->out_addr_netmask == 0 || ((filter->out_addr & filter->out_addr_netmask) == (temp.out_addr & filter->out_addr_netmask)) ) &&\
+ (filter->in_addr_netmask == 0 || ((filter->in_addr & filter->in_addr_netmask) == (temp.in_addr & filter->in_addr_netmask)) )\
+ ) )
+ return 0;
+
+ //No filter match!
+ ipf_dbg("ipfilter> #no match\n");
+ return 1;
+}
+
+int ipfilter(struct pico_frame *f)
+{
+ struct filter_node *work = head;
+
+ /*return 1 if pico_frame is discarded as result of the filtering, 0 for an incomming packet, -1 for faults*/
+ if (!tail || !head) {
+ return 0;
+ }
+
+ if ( match_filter(work, f) == 0 ) {
+ ipf_dbg("ipfilter> # ipfilter match\n");
+ /*filter match, execute filter!*/
+ return work->function_ptr(work, f);
+ }
+ while (tail != work) {
+ ipf_dbg("ipfilter> next filter..\n");
+ work = work->next_filter;
+ if ( match_filter(work, f) == 0 ) {
+ ipf_dbg("ipfilter> # ipfilter match\n");
+ /*filter match, execute filter!*/
+ return work->function_ptr(work, f);
+ }
+ }
+ return 0;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipfilter.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,31 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Simon Maes
+*********************************************************************/
+#ifndef _INCLUDE_PICO_IPFILTER
+#define _INCLUDE_PICO_IPFILTER
+
+#include "pico_device.h"
+
+enum filter_action {
+ FILTER_ACCEPT = 0,
+ FILTER_PRIORITY,
+ FILTER_REJECT,
+ FILTER_DROP,
+};
+
+
+
+int pico_ipv4_filter_add(struct pico_device *dev, uint8_t proto,
+ struct pico_ip4 *out_addr, struct pico_ip4 *out_addr_netmask, struct pico_ip4 *in_addr,
+ struct pico_ip4 *in_addr_netmask, uint16_t out_port, uint16_t in_port,
+ int8_t priority, uint8_t tos, enum filter_action action);
+
+int pico_ipv4_filter_del(uint8_t filter_id);
+
+int ipfilter(struct pico_frame *f);
+
+#endif /* _INCLUDE_PICO_IPFILTER */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipv4.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,1423 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Daniele Lacamera, Markian Yskout
+*********************************************************************/
+
+
+#include "pico_config.h"
+#include "pico_ipfilter.h"
+#include "pico_ipv4.h"
+#include "pico_icmp4.h"
+#include "pico_stack.h"
+#include "pico_eth.h"
+#include "pico_udp.h"
+#include "pico_tcp.h"
+#include "pico_socket.h"
+#include "pico_device.h"
+#include "pico_nat.h"
+#include "pico_igmp.h"
+#include "pico_tree.h"
+
+#ifdef PICO_SUPPORT_IPV4
+
+#ifdef PICO_SUPPORT_MCAST
+# define ip_mcast_dbg(...) do{}while(0) /* so_mcast_dbg in pico_socket.c */
+# define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */
+/* Default network interface for multicast transmission */
+static struct pico_ipv4_link *mcast_default_link = NULL;
+#endif
+#ifdef PICO_SUPPORT_IPFRAG
+# define reassembly_dbg(...) do{}while(0)
+#endif
+
+/* Queues */
+static struct pico_queue in = {0};
+static struct pico_queue out = {0};
+
+/* Functions */
+static int ipv4_route_compare(void *ka, void * kb);
+
+int pico_ipv4_to_string(char *ipbuf, const uint32_t ip)
+{
+ const unsigned char *addr = (const unsigned char *) &ip;
+ int i;
+
+ if (!ipbuf) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ for(i = 0; i < 4; i++)
+ {
+ if(addr[i] > 99){
+ *ipbuf++ = '0' + (addr[i] / 100);
+ *ipbuf++ = '0' + ((addr[i] % 100) / 10);
+ *ipbuf++ = '0' + ((addr[i] % 100) % 10);
+ }else if(addr[i] > 9){
+ *ipbuf++ = '0' + (addr[i] / 10);
+ *ipbuf++ = '0' + (addr[i] % 10);
+ }else{
+ *ipbuf++ = '0' + addr[i];
+ }
+ if(i < 3)
+ *ipbuf++ = '.';
+ }
+ *ipbuf = '\0';
+
+ return 0;
+}
+
+int pico_string_to_ipv4(const char *ipstr, uint32_t *ip)
+{
+ unsigned char buf[4] = {0};
+ int cnt = 0;
+ int p;
+
+ if(!ipstr || !ip) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ while((p = *ipstr++) != 0)
+ {
+ if(pico_is_digit(p)){
+ buf[cnt] = (10 * buf[cnt]) + (p - '0');
+ }else if(p == '.'){
+ cnt++;
+ }else{
+ return -1;
+ }
+ }
+
+ /* Handle short notation */
+ if(cnt == 1){
+ buf[3] = buf[1];
+ buf[1] = 0;
+ buf[2] = 0;
+ }else if (cnt == 2){
+ buf[3] = buf[2];
+ buf[2] = 0;
+ }else if(cnt != 3){
+ /* String could not be parsed, return error */
+ return -1;
+ }
+
+ *ip = long_from(buf);
+
+ return 0;
+
+}
+
+int pico_ipv4_valid_netmask(uint32_t mask)
+{
+ int cnt = 0;
+ int end = 0;
+ int i;
+ uint32_t mask_swap = long_be(mask);
+
+ /*
+ * Swap bytes for convenient parsing
+ * e.g. 0x..f8ff will become 0xfff8..
+ * Then, we count the consecutive bits
+ *
+ * */
+
+ for(i = 0; i < 32; i++){
+ if((mask_swap << i) & 0x80000000){
+ if(end) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ cnt++;
+ }else{
+ end = 1;
+ }
+ }
+ return cnt;
+}
+
+int pico_ipv4_is_unicast(uint32_t address)
+{
+ const unsigned char *addr = (unsigned char *) &address;
+ if((addr[0] & 0xe0) == 0xe0)
+ return 0; /* multicast */
+
+ return 1;
+}
+
+int pico_ipv4_is_multicast(uint32_t address)
+{
+ const unsigned char *addr = (unsigned char *) &address;
+ if((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0))
+ return 1; /* multicast */
+
+ return 0;
+}
+
+static int pico_ipv4_checksum(struct pico_frame *f)
+{
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ if (!hdr)
+ return -1;
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_checksum(hdr, f->net_len));
+ return 0;
+}
+
+#ifdef PICO_SUPPORT_IPFRAG
+struct pico_ipv4_fragmented_packet {
+ uint16_t id;
+ uint8_t proto;
+ struct pico_ip4 src;
+ struct pico_ip4 dst;
+ uint16_t total_len;
+ struct pico_tree *t;
+};
+
+static int pico_ipv4_fragmented_packet_cmp(void *ka, void *kb)
+{
+ struct pico_ipv4_fragmented_packet *a = ka, *b = kb;
+
+ if (a->id < b->id)
+ return -1;
+ else if (a->id > b->id)
+ return 1;
+ else {
+ if (a->proto < b->proto)
+ return -1;
+ else if (a->proto > b->proto)
+ return 1;
+ else {
+ if (a->src.addr < b->src.addr)
+ return -1;
+ else if (a->src.addr > b->src.addr)
+ return 1;
+ else {
+ if (a->dst.addr < b->dst.addr)
+ return -1;
+ else if (a->dst.addr > b->dst.addr)
+ return 1;
+ else
+ return 0;
+ }
+ }
+ }
+}
+
+static int pico_ipv4_fragmented_element_cmp(void *ka, void *kb)
+{
+ struct pico_frame *frame_a = ka, *frame_b = kb;
+ struct pico_ipv4_hdr *a, *b;
+ a = (struct pico_ipv4_hdr *) frame_a->net_hdr;
+ b = (struct pico_ipv4_hdr *) frame_b->net_hdr;
+
+ if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) < short_be((b->frag & PICO_IPV4_FRAG_MASK)))
+ return -1;
+ else if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) > short_be((b->frag & PICO_IPV4_FRAG_MASK)))
+ return 1;
+ else
+ return 0;
+}
+
+PICO_TREE_DECLARE(pico_ipv4_fragmented_tree, pico_ipv4_fragmented_packet_cmp);
+
+static inline void pico_ipv4_fragmented_cleanup(struct pico_ipv4_fragmented_packet *pfrag)
+{
+ struct pico_tree_node *index = NULL, *_tmp = NULL;
+ struct pico_frame *f_frag = NULL;
+
+ pico_tree_foreach_safe(index, pfrag->t, _tmp) {
+ f_frag = index->keyValue;
+ reassembly_dbg("REASSEMBLY: remove packet with offset %u\n", short_be(((struct pico_ipv4_hdr *)f_frag->net_hdr)->frag) & PICO_IPV4_FRAG_MASK);
+ pico_tree_delete(pfrag->t, f_frag);
+ pico_frame_discard(f_frag);
+ }
+ pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
+ pico_free(pfrag->t);
+ pico_free(pfrag);
+}
+#endif /* PICO_SUPPORT_IPFRAG */
+
+#ifdef PICO_SUPPORT_IPFRAG
+static inline int pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f)
+{
+ uint8_t *running_pointer = NULL;
+ uint16_t running_offset = 0;
+ uint16_t offset = 0;
+ uint16_t data_len = 0;
+ struct pico_ipv4_hdr *f_frag_hdr = NULL, *hdr = (struct pico_ipv4_hdr *) (*f)->net_hdr;
+ struct pico_udp_hdr *udp_hdr = NULL;
+ struct pico_tcp_hdr *tcp_hdr = NULL;
+ struct pico_ipv4_fragmented_packet *pfrag = NULL, frag;
+ struct pico_frame *f_new = NULL, *f_frag = NULL;
+ struct pico_tree_node *index, *_tmp;
+
+ data_len = short_be(hdr->len) - (*f)->net_len;
+ offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK;
+ if (short_be(hdr->frag) & PICO_IPV4_MOREFRAG) {
+ if (!offset) {
+ reassembly_dbg("REASSEMBLY: first element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
+ if (!pico_tree_empty(&pico_ipv4_fragmented_tree)) {
+ reassembly_dbg("REASSEMBLY: cleanup tree\n");
+ // only one entry allowed in this tree
+ pfrag = pico_tree_first(&pico_ipv4_fragmented_tree);
+ pico_ipv4_fragmented_cleanup(pfrag);
+ }
+ // add entry in tree for this ID and create secondary tree to contain fragmented elements
+ pfrag = pico_zalloc(sizeof(struct pico_ipv4_fragmented_packet));
+ if (!pfrag) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ pfrag->id = short_be(hdr->id);
+ pfrag->proto = hdr->proto;
+ pfrag->src.addr = long_be(hdr->src.addr);
+ pfrag->dst.addr = long_be(hdr->dst.addr);
+ pfrag->total_len = short_be(hdr->len) - (*f)->net_len;
+ pfrag->t = pico_zalloc(sizeof(struct pico_tree));
+ if (!pfrag->t) {
+ pico_free(pfrag);
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ pfrag->t->root = &LEAF;
+ pfrag->t->compare = pico_ipv4_fragmented_element_cmp;
+
+ pico_tree_insert(pfrag->t, *f);
+ pico_tree_insert(&pico_ipv4_fragmented_tree, pfrag);
+ return 0;
+ }
+ else {
+ reassembly_dbg("REASSEMBLY: intermediate element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
+ frag.id = short_be(hdr->id);
+ frag.proto = hdr->proto;
+ frag.src.addr = long_be(hdr->src.addr);
+ frag.dst.addr = long_be(hdr->dst.addr);
+ pfrag = pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
+ if (pfrag) {
+ pfrag->total_len += (short_be(hdr->len) - (*f)->net_len);
+ pico_tree_insert(pfrag->t, *f);
+ return 0;
+ } else {
+ reassembly_dbg("REASSEMBLY: silently discard intermediate frame, first packet was lost or disallowed (one fragmented packet at a time)\n");
+ pico_frame_discard(*f);
+ return 0;
+ }
+ }
+ } else if (offset) {
+ reassembly_dbg("REASSEMBLY: last element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
+ frag.id = short_be(hdr->id);
+ frag.proto = hdr->proto;
+ frag.src.addr = long_be(hdr->src.addr);
+ frag.dst.addr = long_be(hdr->dst.addr);
+ pfrag = pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
+ if (pfrag) {
+ pfrag->total_len += (short_be(hdr->len) - (*f)->net_len);
+ reassembly_dbg("REASSEMBLY: fragmented packet in tree, reassemble packet of %u data bytes\n", pfrag->total_len);
+ f_new = self->alloc(self, pfrag->total_len);
+
+ f_frag = pico_tree_first(pfrag->t);
+ reassembly_dbg("REASSEMBLY: copy IP header information len = %lu\n", f_frag->net_len);
+ f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr;
+ data_len = short_be(f_frag_hdr->len) - f_frag->net_len;
+ memcpy(f_new->net_hdr, f_frag->net_hdr, f_frag->net_len);
+ memcpy(f_new->transport_hdr, f_frag->transport_hdr, data_len);
+ running_pointer = f_new->transport_hdr + data_len;
+ offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK;
+ running_offset = data_len / 8;
+ pico_tree_delete(pfrag->t, f_frag);
+ pico_frame_discard(f_frag);
+ reassembly_dbg("REASSEMBLY: reassembled first packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset);
+
+ pico_tree_foreach_safe(index, pfrag->t, _tmp)
+ {
+ f_frag = index->keyValue;
+ f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr;
+ data_len = short_be(f_frag_hdr->len) - f_frag->net_len;
+ memcpy(running_pointer, f_frag->transport_hdr, data_len);
+ running_pointer += data_len;
+ offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK;
+ if (offset != running_offset) {
+ reassembly_dbg("REASSEMBLY: error reassembling intermediate packet: offset %u != expected offset %u (missing fragment)\n", offset, running_offset);
+ pico_ipv4_fragmented_cleanup(pfrag);
+ return -1;
+ }
+ running_offset += (data_len / 8);
+ pico_tree_delete(pfrag->t, f_frag);
+ pico_frame_discard(f_frag);
+ reassembly_dbg("REASSEMBLY: reassembled intermediate packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset);
+ }
+ pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
+ pico_free(pfrag);
+
+ data_len = short_be(hdr->len) - (*f)->net_len;
+ memcpy(running_pointer, (*f)->transport_hdr, data_len);
+ offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK;
+ pico_frame_discard(*f);
+ reassembly_dbg("REASSEMBLY: reassembled last packet of %u data bytes, offset = %u\n", data_len, offset);
+
+ hdr = (struct pico_ipv4_hdr *)f_new->net_hdr;
+ hdr->len = pfrag->total_len;
+ hdr->frag = 0; /* flags cleared and no offset */
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_checksum(hdr, f_new->net_len));
+ /* Optional, the UDP/TCP CRC should already be correct */
+ if (0) {
+ #ifdef PICO_SUPPORT_TCP
+ } else if (hdr->proto == PICO_PROTO_TCP) {
+ tcp_hdr = (struct pico_tcp_hdr *) f_new->transport_hdr;
+ tcp_hdr->crc = 0;
+ tcp_hdr->crc = short_be(pico_tcp_checksum_ipv4(f_new));
+ #endif
+ #ifdef PICO_SUPPORT_UDP
+ } else if (hdr->proto == PICO_PROTO_UDP){
+ udp_hdr = (struct pico_udp_hdr *) f_new->transport_hdr;
+ udp_hdr->crc = 0;
+ udp_hdr->crc = short_be(pico_udp_checksum_ipv4(f_new));
+ #endif
+ }
+ reassembly_dbg("REASSEMBLY: packet with id %X reassembled correctly\n", short_be(hdr->id));
+ *f = f_new;
+ return 1;
+ } else {
+ reassembly_dbg("REASSEMBLY: silently discard last frame, first packet was lost or disallowed (one fragmented packet at a time)\n");
+ pico_frame_discard(*f);
+ return 0;
+ }
+ } else {
+ return 1;
+ }
+}
+#else
+static inline int pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f)
+{
+ return 1;
+}
+#endif /* PICO_SUPPORT_IPFRAG */
+
+#ifdef PICO_SUPPORT_CRC
+static inline int pico_ipv4_crc_check(struct pico_frame *f)
+{
+ uint16_t checksum_invalid = 1;
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+
+ checksum_invalid = short_be(pico_checksum(hdr, f->net_len));
+ if (checksum_invalid) {
+ dbg("IP: checksum failed!\n");
+ pico_frame_discard(f);
+ return 0;
+ }
+ return 1;
+}
+#else
+static inline int pico_ipv4_crc_check(struct pico_frame *f)
+{
+ IGNORE_PARAMETER(f);
+ return 1;
+}
+#endif /* PICO_SUPPORT_CRC */
+
+static int pico_ipv4_forward(struct pico_frame *f);
+#ifdef PICO_SUPPORT_MCAST
+static int pico_ipv4_mcast_filter(struct pico_frame *f);
+#endif
+
+static int ipv4_link_compare(void *ka, void *kb)
+{
+ struct pico_ipv4_link *a = ka, *b =kb;
+ if (a->address.addr < b->address.addr)
+ return -1;
+ if (a->address.addr > b->address.addr)
+ return 1;
+
+ //zero can be assigned multiple times (e.g. for DHCP)
+ if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY){
+ if (a->dev < b->dev)
+ return -1;
+ if (a->dev > b->dev)
+ return 1;
+ }
+ return 0;
+}
+
+PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare);
+
+static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f)
+{
+ uint8_t option_len = 0;
+ int ret = 0;
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ struct pico_ipv4_link test = {.address = {.addr = PICO_IP4_ANY}, .dev = NULL};
+
+ /* NAT needs transport header information */
+ if(((hdr->vhl) & 0x0F )> 5){
+ option_len = 4*(((hdr->vhl) & 0x0F)-5);
+ }
+ f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len;
+ f->transport_len = short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len;
+ f->net_len = PICO_SIZE_IP4HDR + option_len;
+
+#ifdef PICO_SUPPORT_IPFILTER
+ if (ipfilter(f)) {
+ /*pico_frame is discarded as result of the filtering*/
+ return 0;
+ }
+#endif
+
+ /* ret == 1 indicates to continue the function */
+ ret = pico_ipv4_crc_check(f);
+ if (ret < 1)
+ return ret;
+ ret = pico_ipv4_fragmented_check(self, &f);
+ if (ret < 1)
+ return ret;
+
+#ifdef PICO_SUPPORT_MCAST
+ /* Multicast address in source, discard quietly */
+ if (pico_ipv4_is_multicast(hdr->src.addr)) {
+ ip_mcast_dbg("MCAST: ERROR multicast address %08X in source address\n", hdr->src.addr);
+ pico_frame_discard(f);
+ return 0;
+ }
+#endif
+ if (hdr->frag & 0x80) {
+ pico_frame_discard(f); //RFC 3514
+ return 0;
+ }
+ if (0) {
+#ifdef PICO_SUPPORT_UDP
+ } else if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) {
+ /* Receiving UDP broadcast datagram */
+ f->flags |= PICO_FRAME_FLAG_BCAST;
+ pico_enqueue(pico_proto_udp.q_in, f);
+#endif
+ } else if (pico_ipv4_is_multicast(hdr->dst.addr)) {
+#ifdef PICO_SUPPORT_MCAST
+ /* Receiving UDP multicast datagram TODO set f->flags? */
+ if (hdr->proto == PICO_PROTO_IGMP) {
+ ip_mcast_dbg("MCAST: received IGMP message\n");
+ pico_transport_receive(f, PICO_PROTO_IGMP);
+ } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) {
+ pico_enqueue(pico_proto_udp.q_in, f);
+ } else {
+ pico_frame_discard(f);
+ }
+#endif
+ } else if (pico_ipv4_link_find(&hdr->dst)) {
+ if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0)
+ pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */
+ else
+ pico_transport_receive(f, hdr->proto);
+ } else if (pico_tree_findKey(&Tree_dev_link, &test)){
+#ifdef PICO_SUPPORT_UDP
+ //address of this device is apparently 0.0.0.0; might be a DHCP packet
+ /* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages.
+ * incomming DHCP messages are to be broadcasted. Our current DHCP server
+ * implementation does not take this flag into account yet though ... */
+ pico_enqueue(pico_proto_udp.q_in, f);
+#endif
+ } else {
+
+ if((pico_ipv4_is_broadcast(hdr->dst.addr)))
+ {
+ /* don't forward broadcast frame, discard! */
+ pico_frame_discard(f);
+ } else if (pico_ipv4_forward(f) != 0) {
+ /* Packet is not local. Try to forward. */
+ pico_ipv4_forward(f);
+ }
+ }
+ return 0;
+}
+
+PICO_TREE_DECLARE(Routes, ipv4_route_compare);
+
+
+static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f)
+{
+ IGNORE_PARAMETER(self);
+ f->start = (uint8_t*) f->net_hdr;
+ #ifdef PICO_SUPPORT_IPFILTER
+ if (ipfilter(f)) {
+ /*pico_frame is discarded as result of the filtering*/
+ return 0;
+ }
+ #endif
+ return pico_sendto_dev(f);
+}
+
+
+static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, int size)
+{
+ struct pico_frame *f = pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR);
+ IGNORE_PARAMETER(self);
+
+ if (!f)
+ return NULL;
+ f->datalink_hdr = f->buffer;
+ f->net_hdr = f->buffer + PICO_SIZE_ETHHDR;
+ f->net_len = PICO_SIZE_IP4HDR;
+ f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR;
+ f->transport_len = size;
+ f->len = size + PICO_SIZE_IP4HDR;
+ return f;
+}
+
+static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f);
+
+/* Interface: protocol definition */
+struct pico_protocol pico_proto_ipv4 = {
+ .name = "ipv4",
+ .proto_number = PICO_PROTO_IPV4,
+ .layer = PICO_LAYER_NETWORK,
+ .alloc = pico_ipv4_alloc,
+ .process_in = pico_ipv4_process_in,
+ .process_out = pico_ipv4_process_out,
+ .push = pico_ipv4_frame_sock_push,
+ .q_in = &in,
+ .q_out = &out,
+};
+
+struct pico_ipv4_route
+{
+ struct pico_ip4 dest;
+ struct pico_ip4 netmask;
+ struct pico_ip4 gateway;
+ struct pico_ipv4_link *link;
+ uint32_t metric;
+};
+
+
+static int ipv4_route_compare(void *ka, void * kb)
+{
+ struct pico_ipv4_route *a = ka, *b = kb;
+
+ /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
+ if (long_be(a->netmask.addr) < long_be(b->netmask.addr))
+ return -1;
+
+ if (long_be(a->netmask.addr) > long_be(b->netmask.addr))
+ return 1;
+
+ if (a->dest.addr < b->dest.addr)
+ return -1;
+
+ if (a->dest.addr > b->dest.addr)
+ return 1;
+
+ if (a->metric < b->metric)
+ return -1;
+
+ if (a->metric > b->metric)
+ return 1;
+
+ return 0;
+}
+
+static struct pico_ipv4_route *route_find(const struct pico_ip4 *addr)
+{
+ struct pico_ipv4_route *r;
+ struct pico_tree_node * index;
+
+ if(addr->addr != PICO_IP4_BCAST)
+ {
+ pico_tree_foreach_reverse(index, &Routes) {
+ r = index->keyValue;
+ if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) {
+ return r;
+ }
+ }
+ }
+ else
+ {
+ r = pico_tree_first(&Routes);
+ if(!r->netmask.addr)
+ {
+ return r;
+ }
+ else
+ {
+ dbg("WARNING: no default route for a global broadcast found\n");
+ }
+ }
+
+ return NULL;
+}
+
+struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
+{
+ struct pico_ip4 nullip;
+ struct pico_ipv4_route *route;
+ nullip.addr = 0U;
+
+ if(!addr) {
+ pico_err = PICO_ERR_EINVAL;
+ return nullip;
+ }
+
+ route = route_find(addr);
+ if (!route) {
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ return nullip;
+ }
+ else
+ return route->gateway;
+}
+
+struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst)
+{
+ struct pico_ip4 *myself = NULL;
+ struct pico_ipv4_route *rt;
+
+ if(!dst) {
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+ }
+
+ rt = route_find(dst);
+ if (rt) {
+ myself = &rt->link->address;
+ } else
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ return myself;
+}
+
+
+#ifdef PICO_SUPPORT_MCAST
+/* link
+ * |
+ * MCASTGroups
+ * | | |
+ * ------------ | ------------
+ * | | |
+ * MCASTSources MCASTSources MCASTSources
+ * | | | | | | | | | | | |
+ * S S S S S S S S S S S S
+ *
+ * MCASTGroups: RBTree(mcast_group)
+ * MCASTSources: RBTree(source)
+ */
+static int ipv4_mcast_groups_cmp(void * ka, void * kb)
+{
+ struct pico_mcast_group *a = ka, *b = kb;
+ if (a->mcast_addr.addr < b->mcast_addr.addr) {
+ return -1;
+ } else if (a->mcast_addr.addr > b->mcast_addr.addr) {
+ return 1;
+ } else {
+ return 0;
+ }
+}
+
+static int ipv4_mcast_sources_cmp(void *ka, void *kb)
+{
+ struct pico_ip4 *a = ka, *b = kb;
+ if (a->addr < b->addr)
+ return -1;
+ if (a->addr > b->addr)
+ return 1;
+ return 0;
+}
+
+static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
+{
+ uint16_t i = 0;
+ struct pico_mcast_group __attribute__ ((unused)) *g = NULL;
+ struct pico_ip4 __attribute__ ((unused)) *source = NULL;
+ struct pico_tree_node *index = NULL, *index2 = NULL;
+
+ ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+ ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name);
+ ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
+ ip_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n");
+ ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
+
+ pico_tree_foreach(index, mcast_link->MCASTGroups)
+ {
+ g = index->keyValue;
+ ip_mcast_dbg("+ %04d | %16s | %08X | %05u | %u | %8s +\n", i, mcast_link->dev->name, g->mcast_addr.addr, g->reference_count, g->filter_mode, "");
+ pico_tree_foreach(index2, &g->MCASTSources)
+ {
+ source = index2->keyValue;
+ ip_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %08X +\n", "", "", "", "", "", source->addr);
+ }
+ i++;
+ }
+ ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+}
+
+int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
+{
+ struct pico_mcast_group *g = NULL, test = {0};
+ struct pico_ipv4_link *link = NULL;
+ struct pico_tree_node *index = NULL, *_tmp = NULL;
+ struct pico_ip4 *source = NULL;
+
+ if (mcast_link)
+ link = pico_ipv4_link_get(mcast_link);
+ else
+ link = mcast_default_link;
+
+ test.mcast_addr = *mcast_group;
+ g = pico_tree_findKey(link->MCASTGroups, &test);
+ if (g) {
+ if (reference_count)
+ g->reference_count++;
+ pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
+ } else {
+ g = pico_zalloc(sizeof(struct pico_mcast_group));
+ if (!g) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ /* "non-existent" state of filter mode INCLUDE and empty source list */
+ g->filter_mode = PICO_IP_MULTICAST_INCLUDE;
+ g->reference_count = 1;
+ g->mcast_addr = *mcast_group;
+ g->MCASTSources.root = &LEAF;
+ g->MCASTSources.compare = ipv4_mcast_sources_cmp;
+ pico_tree_insert(link->MCASTGroups, g);
+ pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
+ }
+
+ /* cleanup filter */
+ pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
+ {
+ source = index->keyValue;
+ pico_tree_delete(&g->MCASTSources, source);
+ pico_free(source);
+ }
+ /* insert new filter */
+ if (MCASTFilter) {
+ pico_tree_foreach(index, MCASTFilter)
+ {
+ source = pico_zalloc(sizeof(struct pico_ip4));
+ if (!source) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
+ pico_tree_insert(&g->MCASTSources, source);
+ }
+ }
+ g->filter_mode = filter_mode;
+
+ pico_ipv4_mcast_print_groups(link);
+ return 0;
+}
+
+int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
+{
+
+ struct pico_mcast_group *g = NULL, test = {0};
+ struct pico_ipv4_link *link = NULL;
+ struct pico_tree_node *index = NULL, *_tmp = NULL;
+ struct pico_ip4 *source = NULL;
+
+ if (mcast_link)
+ link = pico_ipv4_link_get(mcast_link);
+ else
+ link = mcast_default_link;
+
+ test.mcast_addr = *mcast_group;
+ g = pico_tree_findKey(link->MCASTGroups, &test);
+ if (!g) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ if (reference_count && (--(g->reference_count) < 1)) {
+ pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE);
+ /* cleanup filter */
+ pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
+ {
+ source = index->keyValue;
+ pico_tree_delete(&g->MCASTSources, source);
+ pico_free(source);
+ }
+ pico_tree_delete(link->MCASTGroups, g);
+ pico_free(g);
+ } else {
+ pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
+ /* cleanup filter */
+ pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
+ {
+ source = index->keyValue;
+ pico_tree_delete(&g->MCASTSources, source);
+ pico_free(source);
+ }
+ /* insert new filter */
+ if (MCASTFilter) {
+ pico_tree_foreach(index, MCASTFilter)
+ {
+ source = pico_zalloc(sizeof(struct pico_ip4));
+ if (!source) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
+ pico_tree_insert(&g->MCASTSources, source);
+ }
+ }
+ g->filter_mode = filter_mode;
+ }
+ }
+
+ pico_ipv4_mcast_print_groups(link);
+ return 0;
+}
+
+struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
+{
+ return mcast_default_link;
+}
+
+static int pico_ipv4_mcast_filter(struct pico_frame *f)
+{
+ struct pico_ipv4_link *link = NULL;
+ struct pico_tree_node *index = NULL, *index2 = NULL;
+ struct pico_mcast_group *g = NULL, test = {0};
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+
+ test.mcast_addr = hdr->dst;
+
+ pico_tree_foreach(index, &Tree_dev_link)
+ {
+ link = index->keyValue;
+ g = pico_tree_findKey(link->MCASTGroups, &test);
+ if (g) {
+ if (f->dev == link->dev) {
+ ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name);
+ /* perform source filtering */
+ switch (g->filter_mode)
+ {
+ case PICO_IP_MULTICAST_INCLUDE:
+ pico_tree_foreach(index2, &g->MCASTSources)
+ {
+ if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
+ ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr);
+ return 0;
+ }
+ }
+ ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr);
+ return -1;
+
+ case PICO_IP_MULTICAST_EXCLUDE:
+ pico_tree_foreach(index2, &g->MCASTSources)
+ {
+ if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
+ ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr);
+ return -1;
+ }
+ }
+ ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr);
+ return 0;
+
+ default:
+ return -1;
+ }
+ } else {
+ ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name);
+ }
+ } else {
+ ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name);
+ }
+ }
+ return -1;
+}
+
+#else
+
+int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return NULL;
+}
+#endif /* PICO_SUPPORT_MCAST */
+
+int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto)
+{
+
+ struct pico_ipv4_route *route;
+ struct pico_ipv4_link *link;
+ struct pico_ipv4_hdr *hdr;
+ uint8_t ttl = PICO_IPV4_DEFAULT_TTL;
+ uint8_t vhl = 0x45; /* version 4, header length 20 */
+ static uint16_t ipv4_progressive_id = 0x91c0;
+#ifdef PICO_SUPPORT_MCAST
+ struct pico_tree_node *index;
+#endif
+
+ if(!f || !dst) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ if (!hdr) {
+ dbg("IP header error\n");
+ pico_err = PICO_ERR_EINVAL;
+ goto drop;
+ }
+
+ if (dst->addr == 0) {
+ dbg("IP destination addr error\n");
+ pico_err = PICO_ERR_EINVAL;
+ goto drop;
+ }
+
+ route = route_find(dst);
+ if (!route) {
+ dbg("Route to %08x not found.\n", long_be(dst->addr));
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ goto drop;
+ } else {
+ link = route->link;
+#ifdef PICO_SUPPORT_MCAST
+ if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */
+ switch (proto) {
+ case PICO_PROTO_UDP:
+ if(pico_udp_get_mc_ttl(f->sock, &ttl) < 0)
+ ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
+ break;
+ case PICO_PROTO_IGMP:
+ vhl = 0x46; /* header length 24 */
+ ttl = 1;
+ /* router alert (RFC 2113) */
+ hdr->options[0] = 0x94;
+ hdr->options[1] = 0x04;
+ hdr->options[2] = 0x00;
+ hdr->options[3] = 0x00;
+ if (f->dev && link->dev != f->dev) { /* default link is not requested link */
+ pico_tree_foreach(index, &Tree_dev_link) {
+ link = index->keyValue;
+ if (link->dev == f->dev)
+ break;
+ }
+ }
+ break;
+ default:
+ ttl = PICO_IPV4_DEFAULT_TTL;
+ }
+ }
+#endif
+ }
+
+ hdr->vhl = vhl;
+ hdr->len = short_be(f->transport_len + f->net_len);
+ if (f->transport_hdr != f->payload)
+ ipv4_progressive_id++;
+ hdr->id = short_be(ipv4_progressive_id);
+ hdr->dst.addr = dst->addr;
+ hdr->src.addr = link->address.addr;
+ hdr->ttl = ttl;
+ hdr->proto = proto;
+ hdr->frag = short_be(PICO_IPV4_DONTFRAG);
+#ifdef PICO_SUPPORT_IPFRAG
+# ifdef PICO_SUPPORT_UDP
+ if (proto == PICO_PROTO_UDP) {
+ /* first fragment, can not use transport_len to calculate IP length */
+ if (f->transport_hdr != f->payload)
+ hdr->len = short_be(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len);
+ /* set fragmentation flags and offset calculated in socket layer */
+ hdr->frag = f->frag;
+ }
+# endif /* PICO_SUPPORT_UDP */
+#endif /* PICO_SUPPORT_IPFRAG */
+ pico_ipv4_checksum(f);
+
+ if (f->sock && f->sock->dev){
+ //if the socket has its device set, use that (currently used for DHCP)
+ f->dev = f->sock->dev;
+ } else {
+ f->dev = link->dev;
+ }
+
+#ifdef PICO_SUPPORT_MCAST
+ if (pico_ipv4_is_multicast(hdr->dst.addr)) {
+ struct pico_frame *cpy;
+ /* Sending UDP multicast datagram, am I member? If so, loopback copy */
+ if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) {
+ ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n");
+ cpy = pico_frame_copy(f);
+ pico_enqueue(&in, cpy);
+ }
+ }
+#endif
+
+ if(pico_ipv4_link_get(&hdr->dst)){
+ //it's our own IP
+ return pico_enqueue(&in, f);
+ }else{
+ /* TODO: Check if there are members subscribed here */
+ return pico_enqueue(&out, f);
+ }
+
+drop:
+ pico_frame_discard(f);
+ return -1;
+}
+
+
+static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f)
+{
+ struct pico_ip4 *dst;
+ struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info;
+ IGNORE_PARAMETER(self);
+
+ if (!f->sock) {
+ pico_frame_discard(f);
+ return -1;
+ }
+
+ if (remote_duple) {
+ dst = &remote_duple->remote_addr.ip4;
+ } else {
+ dst = &f->sock->remote_addr.ip4;
+ }
+
+ return pico_ipv4_frame_push(f, dst, f->sock->proto->proto_number);
+}
+
+
+#ifdef DEBUG_ROUTE
+static void dbg_route(void)
+{
+ struct pico_ipv4_route *r;
+ struct pico_tree_node * index;
+ pico_tree_foreach(index,&Routes){
+ r = index->keyValue;
+ dbg("Route to %08x/%08x, gw %08x, dev: %s, metric: %d\n", r->dest.addr, r->netmask.addr, r->gateway.addr, r->link->dev->name, r->metric);
+ }
+}
+#else
+#define dbg_route() do{ }while(0)
+#endif
+
+int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
+{
+ struct pico_ipv4_route test, *new;
+ test.dest.addr = address.addr;
+ test.netmask.addr = netmask.addr;
+ test.metric = metric;
+
+ if(pico_tree_findKey(&Routes,&test)){
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ new = pico_zalloc(sizeof(struct pico_ipv4_route));
+ if (!new) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ new->dest.addr = address.addr;
+ new->netmask.addr = netmask.addr;
+ new->gateway.addr = gateway.addr;
+ new->metric = metric;
+ if (gateway.addr == 0) {
+ /* No gateway provided, use the link */
+ new->link = link;
+ } else {
+ struct pico_ipv4_route *r = route_find(&gateway);
+ if (!r ) { /* Specified Gateway is unreachable */
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ pico_free(new);
+ return -1;
+ }
+ if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
+ pico_err = PICO_ERR_ENETUNREACH;
+ pico_free(new);
+ return -1;
+ }
+ new->link = r->link;
+ }
+ if (!new->link) {
+ pico_err = PICO_ERR_EINVAL;
+ pico_free(new);
+ return -1;
+ }
+
+ pico_tree_insert(&Routes,new);
+ dbg_route();
+ return 0;
+}
+
+int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
+{
+ struct pico_ipv4_route test, *found;
+ IGNORE_PARAMETER(gateway);
+
+ if (!link) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ test.dest.addr = address.addr;
+ test.netmask.addr = netmask.addr;
+ test.metric = metric;
+
+ found = pico_tree_findKey(&Routes,&test);
+ if (found) {
+
+ pico_tree_delete(&Routes,found);
+ pico_free(found);
+
+ dbg_route();
+ return 0;
+ }
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+}
+
+
+int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask)
+{
+ struct pico_ipv4_link test, *new;
+ struct pico_ip4 network, gateway;
+ char ipstr[30];
+
+ if(!dev) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ test.address.addr = address.addr;
+ test.netmask.addr = netmask.addr;
+ test.dev = dev;
+ /** XXX: Valid netmask / unicast address test **/
+
+ if(pico_tree_findKey(&Tree_dev_link, &test)) {
+ dbg("IPv4: Trying to assign an invalid address (in use)\n");
+ pico_err = PICO_ERR_EADDRINUSE;
+ return -1;
+ }
+
+ /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/
+ new = pico_zalloc(sizeof(struct pico_ipv4_link));
+ if (!new) {
+ dbg("IPv4: Out of memory!\n");
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ new->address.addr = address.addr;
+ new->netmask.addr = netmask.addr;
+ new->dev = dev;
+#ifdef PICO_SUPPORT_MCAST
+ new->MCASTGroups = pico_zalloc(sizeof(struct pico_tree));
+ if (!new->MCASTGroups) {
+ pico_free(new);
+ dbg("IPv4: Out of memory!\n");
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+
+ new->MCASTGroups->root = &LEAF;
+ new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
+ new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
+ new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
+#endif
+
+ pico_tree_insert(&Tree_dev_link, new);
+#ifdef PICO_SUPPORT_MCAST
+ do {
+ struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
+ if (!mcast_default_link) {
+ mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
+ mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
+ mcast_gw.addr = long_be(0x00000000);
+ mcast_default_link = new;
+ pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new);
+ }
+ mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
+ pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
+ } while(0);
+#endif
+
+ network.addr = address.addr & netmask.addr;
+ gateway.addr = 0U;
+ pico_ipv4_route_add(network, netmask, gateway, 1, new);
+ pico_ipv4_to_string(ipstr, new->address.addr);
+ dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name);
+ return 0;
+}
+
+
+int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
+{
+ struct pico_ipv4_link test, *found;
+ struct pico_ip4 network;
+
+ if(!dev) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ test.address.addr = address.addr;
+ test.dev = dev;
+ found = pico_tree_findKey(&Tree_dev_link, &test);
+ if (!found) {
+ pico_err = PICO_ERR_ENXIO;
+ return -1;
+ }
+
+ network.addr = found->address.addr & found->netmask.addr;
+ pico_ipv4_route_del(network, found->netmask,pico_ipv4_route_get_gateway(&found->address), 1, found);
+#ifdef PICO_SUPPORT_MCAST
+ do {
+ struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
+ struct pico_mcast_group *g = NULL;
+ struct pico_tree_node * index, * _tmp;
+ if (found == mcast_default_link) {
+ mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
+ mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
+ mcast_gw.addr = long_be(0x00000000);
+ mcast_default_link = NULL;
+ pico_ipv4_route_del(mcast_addr, mcast_nm, mcast_gw, 1, found);
+ }
+ mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
+ pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
+ pico_tree_foreach_safe(index,found->MCASTGroups, _tmp)
+ {
+ g = index->keyValue;
+ pico_tree_delete(found->MCASTGroups, g);
+ pico_free(g);
+ }
+ } while(0);
+#endif
+
+ pico_tree_delete(&Tree_dev_link, found);
+ /* XXX: pico_free(found); */
+ /* XXX: cleanup all routes containing the removed link */
+ return 0;
+}
+
+
+struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address)
+{
+ struct pico_ipv4_link test = {0}, *found = NULL;
+ test.address.addr = address->addr;
+
+ found = pico_tree_findKey(&Tree_dev_link, &test);
+ if (!found)
+ return NULL;
+ else
+ return found;
+}
+
+struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev)
+{
+ struct pico_tree_node *index = NULL;
+ struct pico_ipv4_link *link = NULL;
+
+ pico_tree_foreach(index, &Tree_dev_link)
+ {
+ link = index->keyValue;
+ if (link->dev == dev)
+ return link;
+ }
+ return NULL;
+}
+
+
+struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address)
+{
+ struct pico_ipv4_link test, *found;
+ if(!address) {
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+ }
+ test.dev = NULL;
+ test.address.addr = address->addr;
+ found = pico_tree_findKey(&Tree_dev_link, &test);
+ if (!found) {
+ pico_err = PICO_ERR_ENXIO;
+ return NULL;
+ }
+ return found->dev;
+}
+
+int pico_ipv4_rebound(struct pico_frame *f)
+{
+ struct pico_ip4 dst;
+ struct pico_ipv4_hdr *hdr;
+ if(!f) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ if (!hdr) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ dst.addr = hdr->src.addr;
+ return pico_ipv4_frame_push(f, &dst, hdr->proto);
+}
+
+static int pico_ipv4_forward(struct pico_frame *f)
+{
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ struct pico_ipv4_route *rt;
+ if (!hdr) {
+ return -1;
+ }
+
+ rt = route_find(&hdr->dst);
+ if (!rt) {
+ pico_notify_dest_unreachable(f);
+ return -1;
+ }
+
+ f->dev = rt->link->dev;
+ hdr->ttl-=1;
+ if (hdr->ttl < 1) {
+ pico_notify_ttl_expired(f);
+ return -1;
+ }
+ hdr->crc++;
+
+ pico_ipv4_nat_outbound(f, &rt->link->address);
+
+ f->start = f->net_hdr;
+ if(f->dev->eth != NULL)
+ f->len -= PICO_SIZE_ETHHDR;
+ pico_sendto_dev(f);
+ return 0;
+
+}
+
+int pico_ipv4_is_broadcast(uint32_t addr)
+{
+ struct pico_ipv4_link *link;
+ struct pico_tree_node * index;
+ if (addr == PICO_IP4_ANY)
+ return 1;
+ if (addr == PICO_IP4_BCAST)
+ return 1;
+
+ pico_tree_foreach(index,&Tree_dev_link) {
+ link = index->keyValue;
+ if ((link->address.addr | (~link->netmask.addr)) == addr)
+ return 1;
+ }
+ return 0;
+}
+
+void pico_ipv4_unreachable(struct pico_frame *f, int err)
+{
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+#if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP
+ f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR;
+ pico_transport_error(f, hdr->proto, err);
+#endif
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipv4.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,96 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_IPV4
+#define _INCLUDE_PICO_IPV4
+#include "pico_addressing.h"
+#include "pico_protocol.h"
+#include "pico_tree.h"
+
+#define PICO_IPV4_INADDR_ANY 0x00000000U
+
+#define PICO_SIZE_IP4HDR ((sizeof(struct pico_ipv4_hdr)))
+#define PICO_IPV4_DONTFRAG 0x4000
+#define PICO_IPV4_MOREFRAG 0x2000
+#define PICO_IPV4_FRAG_MASK 0x1FFF
+#define PICO_IPV4_DEFAULT_TTL 64
+
+extern struct pico_protocol pico_proto_ipv4;
+
+struct __attribute__((packed)) pico_ipv4_hdr {
+ uint8_t vhl;
+ uint8_t tos;
+ uint16_t len;
+ uint16_t id;
+ uint16_t frag;
+ uint8_t ttl;
+ uint8_t proto;
+ uint16_t crc;
+ struct pico_ip4 src;
+ struct pico_ip4 dst;
+ uint8_t options[0];
+};
+
+struct __attribute__((packed)) pico_ipv4_pseudo_hdr
+{
+ struct pico_ip4 src;
+ struct pico_ip4 dst;
+ uint8_t zeros;
+ uint8_t proto;
+ uint16_t len;
+};
+
+/* Interface: link to device */
+struct pico_mcast_list;
+
+struct pico_ipv4_link
+{
+ struct pico_device *dev;
+ struct pico_ip4 address;
+ struct pico_ip4 netmask;
+#ifdef PICO_SUPPORT_MCAST
+ struct pico_tree *MCASTGroups;
+ uint8_t mcast_compatibility;
+ uint8_t mcast_last_query_interval;
+#endif
+};
+
+#ifdef PICO_SUPPORT_MCAST
+struct pico_mcast_group {
+ uint8_t filter_mode;
+ uint16_t reference_count;
+ struct pico_ip4 mcast_addr;
+ struct pico_tree MCASTSources;
+};
+#endif
+
+int pico_ipv4_to_string(char *ipbuf, const uint32_t ip);
+int pico_string_to_ipv4(const char *ipstr, uint32_t *ip);
+int pico_ipv4_valid_netmask(uint32_t mask);
+int pico_ipv4_is_unicast(uint32_t address);
+int pico_ipv4_is_multicast(uint32_t address);
+int pico_ipv4_is_broadcast(uint32_t addr);
+
+int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask);
+int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address);
+int pico_ipv4_rebound(struct pico_frame *f);
+
+int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto);
+struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address);
+struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev);
+struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address);
+struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst);
+int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link);
+int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link);
+struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr);
+void pico_ipv4_unreachable(struct pico_frame *f, int err);
+
+int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter);
+int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter);
+struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void);
+
+#endif /* _INCLUDE_PICO_IPV4 */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_ipv6.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,30 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +. + +*********************************************************************/ +#ifndef _INCLUDE_PICO_IPV6 +#define _INCLUDE_PICO_IPV6 +#include "pico_addressing.h" +#include "pico_protocol.h" + +extern struct pico_protocol pico_proto_ipv6; +extern const uint8_t PICO_IPV6_ANY[PICO_SIZE_IP6]; + + +/* This module is responsible for routing outgoing packets and + * delivering incoming packets to other layers + */ + +/* Interface for processing incoming ipv6 packets (decap/deliver) */ +int pico_ipv6_process_in(struct pico_frame *f); + +/* Interface for processing outgoing ipv6 frames (encap/push) */ +int pico_ipv6_process_out(struct pico_frame *f); + +/* Return estimated overhead for ipv6 frames to define allocation */ +int pico_ipv6_overhead(struct pico_frame *f); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_nat.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,500 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Kristof Roelants, Brecht Van Cauwenberghe,
+ Simon Maes, Philippe Mariman
+*********************************************************************/
+
+#include "pico_stack.h"
+#include "pico_frame.h"
+#include "pico_tcp.h"
+#include "pico_udp.h"
+#include "pico_ipv4.h"
+#include "pico_addressing.h"
+#include "pico_nat.h"
+
+#ifdef PICO_SUPPORT_IPV4
+#ifdef PICO_SUPPORT_NAT
+
+#define nat_dbg(...) do{}while(0)
+//#define nat_dbg dbg
+#define PICO_NAT_TIMEWAIT 240000 /* msec (4 mins) */
+
+#define PICO_NAT_INBOUND 0
+#define PICO_NAT_OUTBOUND 1
+
+struct pico_nat_tuple {
+ uint8_t proto;
+ uint16_t conn_active:11;
+ uint16_t portforward:1;
+ uint16_t rst:1;
+ uint16_t syn:1;
+ uint16_t fin_in:1;
+ uint16_t fin_out:1;
+ uint16_t src_port;
+ uint16_t dst_port;
+ uint16_t nat_port;
+ struct pico_ip4 src_addr;
+ struct pico_ip4 dst_addr;
+ struct pico_ip4 nat_addr;
+};
+
+static struct pico_ipv4_link *nat_link = NULL;
+
+static int nat_cmp_inbound(void * ka, void * kb)
+{
+ struct pico_nat_tuple *a = ka, *b = kb;
+
+ if (a->nat_port < b->nat_port)
+ return -1;
+ if (a->nat_port > b->nat_port)
+ return 1;
+ if (a->proto < b->proto)
+ return -1;
+ if (a->proto > b->proto)
+ return 1;
+ return 0; /* identical */
+}
+
+static int nat_cmp_outbound(void * ka, void * kb)
+{
+ struct pico_nat_tuple *a =ka, *b = kb;
+
+ if (a->src_addr.addr < b->src_addr.addr)
+ return -1;
+ if (a->src_addr.addr > b->src_addr.addr)
+ return 1;
+ if (a->src_port < b->src_port)
+ return -1;
+ if (a->src_port > b->src_port)
+ return 1;
+ if (a->proto < b->proto)
+ return -1;
+ if (a->proto > b->proto)
+ return 1;
+ return 0; /* identical */
+}
+
+PICO_TREE_DECLARE(NATOutbound, nat_cmp_outbound);
+PICO_TREE_DECLARE(NATInbound, nat_cmp_inbound);
+
+void pico_ipv4_nat_print_table(void)
+{
+ struct pico_nat_tuple __attribute__((unused)) *t = NULL;
+ struct pico_tree_node *index = NULL;
+
+ nat_dbg("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+ nat_dbg("+ NAT table +\n");
+ nat_dbg("+------------------------------------------------------------------------------------------------------------------------+\n");
+ nat_dbg("+ src_addr | src_port | dst_addr | dst_port | nat_addr | nat_port | proto | conn active | FIN1 | FIN2 | SYN | RST | FORW +\n");
+ nat_dbg("+------------------------------------------------------------------------------------------------------------------------+\n");
+
+ pico_tree_foreach(index, &NATOutbound)
+ {
+ t = index->keyValue;
+ nat_dbg("+ %08X | %05u | %08X | %05u | %08X | %05u | %03u | %03u | %u | %u | %u | %u | %u +\n",
+ long_be(t->src_addr.addr), t->src_port, long_be(t->dst_addr.addr), t->dst_port, long_be(t->nat_addr.addr), t->nat_port,
+ t->proto, t->conn_active, t->fin_in, t->fin_out, t->syn, t->rst, t->portforward);
+ }
+ nat_dbg("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+}
+
+/*
+ 2 options:
+ find on nat_port and proto
+ find on src_addr, src_port and proto
+ zero the unused parameters
+*/
+static struct pico_nat_tuple *pico_ipv4_nat_find_tuple(uint16_t nat_port, struct pico_ip4 *src_addr, uint16_t src_port, uint8_t proto)
+{
+ struct pico_nat_tuple *found = NULL, test = {0};
+
+ test.nat_port = nat_port;
+ test.src_port = src_port;
+ test.proto = proto;
+ if (src_addr)
+ test.src_addr = *src_addr;
+
+ if (nat_port)
+ found = pico_tree_findKey(&NATInbound, &test);
+ else
+ found = pico_tree_findKey(&NATOutbound, &test);
+
+ if (found)
+ return found;
+ else
+ return NULL;
+}
+
+int pico_ipv4_nat_find(uint16_t nat_port, struct pico_ip4 *src_addr, uint16_t src_port, uint8_t proto)
+{
+ struct pico_nat_tuple *t = NULL;
+
+ t = pico_ipv4_nat_find_tuple(nat_port, src_addr, src_port, proto);
+ if (t)
+ return 1;
+ else
+ return 0;
+}
+
+static struct pico_nat_tuple *pico_ipv4_nat_add(struct pico_ip4 dst_addr, uint16_t dst_port, struct pico_ip4 src_addr, uint16_t src_port,
+ struct pico_ip4 nat_addr, uint16_t nat_port, uint8_t proto)
+{
+ struct pico_nat_tuple *t = pico_zalloc(sizeof(struct pico_nat_tuple));
+ if (!t) {
+ pico_err = PICO_ERR_ENOMEM;
+ return NULL;
+ }
+
+ t->dst_addr = dst_addr;
+ t->dst_port = dst_port;
+ t->src_addr = src_addr;
+ t->src_port = src_port;
+ t->nat_addr = nat_addr;
+ t->nat_port = nat_port;
+ t->proto = proto;
+ t->conn_active = 1;
+ t->portforward = 0;
+ t->rst = 0;
+ t->syn = 0;
+ t->fin_in = 0;
+ t->fin_out = 0;
+
+ if (pico_tree_insert(&NATOutbound, t)) {
+ pico_free(t);
+ return NULL;
+ }
+ if (pico_tree_insert(&NATInbound, t)) {
+ pico_tree_delete(&NATOutbound, t);
+ pico_free(t);
+ return NULL;
+ }
+
+ return t;
+}
+
+static int pico_ipv4_nat_del(uint16_t nat_port, uint8_t proto)
+{
+ struct pico_nat_tuple *t = NULL;
+ t = pico_ipv4_nat_find_tuple(nat_port, NULL, 0, proto);
+ if (t) {
+ pico_tree_delete(&NATOutbound, t);
+ pico_tree_delete(&NATInbound, t);
+ pico_free(t);
+ }
+ return 0;
+}
+
+static struct pico_nat_tuple *pico_ipv4_nat_generate_tuple(struct pico_frame *f)
+{
+ struct pico_trans *trans = NULL;
+ struct pico_ipv4_hdr *net = (struct pico_ipv4_hdr *)f->net_hdr;
+ uint16_t nport = 0;
+ uint8_t retry = 32;
+
+ /* generate NAT port */
+ do {
+ uint32_t rand = pico_rand();
+ nport = (uint16_t) (rand & 0xFFFFU);
+ nport = (uint16_t)(nport % (65535 - 1024)) + 1024U;
+ nport = short_be(nport);
+
+ if (pico_is_port_free(net->proto, nport, NULL, &pico_proto_ipv4))
+ break;
+ } while (--retry);
+
+ if (!retry)
+ return NULL;
+
+ switch (net->proto) {
+ case PICO_PROTO_TCP:
+ {
+ struct pico_tcp_hdr *tcp = (struct pico_tcp_hdr *)f->transport_hdr;
+ trans = &tcp->trans;
+ break;
+ }
+ case PICO_PROTO_UDP:
+ {
+ struct pico_udp_hdr *udp = (struct pico_udp_hdr *)f->transport_hdr;
+ trans = &udp->trans;
+ break;
+ }
+ case PICO_PROTO_ICMP4:
+ /* XXX: implement */
+ break;
+
+ default:
+ return NULL;
+ }
+
+ return pico_ipv4_nat_add(net->dst, trans->dport, net->src, trans->sport, nat_link->address, nport, net->proto);
+ // XXX return pico_ipv4_nat_add(nat_link->address, port, net->src, trans->sport, net->proto);
+}
+
+static int pico_ipv4_nat_snif_session(struct pico_nat_tuple *t, struct pico_frame *f, uint8_t direction)
+{
+ struct pico_ipv4_hdr *net = (struct pico_ipv4_hdr *)f->net_hdr;
+
+ switch (net->proto) {
+ case PICO_PROTO_TCP:
+ {
+ struct pico_tcp_hdr *tcp = (struct pico_tcp_hdr *)f->transport_hdr;
+ if (tcp->flags & PICO_TCP_SYN)
+ t->syn = 1;
+ if (tcp->flags & PICO_TCP_RST)
+ t->rst = 1;
+ if ((tcp->flags & PICO_TCP_FIN) && (direction == PICO_NAT_INBOUND))
+ t->fin_in = 1;
+ if ((tcp->flags & PICO_TCP_FIN) && (direction == PICO_NAT_OUTBOUND))
+ t->fin_out = 1;
+ break;
+ }
+
+ case PICO_PROTO_UDP:
+ t->conn_active = 1;
+ break;
+
+ case PICO_PROTO_ICMP4:
+ /* XXX: implement */
+ break;
+
+ default:
+ return -1;
+ }
+
+ return 0;
+}
+
+static void pico_ipv4_nat_table_cleanup(unsigned long now, void *_unused)
+{
+ struct pico_tree_node *index = NULL, *_tmp = NULL;
+ struct pico_nat_tuple *t = NULL;
+ IGNORE_PARAMETER(now);
+ IGNORE_PARAMETER(_unused);
+ nat_dbg("NAT: before table cleanup:\n");
+ pico_ipv4_nat_print_table();
+
+ pico_tree_foreach_reverse_safe(index, &NATOutbound, _tmp)
+ {
+ t = index->keyValue;
+ switch (t->proto)
+ {
+ case PICO_PROTO_TCP:
+ if (t->portforward)
+ break;
+ else if (t->conn_active == 0 || t->conn_active > 360) /* conn active for > 24 hours */
+ pico_ipv4_nat_del(t->nat_port, t->proto);
+ else if (t->rst || (t->fin_in && t->fin_out))
+ t->conn_active = 0;
+ else
+ t->conn_active++;
+ break;
+
+ case PICO_PROTO_UDP:
+ if (t->portforward)
+ break;
+ else if (t->conn_active > 1)
+ pico_ipv4_nat_del(t->nat_port, t->proto);
+ else
+ t->conn_active++;
+ break;
+
+ case PICO_PROTO_ICMP4:
+ if (t->conn_active > 1)
+ pico_ipv4_nat_del(t->nat_port, t->proto);
+ else
+ t->conn_active++;
+
+ default:
+ /* unknown protocol in NAT table, delete when it has existed NAT_TIMEWAIT */
+ if (t->conn_active > 1)
+ pico_ipv4_nat_del(t->nat_port, t->proto);
+ else
+ t->conn_active++;
+ }
+ }
+
+ nat_dbg("NAT: after table cleanup:\n");
+ pico_ipv4_nat_print_table();
+ pico_timer_add(PICO_NAT_TIMEWAIT, pico_ipv4_nat_table_cleanup, NULL);
+}
+
+int pico_ipv4_port_forward(struct pico_ip4 nat_addr, uint16_t nat_port, struct pico_ip4 src_addr, uint16_t src_port, uint8_t proto, uint8_t flag)
+{
+ struct pico_nat_tuple *t = NULL;
+ struct pico_ip4 any_addr = {0};
+ uint16_t any_port = 0;
+
+ switch (flag)
+ {
+ case PICO_NAT_PORT_FORWARD_ADD:
+ t = pico_ipv4_nat_add(any_addr, any_port, src_addr, src_port, nat_addr, nat_port, proto);
+ if (!t) {
+ pico_err = PICO_ERR_EAGAIN;
+ return -1;
+ }
+ t->portforward = 1;
+ break;
+
+ case PICO_NAT_PORT_FORWARD_DEL:
+ return pico_ipv4_nat_del(nat_port, proto);
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ pico_ipv4_nat_print_table();
+ return 0;
+}
+
+int pico_ipv4_nat_inbound(struct pico_frame *f, struct pico_ip4 *link_addr)
+{
+ struct pico_nat_tuple *tuple = NULL;
+ struct pico_trans *trans = NULL;
+ struct pico_ipv4_hdr *net = (struct pico_ipv4_hdr *)f->net_hdr;
+
+ if (!pico_ipv4_nat_is_enabled(link_addr))
+ return -1;
+
+ switch (net->proto) {
+ case PICO_PROTO_TCP:
+ {
+ struct pico_tcp_hdr *tcp = (struct pico_tcp_hdr *)f->transport_hdr;
+ trans = &tcp->trans;
+ tuple = pico_ipv4_nat_find_tuple(trans->dport, 0, 0, net->proto);
+ if (!tuple)
+ return -1;
+ /* replace dst IP and dst PORT */
+ net->dst = tuple->src_addr;
+ trans->dport = tuple->src_port;
+ /* recalculate CRC */
+ tcp->crc = 0;
+ tcp->crc = short_be(pico_tcp_checksum_ipv4(f));
+ break;
+ }
+ case PICO_PROTO_UDP:
+ {
+ struct pico_udp_hdr *udp = (struct pico_udp_hdr *)f->transport_hdr;
+ trans = &udp->trans;
+ tuple = pico_ipv4_nat_find_tuple(trans->dport, 0, 0, net->proto);
+ if (!tuple)
+ return -1;
+ /* replace dst IP and dst PORT */
+ net->dst = tuple->src_addr;
+ trans->dport = tuple->src_port;
+ /* recalculate CRC */
+ udp->crc = 0;
+ udp->crc = short_be(pico_udp_checksum_ipv4(f));
+ break;
+ }
+ case PICO_PROTO_ICMP4:
+ /* XXX reimplement */
+ break;
+
+ default:
+ nat_dbg("NAT ERROR: inbound NAT on erroneous protocol\n");
+ return -1;
+ }
+
+ pico_ipv4_nat_snif_session(tuple, f, PICO_NAT_INBOUND);
+ net->crc = 0;
+ net->crc = short_be(pico_checksum(net, f->net_len));
+
+ nat_dbg("NAT: inbound translation {dst.addr, dport}: {%08X,%u} -> {%08X,%u}\n",
+ tuple->nat_addr.addr, short_be(tuple->nat_port), tuple->src_addr.addr, short_be(tuple->src_port));
+
+ return 0;
+}
+
+int pico_ipv4_nat_outbound(struct pico_frame *f, struct pico_ip4 *link_addr)
+{
+ struct pico_nat_tuple *tuple = NULL;
+ struct pico_trans *trans = NULL;
+ struct pico_ipv4_hdr *net = (struct pico_ipv4_hdr *)f->net_hdr;
+
+ if (!pico_ipv4_nat_is_enabled(link_addr))
+ return -1;
+
+ switch (net->proto) {
+ case PICO_PROTO_TCP:
+ {
+ struct pico_tcp_hdr *tcp = (struct pico_tcp_hdr *)f->transport_hdr;
+ trans = &tcp->trans;
+ tuple = pico_ipv4_nat_find_tuple(0, &net->src, trans->sport, net->proto);
+ if (!tuple)
+ tuple = pico_ipv4_nat_generate_tuple(f);
+ /* replace src IP and src PORT */
+ net->src = tuple->nat_addr;
+ trans->sport = tuple->nat_port;
+ /* recalculate CRC */
+ tcp->crc = 0;
+ tcp->crc = short_be(pico_tcp_checksum_ipv4(f));
+ break;
+ }
+ case PICO_PROTO_UDP:
+ {
+ struct pico_udp_hdr *udp = (struct pico_udp_hdr *)f->transport_hdr;
+ trans = &udp->trans;
+ tuple = pico_ipv4_nat_find_tuple(0, &net->src, trans->sport, net->proto);
+ if (!tuple)
+ tuple = pico_ipv4_nat_generate_tuple(f);
+ /* replace src IP and src PORT */
+ net->src = tuple->nat_addr;
+ trans->sport = tuple->nat_port;
+ /* recalculate CRC */
+ udp->crc = 0;
+ udp->crc = short_be(pico_udp_checksum_ipv4(f));
+ break;
+ }
+ case PICO_PROTO_ICMP4:
+ /* XXX reimplement */
+ break;
+
+ default:
+ nat_dbg("NAT ERROR: outbound NAT on erroneous protocol\n");
+ return -1;
+ }
+
+ pico_ipv4_nat_snif_session(tuple, f, PICO_NAT_OUTBOUND);
+ net->crc = 0;
+ net->crc = short_be(pico_checksum(net, f->net_len));
+
+ nat_dbg("NAT: outbound translation {src.addr, sport}: {%08X,%u} -> {%08X,%u}\n",
+ tuple->src_addr.addr, short_be(tuple->src_port), tuple->nat_addr.addr, short_be(tuple->nat_port));
+
+ return 0;
+}
+
+int pico_ipv4_nat_enable(struct pico_ipv4_link *link)
+{
+ if (link == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ nat_link = link;
+ pico_timer_add(PICO_NAT_TIMEWAIT, pico_ipv4_nat_table_cleanup, NULL);
+ return 0;
+}
+
+int pico_ipv4_nat_disable(void)
+{
+ nat_link = NULL;
+ return 0;
+}
+
+int pico_ipv4_nat_is_enabled(struct pico_ip4 *link_addr)
+{
+ if (!nat_link)
+ return 0;
+ if (nat_link->address.addr != link_addr->addr)
+ return 0;
+ return 1;
+}
+
+#endif
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_nat.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,74 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Kristof Roelants, Simon Maes, Brecht Van Cauwenberghe
+*********************************************************************/
+
+#ifndef _INCLUDE_PICO_NAT
+#define _INCLUDE_PICO_NAT
+#include "pico_frame.h"
+
+#define PICO_NAT_PORT_FORWARD_DEL 0
+#define PICO_NAT_PORT_FORWARD_ADD 1
+
+#ifdef PICO_SUPPORT_NAT
+void pico_ipv4_nat_print_table(void);
+int pico_ipv4_nat_find(uint16_t nat_port, struct pico_ip4 *src_addr, uint16_t src_port, uint8_t proto);
+int pico_ipv4_port_forward(struct pico_ip4 nat_addr, uint16_t nat_port, struct pico_ip4 src_addr, uint16_t src_port, uint8_t proto, uint8_t flag);
+
+int pico_ipv4_nat_inbound(struct pico_frame *f, struct pico_ip4 *link_addr);
+int pico_ipv4_nat_outbound(struct pico_frame *f, struct pico_ip4 *link_addr);
+int pico_ipv4_nat_enable(struct pico_ipv4_link *link);
+int pico_ipv4_nat_disable(void);
+int pico_ipv4_nat_is_enabled(struct pico_ip4 *link_addr);
+#else
+
+#define pico_ipv4_nat_print_table() do{}while(0)
+static inline int pico_ipv4_nat_inbound(struct pico_frame *f, struct pico_ip4 *link_addr)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_nat_outbound(struct pico_frame *f, struct pico_ip4 *link_addr)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_nat_enable(struct pico_ipv4_link *link)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_nat_disable(void)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_nat_is_enabled(struct pico_ip4 *link_addr)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_nat_find(uint16_t nat_port, struct pico_ip4 *src_addr, uint16_t src_port, uint8_t proto)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_port_forward(struct pico_ip4 nat_addr, uint16_t nat_port, struct pico_ip4 src_addr, uint16_t src_port, uint8_t proto, uint8_t flag)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+#endif
+
+#endif /* _INCLUDE_PICO_NAT */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_simple_http.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,128 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Author: Andrei Carp <andrei.carp@tass.be>
+*********************************************************************/
+
+#include "pico_config.h"
+#include "pico_socket.h"
+#include "pico_tcp.h"
+#include "pico_ipv4.h"
+#include "pico_simple_http.h"
+
+/* The HTTP Server cannot be available without TCP support */
+#if (defined PICO_SUPPORT_HTTP) && (defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_TCP)
+
+#define HTTP_LISTEN_PORT 80u
+#define HTTP_BACKLOG 5u
+#define HTTP_HEADER_SIZE 256u
+
+#define HTTP_SUCCESS 0
+#define HTTP_ERROR -1
+
+static struct pico_socket * httpServer = NULL;
+static char httpResponse[] =
+"HTTP/1.0 200 OK\r\n\
+Content-Type: text/html\r\n\
+\r\n\
+<html><head>\r\n\
+<title>picoTCP Simple Http server</title>\r\n\
+</head>\r\n\
+<body>\r\n\
+<h1>Hello world from picoTCP !!</h1>\r\n\
+</body>\r\n";
+
+static void httpEventCbk(uint16_t ev, struct pico_socket *self)
+{
+ static struct pico_socket * client = NULL;
+ uint32_t peer;
+ uint16_t port;
+ int r;
+ char buffer[HTTP_HEADER_SIZE];
+
+ switch(ev)
+ {
+ case PICO_SOCK_EV_CONN :
+ if(!client)
+ client = pico_socket_accept(self, &peer, &port);
+ break;
+
+ case PICO_SOCK_EV_RD:
+ // do not check http integrity, just mark that the http header has arrived
+ // prepare to send the response
+ r = pico_socket_recvfrom(self, buffer, HTTP_HEADER_SIZE, &peer, &port);
+ if(r>0 && memcmp(buffer,"GET",3u) == 0u)
+ { // it is an http header asking for data, return data and close
+ pico_socket_write(self,httpResponse,sizeof(httpResponse));
+ pico_socket_close(self);
+ }
+ else
+ {
+ // kill the connection, invalid header
+ pico_socket_close(self);
+ }
+ break;
+
+ case PICO_SOCK_EV_ERR:
+ case PICO_SOCK_EV_CLOSE:
+ // free the used socket
+ client = NULL;
+ break;
+
+ default :
+ break;
+ }
+}
+
+int pico_startHttpServer(struct pico_ip4 * address)
+{
+
+ uint16_t localHttpPort = short_be(HTTP_LISTEN_PORT);
+
+ if(!pico_is_port_free(localHttpPort,PICO_PROTO_TCP, address, &pico_proto_ipv4))
+ {
+ pico_err = PICO_ERR_EADDRINUSE;
+ return HTTP_ERROR;
+ }
+
+ httpServer = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_TCP, httpEventCbk);
+
+ if(!httpServer)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ return HTTP_ERROR;
+ }
+
+ // both functions set the pico_err themselves.
+ if(pico_socket_bind(httpServer,address,&localHttpPort))
+ return HTTP_ERROR;
+
+ if(pico_socket_listen(httpServer,HTTP_BACKLOG))
+ return HTTP_ERROR;
+
+ return HTTP_SUCCESS;
+}
+
+int pico_stopHttpServer(void)
+{
+ if(!httpServer)
+ {
+ pico_err = PICO_ERR_EINVAL;
+ return HTTP_ERROR;
+ }
+
+ if(pico_socket_close(httpServer))
+ {
+ // no need to set the error here, function already set it
+ httpServer = NULL;
+ return HTTP_ERROR;
+ }
+
+ httpServer = NULL;
+ return HTTP_SUCCESS;
+}
+
+#endif
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_simple_http.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,14 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +Author: Andrei Carp <andrei.carp@tass.be> +*********************************************************************/ + +#ifndef PICO_SIMPLE_HTTP +#define PICO_SIMPLE_HTTP + +extern int pico_startHttpServer(struct pico_ip4 * address); +extern int pico_stopHttpServer(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_tcp.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,2291 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera, Philippe Mariman
+*********************************************************************/
+
+#include "pico_tcp.h"
+#include "pico_config.h"
+#include "pico_eth.h"
+#include "pico_socket.h"
+#include "pico_stack.h"
+#include "pico_socket.h"
+#include "pico_queue.h"
+#include "pico_tree.h"
+
+#define TCP_IS_STATE(s,st) (s->state & st)
+#define TCP_SOCK(s) ((struct pico_socket_tcp *)s)
+#define SEQN(f) (f?(long_be(((struct pico_tcp_hdr *)(f->transport_hdr))->seq)):0)
+#define ACKN(f) (f?(long_be(((struct pico_tcp_hdr *)(f->transport_hdr))->ack)):0)
+
+#define PICO_TCP_RTO_MIN 10
+#define PICO_TCP_RTO_MAX 120000
+#define PICO_TCP_IW 2
+#define PICO_TCP_SYN_TO 1000
+#define PICO_TCP_ZOMBIE_TO 30000
+
+#define PICO_TCP_MAX_CONNECT_RETRIES 7
+
+#define PICO_TCP_LOOKAHEAD 0x00
+#define PICO_TCP_FIRST_DUPACK 0x01
+#define PICO_TCP_SECOND_DUPACK 0x02
+#define PICO_TCP_RECOVER 0x03
+#define PICO_TCP_BLACKOUT 0x04
+#define PICO_TCP_UNREACHABLE 0x05
+#define PICO_TCP_WINDOW_FULL 0x06
+
+/* check if the Nagle algorithm is enabled on the socket */
+#define IS_NAGLE_ENABLED(s) (!(!(!(s->opt_flags & (1 << PICO_SOCKET_OPT_TCPNODELAY)))))
+/* check if tcp connection is "idle" according to Nagle (RFC 896) */
+#define IS_TCP_IDLE(t) ((t->in_flight == 0) && (t->tcpq_out.size == 0))
+/* check if the hold queue contains data (again Nagle) */
+#define IS_TCP_HOLDQ_EMPTY(t) (t->tcpq_hold.size == 0)
+
+#ifdef PICO_SUPPORT_TCP
+#define tcp_dbg_nagle(...) do{}while(0)
+#define tcp_dbg_options(...) do{}while(0)
+
+
+#define tcp_dbg(...) do{}while(0)
+//#define tcp_dbg dbg
+
+struct tcp_port_pair
+{
+ uint16_t local;
+ uint16_t remote;
+};
+
+#ifdef PICO_SUPPORT_MUTEX
+static void * Mutex = NULL;
+#define LOCK(x) {\
+ if (x == NULL) \
+ x = pico_mutex_init(); \
+ pico_mutex_lock(x); \
+}
+#define UNLOCK(x) pico_mutex_unlock(x);
+
+#else
+#define LOCK(x) do{}while(0)
+#define UNLOCK(x) do{}while(0)
+#endif
+
+
+static inline int seq_compare(uint32_t a, uint32_t b)
+{
+ uint32_t thresh = ((uint32_t)(-1))>>1;
+ if (((a > thresh) && (b > thresh)) || ((a <= thresh) && (b <= thresh))) {
+ if (a > b)
+ return 1;
+ if (b > a)
+ return -1;
+ } else {
+ if (a > b)
+ return -2;
+ if (b > a)
+ return 2;
+ }
+ return 0;
+}
+
+static int segment_compare(void * ka, void * kb)
+{
+ struct pico_frame *a = ka, *b = kb;
+ return seq_compare(SEQN(a), SEQN(b));
+}
+
+struct pico_tcp_queue
+{
+ struct pico_tree pool;
+ uint32_t max_size;
+ uint32_t size;
+ uint32_t frames;
+};
+
+static struct pico_frame *peek_segment(struct pico_tcp_queue *tq, uint32_t seq)
+{
+ struct pico_tcp_hdr H;
+ struct pico_frame f = {0};
+ f.transport_hdr = (uint8_t *) (&H);
+ H.seq = long_be(seq);
+
+ return pico_tree_findKey(&tq->pool,&f);
+}
+
+static struct pico_frame *first_segment(struct pico_tcp_queue *tq)
+{
+ return pico_tree_first(&tq->pool);
+}
+
+static struct pico_frame *next_segment(struct pico_tcp_queue *tq, struct pico_frame *cur)
+{
+ if (!cur)
+ return NULL;
+ return peek_segment(tq, SEQN(cur) + cur->payload_len);
+}
+
+static int pico_enqueue_segment(struct pico_tcp_queue *tq, struct pico_frame *f)
+{
+ int ret = -1;
+ if (f->payload_len <= 0) {
+ tcp_dbg("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! TRIED TO ENQUEUE INVALID SEGMENT!\n");
+ //abort();
+ return -1;
+ }
+ LOCK(Mutex);
+ if ((tq->size + f->payload_len) > tq->max_size)
+ {
+ ret = 0;
+ goto out;
+ }
+ if (pico_tree_insert(&tq->pool,f) != 0)
+ {
+ ret = 0;
+ goto out;
+ }
+ tq->size += f->payload_len;
+ if (f->payload_len > 0)
+ tq->frames++;
+ ret = f->payload_len;
+
+out :
+ UNLOCK(Mutex);
+ return ret;
+}
+
+static void pico_discard_segment(struct pico_tcp_queue *tq, struct pico_frame *f)
+{
+ struct pico_frame *f1;
+ LOCK(Mutex);
+ f1 = pico_tree_delete(&tq->pool,f);
+ if (f1) {
+ tq->size -= f->payload_len;
+ if (f->payload_len > 0)
+ tq->frames--;
+ }
+ pico_frame_discard(f);
+ UNLOCK(Mutex);
+}
+
+/* Structure for TCP socket */
+struct tcp_sack_block {
+ uint32_t left;
+ uint32_t right;
+ struct tcp_sack_block *next;
+};
+
+struct pico_socket_tcp {
+ struct pico_socket sock;
+
+ /* Tree/queues */
+ struct pico_tcp_queue tcpq_in;
+ struct pico_tcp_queue tcpq_out;
+ struct pico_tcp_queue tcpq_hold; /* buffer to hold delayed frames according to Nagle */
+
+ /* tcp_output */
+ uint32_t snd_nxt;
+ uint32_t snd_last;
+ uint32_t snd_old_ack;
+ uint32_t snd_retry;
+ uint32_t snd_last_out;
+
+ /* congestion control */
+ uint32_t avg_rtt;
+ uint32_t rttvar;
+ uint32_t rto;
+ uint32_t in_flight;
+ uint8_t timer_running;
+ uint8_t keepalive_timer_running;
+ uint16_t cwnd_counter;
+ uint16_t cwnd;
+ uint16_t ssthresh;
+ uint16_t recv_wnd;
+ uint16_t recv_wnd_scale;
+
+ /* tcp_input */
+ uint32_t rcv_nxt;
+ uint32_t rcv_ackd;
+ uint32_t rcv_processed;
+ uint16_t wnd;
+ uint16_t wnd_scale;
+
+ /* options */
+ uint32_t ts_nxt;
+ uint16_t mss;
+ uint8_t sack_ok;
+ uint8_t ts_ok;
+ uint8_t mss_ok;
+ uint8_t scale_ok;
+ struct tcp_sack_block *sacks;
+ uint8_t jumbo;
+
+ /* Transmission */
+ uint8_t x_mode;
+ uint8_t dupacks;
+ uint8_t backoff;
+
+};
+
+/* Queues */
+static struct pico_queue tcp_in = {0};
+static struct pico_queue tcp_out = {0};
+
+/* If Nagle enabled, this function can make 1 new segment from smaller segments in hold queue */
+static struct pico_frame * pico_hold_segment_make(struct pico_socket_tcp *t);
+
+/* checks if tcpq_in is empty */
+int pico_tcp_queue_in_is_empty(struct pico_socket *s)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
+
+ if (t->tcpq_in.frames == 0)
+ return 1;
+ else
+ return 0;
+}
+
+/* Useful for getting rid of the beginning of the buffer (read() op) */
+static int release_until(struct pico_tcp_queue *q, uint32_t seq)
+{
+ struct pico_frame *head = first_segment(q);
+ int ret = 0;
+ while (head && (seq_compare(SEQN(head) + head->payload_len, seq) <= 0)) {
+ struct pico_frame *cur = head;
+ head = next_segment(q, cur);
+ tcp_dbg("Releasing %p\n", q);
+ pico_discard_segment(q, cur);
+ ret++;
+ }
+ return ret;
+}
+
+static int release_all_until(struct pico_tcp_queue *q, uint32_t seq)
+{
+ struct pico_frame *f = NULL, *tmp __attribute__((unused));
+ struct pico_tree_node * idx, * temp;
+ int ret = 0;
+
+ pico_tree_foreach_safe(idx,&q->pool,temp){
+ f = idx->keyValue;
+ if (seq_compare(SEQN(f) + f->payload_len, seq) <= 0) {
+ tcp_dbg("Releasing %p\n", f);
+ pico_discard_segment(q, f);
+ ret++;
+ } else
+ return ret;
+ }
+ return ret;
+}
+
+/* API calls */
+
+uint16_t pico_tcp_checksum_ipv4(struct pico_frame *f)
+{
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ struct pico_tcp_hdr *tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ struct pico_socket *s = f->sock;
+ struct pico_ipv4_pseudo_hdr pseudo;
+
+ if (s) {
+ /* Case of outgoing frame */
+ //dbg("TCP CRC: on outgoing frame\n");
+ pseudo.src.addr = s->local_addr.ip4.addr;
+ pseudo.dst.addr = s->remote_addr.ip4.addr;
+ } else {
+ /* Case of incomming frame */
+ //dbg("TCP CRC: on incomming frame\n");
+ pseudo.src.addr = hdr->src.addr;
+ pseudo.dst.addr = hdr->dst.addr;
+ }
+ pseudo.zeros = 0;
+ pseudo.proto = PICO_PROTO_TCP;
+ pseudo.len = short_be(f->transport_len);
+
+ return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv4_pseudo_hdr), tcp_hdr, f->transport_len);
+}
+
+static void tcp_send_fin(struct pico_socket_tcp *t);
+static int pico_tcp_process_out(struct pico_protocol *self, struct pico_frame *f)
+{
+ struct pico_tcp_hdr *hdr;
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)f->sock;
+ IGNORE_PARAMETER(self);
+ hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+ f->sock->timestamp = PICO_TIME_MS();
+ if (f->payload_len > 0) {
+ tcp_dbg("Process out: sending %p (%d bytes)\n",f, f->payload_len);
+ } else {
+ tcp_dbg("Sending empty packet\n");
+ }
+
+ if (f->payload_len > 0) {
+ if (seq_compare(SEQN(f) + f->payload_len, t->snd_nxt) > 0) {
+ t->snd_nxt = SEQN(f) + f->payload_len;
+ tcp_dbg("%s: snd_nxt is now %08x\n", __FUNCTION__, t->snd_nxt);
+ }
+ } else if (hdr->flags == PICO_TCP_ACK) { /* pure ack */
+ //hdr->seq = long_be(t->snd_nxt); /* XXX disabled this to not to mess with seq nrs of ACKs anymore */
+ } else {
+ tcp_dbg("%s: non-pure ACK with len=0, fl:%04x\n", __FUNCTION__, hdr->flags);
+ }
+
+ pico_network_send(f);
+ return 0;
+}
+
+int pico_tcp_push(struct pico_protocol *self, struct pico_frame *data);
+
+/* Interface: protocol definition */
+struct pico_protocol pico_proto_tcp = {
+ .name = "tcp",
+ .proto_number = PICO_PROTO_TCP,
+ .layer = PICO_LAYER_TRANSPORT,
+ .process_in = pico_transport_process_in,
+ .process_out = pico_tcp_process_out,
+ .push = pico_tcp_push,
+ .q_in = &tcp_in,
+ .q_out = &tcp_out,
+};
+
+static uint32_t pico_paws(void)
+{
+ static unsigned long _paws = 0;
+ _paws = pico_rand();
+ return long_be(_paws); /*XXX: implement paws */
+}
+
+static void tcp_add_options(struct pico_socket_tcp *ts, struct pico_frame *f, uint16_t flags, int optsiz)
+{
+ uint32_t tsval = long_be(pico_tick);
+ uint32_t tsecr = long_be(ts->ts_nxt);
+ int i = 0;
+ f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
+
+ memset(f->start, PICO_TCP_OPTION_NOOP, optsiz); /* fill blanks with noop */
+
+ if (flags & PICO_TCP_SYN) {
+ f->start[i++] = PICO_TCP_OPTION_MSS;
+ f->start[i++] = PICO_TCPOPTLEN_MSS;
+ f->start[i++] = (ts->mss >> 8) & 0xFF;
+ f->start[i++] = ts->mss & 0xFF;
+ f->start[i++] = PICO_TCP_OPTION_SACK_OK;
+ f->start[i++] = PICO_TCPOPTLEN_SACK_OK;
+ }
+
+ f->start[i++] = PICO_TCP_OPTION_WS;
+ f->start[i++] = PICO_TCPOPTLEN_WS;
+ f->start[i++] = ts->wnd_scale;
+
+ if (optsiz >= 12) {
+ f->start[i++] = PICO_TCP_OPTION_TIMESTAMP;
+ f->start[i++] = PICO_TCPOPTLEN_TIMESTAMP;
+ memcpy(f->start + i, &tsval, 4);
+ i += 4;
+ memcpy(f->start + i, &tsecr, 4);
+ i += 4;
+ }
+
+ if (flags & PICO_TCP_ACK) {
+ struct tcp_sack_block *sb;
+ int len_off;
+
+ if (ts->sack_ok && ts->sacks) {
+ f->start[i++] = PICO_TCP_OPTION_SACK;
+ len_off = i;
+ f->start[i++] = PICO_TCPOPTLEN_SACK;
+ while(ts->sacks) {
+ sb = ts->sacks;
+ ts->sacks = sb->next;
+ memcpy(f->start + i, sb, 2 * sizeof(uint32_t));
+ i += (2 * sizeof(uint32_t));
+ f->start[len_off] += (2 * sizeof(uint32_t));
+ pico_free(sb);
+ }
+ }
+ }
+ if (i < optsiz)
+ f->start[ optsiz - 1 ] = PICO_TCP_OPTION_END;
+}
+
+static void tcp_send_ack(struct pico_socket_tcp *t);
+
+static void tcp_set_space(struct pico_socket_tcp *t)
+{
+ int mtu, space;
+ int shift = 0;
+
+ mtu = t->mss + PICO_SIZE_TCPHDR + PICO_SIZE_TCPOPT_SYN ;
+ if (t->tcpq_in.max_size == 0) {
+ space = 1024 * 1024 * 1024; /* One Gigabyte, for unlimited sockets. */
+ } else {
+ space = ((t->tcpq_in.max_size - t->tcpq_in.size) / mtu) * t->mss;
+ }
+ if (space < 0)
+ space = 0;
+ while(space > 0xFFFF) {
+ space >>= 1;
+ shift++;
+ }
+ if ((space != t->wnd) || (shift != t->wnd_scale) || ((space - t->wnd) > (space>>2))) {
+ t->wnd = space;
+ t->wnd_scale = shift;
+ }
+}
+
+/* Return 32-bit aligned option size */
+static int tcp_options_size(struct pico_socket_tcp *t, uint16_t flags)
+{
+ int size = 0;
+ struct tcp_sack_block *sb = t->sacks;
+
+ if (flags & PICO_TCP_SYN) { /* Full options */
+ size = PICO_TCPOPTLEN_MSS + PICO_TCP_OPTION_SACK_OK + PICO_TCPOPTLEN_WS + PICO_TCPOPTLEN_TIMESTAMP;
+ } else {
+
+ /* Always update window scale. */
+ size += PICO_TCPOPTLEN_WS;
+
+ if (t->ts_ok)
+ size += PICO_TCPOPTLEN_TIMESTAMP;
+
+ size+= PICO_TCPOPTLEN_END;
+ }
+ if ((flags & PICO_TCP_ACK) && (t->sack_ok && sb)) {
+ size += 2;
+ while(sb) {
+ size += (2 * sizeof(uint32_t));
+ sb = sb->next;
+ }
+ }
+ size = (((size + 3) >> 2) << 2);
+ return size;
+}
+
+int pico_tcp_overhead(struct pico_socket *s)
+{
+ if (!s)
+ return 0;
+
+ return PICO_SIZE_TCPHDR + tcp_options_size((struct pico_socket_tcp *)s, 0); /* hdr + Options size for data pkt */
+
+}
+
+static void tcp_process_sack(struct pico_socket_tcp *t, uint32_t start, uint32_t end)
+{
+ struct pico_frame *f;
+ struct pico_tree_node * index, * temp;
+ int cmp;
+ uint16_t count = 0;
+
+ pico_tree_foreach_safe(index,&t->tcpq_out.pool,temp){
+ f = index->keyValue;
+ cmp = seq_compare(SEQN(f), start);
+ if (cmp > 0)
+ goto done;
+
+ if (cmp == 0) {
+ cmp = seq_compare(SEQN(f) + f->payload_len, end);
+ if (cmp > 0) {
+ tcp_dbg("Invalid SACK: ignoring.\n");
+ }
+
+ tcp_dbg("Marking (by SACK) segment %08x BLK:[%08x::%08x]\n", SEQN(f), start, end);
+ f->flags |= PICO_FRAME_FLAG_SACKED;
+ count++;
+
+ if (cmp == 0) {
+ /* that was last segment sacked. Job done */
+ goto done;
+ }
+ }
+ }
+
+done:
+ if (t->x_mode > PICO_TCP_LOOKAHEAD) {
+ if (t->in_flight > (count))
+ t->in_flight -= (count);
+ else
+ t->in_flight = 0;
+ }
+}
+
+static void tcp_rcv_sack(struct pico_socket_tcp *t, uint8_t *opt, int len)
+{
+ uint32_t start, end;
+ int i = 0;
+ if (len % 8) {
+ tcp_dbg("SACK: Invalid len.\n");
+ return;
+ }
+ while (i < len) {
+ start = long_from(opt + i);
+ i += 4;
+ end = long_from(opt + i);
+ i += 4;
+ tcp_process_sack(t, long_be(start), long_be(end));
+ }
+}
+
+static void tcp_parse_options(struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)f->sock;
+ uint8_t *opt = f->transport_hdr + PICO_SIZE_TCPHDR;
+ uint32_t i = 0;
+ f->timestamp = 0;
+ while (i < (f->transport_len - PICO_SIZE_TCPHDR)) {
+ uint8_t type = opt[i++];
+ uint8_t len;
+ if(i < (f->transport_len - PICO_SIZE_TCPHDR) && (type > 1))
+ len = opt[i++];
+ else
+ len = 1;
+ if (f->payload && ((opt + i) > f->payload))
+ break;
+ tcp_dbg_options("Received option '%d', len = %d \n", type, len);
+ switch (type) {
+ case PICO_TCP_OPTION_NOOP:
+ case PICO_TCP_OPTION_END:
+ break;
+ case PICO_TCP_OPTION_WS:
+ if (len != PICO_TCPOPTLEN_WS) {
+ tcp_dbg_options("TCP Window scale: bad len received (%d).\n", len);
+ i += len - 2;
+ break;
+ }
+ t->recv_wnd_scale = opt[i++];
+ tcp_dbg_options("TCP Window scale: received %d\n", t->recv_wnd_scale);
+ break;
+ case PICO_TCP_OPTION_SACK_OK:
+ if (len != PICO_TCPOPTLEN_SACK_OK) {
+ tcp_dbg_options("TCP option sack: bad len received.\n");
+ i += len - 2;
+ break;
+ }
+ t->sack_ok = 1;
+ break;
+ case PICO_TCP_OPTION_MSS: {
+ uint16_t mss;
+ if (len != PICO_TCPOPTLEN_MSS) {
+ tcp_dbg_options("TCP option mss: bad len received.\n");
+ i += len - 2;
+ break;
+ }
+ t->mss_ok = 1;
+ mss = short_from(opt + i);
+ i += sizeof(uint16_t);
+ if (t->mss > short_be(mss))
+ t->mss = short_be(mss);
+ break;
+ }
+ case PICO_TCP_OPTION_TIMESTAMP: {
+ uint32_t tsval, tsecr;
+ if (len != PICO_TCPOPTLEN_TIMESTAMP) {
+ tcp_dbg_options("TCP option timestamp: bad len received.\n");
+ i += len - 2;
+ break;
+ }
+ t->ts_ok = 1;
+ tsval = long_from(opt + i);
+ i += sizeof(uint32_t);
+ tsecr = long_from(opt + i);
+ f->timestamp = long_be(tsecr);
+ i += sizeof(uint32_t);
+ t->ts_nxt = long_be(tsval);
+ break;
+ }
+ case PICO_TCP_OPTION_SACK:
+ {
+ tcp_rcv_sack(t, opt + i, len - 2);
+ i += len - 2;
+ break;
+ }
+ default:
+ tcp_dbg_options("TCP: received unsupported option %u\n", type);
+ i += len - 2;
+ }
+ }
+}
+
+static int tcp_send(struct pico_socket_tcp *ts, struct pico_frame *f)
+{
+ struct pico_tcp_hdr *hdr= (struct pico_tcp_hdr *) f->transport_hdr;
+ struct pico_frame *cpy;
+ hdr->trans.sport = ts->sock.local_port;
+ hdr->trans.dport = ts->sock.remote_port;
+ if (!hdr->seq)
+ hdr->seq = long_be(ts->snd_nxt);
+
+ if (ts->rcv_nxt != 0) {
+ if ( (ts->rcv_ackd == 0) || (seq_compare(ts->rcv_ackd, ts->rcv_nxt) != 0) || (hdr->flags & PICO_TCP_ACK)) {
+ hdr->flags |= PICO_TCP_ACK;
+ hdr->ack = long_be(ts->rcv_nxt);
+ ts->rcv_ackd = ts->rcv_nxt;
+ }
+ }
+
+ if (hdr->flags & PICO_TCP_SYN) {
+ ts->snd_nxt++;
+ }
+ if (f->payload_len > 0) {
+ hdr->flags |= PICO_TCP_PSH | PICO_TCP_ACK;
+ hdr->ack = long_be(ts->rcv_nxt);
+ ts->rcv_ackd = ts->rcv_nxt;
+ ts->keepalive_timer_running = 2; /* XXX TODO check fix: added 1 to counter to postpone sending keepalive, ACK is in data segments */
+ }
+
+ f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
+ hdr->rwnd = short_be(ts->wnd);
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+
+ /* TCP: ENQUEUE to PROTO ( Transmit ) */
+ cpy = pico_frame_copy(f);
+ if ((pico_enqueue(&tcp_out, cpy) > 0)) {
+ if (f->payload_len > 0) {
+ ts->in_flight++;
+ ts->snd_nxt += f->payload_len; /* update next pointer here to prevent sending same segment twice when called twice in same tick */
+ }
+ tcp_dbg("DBG> [tcp output] state: %02x --> local port:%d remote port: %d seq: %08x ack: %08x flags: %02x = t_len: %d, hdr: %u payload: %d\n",
+ TCPSTATE(&ts->sock) >> 8, short_be(hdr->trans.sport), short_be(hdr->trans.dport), SEQN(f), ACKN(f), hdr->flags, f->transport_len, (hdr->len & 0xf0) >> 2 , f->payload_len );
+ } else {
+ pico_frame_discard(cpy);
+ }
+ return 0;
+}
+
+//#define PICO_TCP_SUPPORT_SOCKET_STATS
+
+#ifdef PICO_TCP_SUPPORT_SOCKET_STATS
+static void sock_stats(unsigned long when, void *arg)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg;
+ tcp_dbg("STATISTIC> [%lu] socket state: %02x --> local port:%d remote port: %d queue size: %d snd_una: %08x snd_nxt: %08x timer: %d cwnd: %d\n",
+ when, t->sock.state, short_be(t->sock.local_port), short_be(t->sock.remote_port), t->tcpq_out.size, SEQN(first_segment(&t->tcpq_out)), t->snd_nxt, t->timer_running, t->cwnd);
+ pico_timer_add(2000, sock_stats, t);
+}
+#endif
+
+struct pico_socket *pico_tcp_open(void)
+{
+ struct pico_socket_tcp *t = pico_zalloc(sizeof(struct pico_socket_tcp));
+ if (!t)
+ return NULL;
+ t->sock.timestamp = PICO_TIME_MS();
+ t->mss = PICO_TCP_DEFAULT_MSS;
+
+ t->tcpq_in.pool.root = t->tcpq_hold.pool.root = t->tcpq_out.pool.root = &LEAF;
+ t->tcpq_hold.pool.compare = t->tcpq_in.pool.compare = t->tcpq_out.pool.compare = segment_compare;
+
+ t->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ;
+ t->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ;
+ t->tcpq_hold.max_size = 2*PICO_TCP_DEFAULT_MSS;
+
+ /* disable Nagle by default */
+ //t->sock.opt_flags |= (1 << PICO_SOCKET_OPT_TCPNODELAY);
+ /* Nagle is enabled by default */
+ t->sock.opt_flags &= ~(1 << PICO_SOCKET_OPT_TCPNODELAY);
+
+#ifdef PICO_TCP_SUPPORT_SOCKET_STATS
+ pico_timer_add(2000, sock_stats, t);
+#endif
+ tcp_set_space(t);
+
+ return &t->sock;
+}
+
+int pico_tcp_read(struct pico_socket *s, void *buf, int len)
+{
+ struct pico_socket_tcp *t = TCP_SOCK(s);
+ struct pico_frame *f;
+ uint32_t in_frame_off, in_frame_len;
+ int tot_rd_len = 0;
+
+ while (tot_rd_len < len) {
+ /* To be sure we don't have garbage at the beginning */
+ release_until(&t->tcpq_in, t->rcv_processed);
+ f = first_segment(&t->tcpq_in);
+ if (!f) {
+ tcp_set_space(t);
+ goto out;
+ }
+
+ /* Hole at the beginning of data, awaiting retransmissions. */
+ if (seq_compare(t->rcv_processed, SEQN(f)) < 0) {
+ tcp_dbg("TCP> read hole beginning of data, %08x - %08x. rcv_nxt is %08x\n",t->rcv_processed, SEQN(f), t->rcv_nxt);
+ goto out;
+ }
+
+ if(seq_compare(t->rcv_processed, SEQN(f)) > 0) {
+ in_frame_off = t->rcv_processed - SEQN(f);
+ in_frame_len = f->payload_len - in_frame_off;
+ } else {
+ in_frame_off = 0;
+ in_frame_len = f->payload_len;
+ }
+ if ((in_frame_len + tot_rd_len) > (uint32_t)len) {
+ in_frame_len = len - tot_rd_len;
+ }
+
+ if (in_frame_len > f->payload_len - in_frame_off)
+ in_frame_len = f->payload_len - in_frame_off;
+
+ memcpy((uint8_t *)buf + tot_rd_len, f->payload + in_frame_off, in_frame_len);
+ tot_rd_len += in_frame_len;
+ t->rcv_processed += in_frame_len;
+
+ if ((in_frame_len == 0u) || (in_frame_len == (uint32_t)f->payload_len)) {
+ pico_discard_segment(&t->tcpq_in, f);
+ }
+ }
+
+out:
+ tcp_set_space(t);
+ if (t->tcpq_in.size == 0) {
+ s->ev_pending &= (~PICO_SOCK_EV_RD);
+ }
+ return tot_rd_len;
+}
+
+int pico_tcp_initconn(struct pico_socket *s);
+static void initconn_retry(unsigned long when, void *arg)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg;
+ IGNORE_PARAMETER(when);
+ if (TCPSTATE(&t->sock) == PICO_SOCKET_STATE_TCP_SYN_SENT) {
+ if (t->backoff > PICO_TCP_MAX_CONNECT_RETRIES) {
+ tcp_dbg("TCP> Connection timeout. \n");
+ if (t->sock.wakeup)
+ t->sock.wakeup(PICO_SOCK_EV_ERR, &t->sock);
+ return;
+ }
+ tcp_dbg("TCP> SYN retry %d...\n", t->backoff);
+ t->backoff++;
+ pico_tcp_initconn(&t->sock);
+ } else {
+ tcp_dbg("TCP> Connection is already established: no retry needed. good.\n");
+ }
+}
+
+int pico_tcp_initconn(struct pico_socket *s)
+{
+ struct pico_socket_tcp *ts = TCP_SOCK(s);
+ struct pico_frame *syn;
+ struct pico_tcp_hdr *hdr;
+ int opt_len = tcp_options_size(ts, PICO_TCP_SYN);
+
+ syn = s->net->alloc(s->net, PICO_SIZE_TCPHDR + opt_len);
+ if (!syn)
+ return -1;
+ hdr = (struct pico_tcp_hdr *) syn->transport_hdr;
+
+ if (!ts->snd_nxt)
+ ts->snd_nxt = long_be(pico_paws());
+ ts->snd_last = ts->snd_nxt;
+ ts->cwnd = PICO_TCP_IW;
+ ts->ssthresh = 40;
+ syn->sock = s;
+ hdr->seq = long_be(ts->snd_nxt);
+ hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | ts->jumbo;
+ hdr->flags = PICO_TCP_SYN;
+ tcp_set_space(ts);
+ hdr->rwnd = short_be(ts->wnd);
+ tcp_add_options(ts,syn, PICO_TCP_SYN, opt_len);
+ hdr->trans.sport = ts->sock.local_port;
+ hdr->trans.dport = ts->sock.remote_port;
+
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_tcp_checksum_ipv4(syn));
+
+ /* TCP: ENQUEUE to PROTO ( SYN ) */
+ tcp_dbg("Sending SYN... (ports: %d - %d) size: %d\n", short_be(ts->sock.local_port), short_be(ts->sock.remote_port), syn->buffer_len);
+ pico_enqueue(&tcp_out, syn);
+ pico_timer_add(PICO_TCP_SYN_TO << ts->backoff, initconn_retry, ts);
+ return 0;
+}
+
+static int tcp_send_synack(struct pico_socket *s)
+{
+ struct pico_socket_tcp *ts = TCP_SOCK(s);
+ struct pico_frame *synack;
+ struct pico_tcp_hdr *hdr;
+ int opt_len = tcp_options_size(ts, PICO_TCP_SYN | PICO_TCP_ACK);
+
+ synack = s->net->alloc(s->net, PICO_SIZE_TCPHDR + opt_len);
+ if (!synack)
+ return -1;
+ hdr = (struct pico_tcp_hdr *) synack->transport_hdr;
+
+ synack->sock = s;
+ hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | ts->jumbo;
+ hdr->flags = PICO_TCP_SYN | PICO_TCP_ACK;
+ hdr->rwnd = short_be(ts->wnd);
+ hdr->seq = long_be(ts->snd_nxt);
+ ts->rcv_processed = long_be(hdr->seq);
+ ts->snd_last = ts->snd_nxt;
+ tcp_set_space(ts);
+ tcp_add_options(ts,synack, hdr->flags, opt_len);
+ synack->payload_len = 0;
+ synack->timestamp = pico_tick;
+ tcp_send(ts, synack);
+ pico_frame_discard(synack);
+ return 0;
+}
+
+static void tcp_send_empty(struct pico_socket_tcp *t, uint16_t flags)
+{
+ struct pico_frame *f;
+ struct pico_tcp_hdr *hdr;
+ int opt_len = tcp_options_size(t, flags);
+ f = t->sock.net->alloc(t->sock.net, PICO_SIZE_TCPHDR + opt_len);
+ if (!f) {
+ return;
+ }
+ f->sock = &t->sock;
+ hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo;
+ hdr->flags = flags;
+ hdr->rwnd = short_be(t->wnd);
+ tcp_set_space(t);
+ tcp_add_options(t,f, flags, opt_len);
+ hdr->trans.sport = t->sock.local_port;
+ hdr->trans.dport = t->sock.remote_port;
+ hdr->seq = long_be(t->snd_nxt);
+ if ((flags & PICO_TCP_ACK) != 0)
+ hdr->ack = long_be(t->rcv_nxt);
+ t->rcv_ackd = t->rcv_nxt;
+
+ f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
+ hdr->rwnd = short_be(t->wnd);
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+
+ /* TCP: ENQUEUE to PROTO */
+ pico_enqueue(&tcp_out, f);
+}
+
+static void tcp_send_ack(struct pico_socket_tcp *t)
+{
+ return tcp_send_empty(t, PICO_TCP_ACK);
+}
+
+static int tcp_send_rst(struct pico_socket *s, struct pico_frame *fr)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
+ struct pico_frame *f;
+ struct pico_tcp_hdr *hdr, *hdr_rcv;
+ int opt_len = tcp_options_size(t, PICO_TCP_RST);
+ int close;
+
+ tcp_dbg("TCP SEND_RST >>>>>>>>>>>>>>> START\n");
+
+ f = t->sock.net->alloc(t->sock.net, PICO_SIZE_TCPHDR + opt_len);
+
+ if (!f) {
+ return -1;
+ }
+
+ hdr_rcv = (struct pico_tcp_hdr *) fr->transport_hdr;
+
+ f->sock = &t->sock;
+ hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo;
+ hdr->flags = PICO_TCP_RST;
+ hdr->rwnd = short_be(t->wnd);
+ tcp_set_space(t);
+ tcp_add_options(t,f, PICO_TCP_RST, opt_len);
+ hdr->trans.sport = t->sock.local_port;
+ hdr->trans.dport = t->sock.remote_port;
+ hdr->seq = long_be(t->snd_nxt);
+
+ /* check if state is synchronized */
+ if (((s->state & PICO_SOCKET_STATE_TCP) > PICO_SOCKET_STATE_TCP_SYN_RECV)) {
+ /* in synchronized state: send RST with seq = ack from previous segment */
+ hdr->seq = hdr_rcv->ack;
+ close = 0;
+ } else {
+ /* non-synchronized state */
+ /* go to CLOSED here to prevent timer callback to go on after timeout */
+ (t->sock).state &= 0x00FFU;
+ (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
+ close = 1;
+ }
+
+ hdr->ack = long_be(t->rcv_nxt);
+ t->rcv_ackd = t->rcv_nxt;
+ f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
+ hdr->rwnd = short_be(t->wnd);
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+
+ /* TCP: ENQUEUE to PROTO */
+ pico_enqueue(&tcp_out, f);
+
+ /* goto CLOSED */
+ if (close) {
+ (t->sock).state &= 0xFF00U;
+ (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
+
+ /* call EV_FIN wakeup before deleting */
+ if ((t->sock).wakeup)
+ (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock));
+
+ /* delete socket */
+ pico_socket_del(&t->sock);
+
+ tcp_dbg("TCP SEND_RST >>>>>>>>>>>>>>> DONE, deleted socket\n");
+ }
+
+ return 0;
+}
+
+int pico_tcp_reply_rst(struct pico_frame *fr)
+{
+ struct pico_tcp_hdr *hdr;
+ struct pico_frame *f;
+ int size = PICO_SIZE_TCPHDR;
+
+ tcp_dbg("TCP>>>>>>>>>>>>>>>> sending RST ... <<<<<<<<<<<<<<<<<<\n");
+
+ f = fr->sock->net->alloc(fr->sock->net, size);
+
+ /* fill in IP data from original frame */
+ // TODO if IPv4
+ ((struct pico_ipv4_hdr *)(f->net_hdr))->dst.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->src.addr;
+ ((struct pico_ipv4_hdr *)(f->net_hdr))->src.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->dst.addr;
+
+ /* fill in TCP data from original frame */
+ ((struct pico_tcp_hdr *)(f->transport_hdr))->trans.dport = ((struct pico_tcp_hdr *)(fr->transport_hdr))->trans.sport;
+ ((struct pico_tcp_hdr *)(f->transport_hdr))->trans.sport = ((struct pico_tcp_hdr *)(fr->transport_hdr))->trans.dport;
+ hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ hdr->len = size << 2;
+ hdr->flags = PICO_TCP_RST | PICO_TCP_ACK;
+ hdr->rwnd = 0;
+ if (((struct pico_tcp_hdr *)(fr->transport_hdr))->flags & PICO_TCP_ACK) {
+ hdr->seq = ((struct pico_tcp_hdr *)(fr->transport_hdr))->ack;
+ } else {
+ hdr->seq = 0U;
+ }
+
+ hdr->ack = ((struct pico_tcp_hdr *)(fr->transport_hdr))->seq + short_be(fr->payload_len);
+
+ /* enqueue for transmission */
+ pico_ipv4_frame_push(f,&(((struct pico_ipv4_hdr *)(f->net_hdr))->dst),PICO_PROTO_TCP);
+
+ return 0;
+}
+
+static int tcp_nosync_rst(struct pico_socket *s, struct pico_frame *fr)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
+ struct pico_frame *f;
+ struct pico_tcp_hdr *hdr, *hdr_rcv;
+ int opt_len = tcp_options_size(t, PICO_TCP_RST | PICO_TCP_ACK);
+
+ tcp_dbg("TCP SEND RST (NON-SYNC) >>>>>>>>>>>>>>>>>> state %x\n",(s->state & PICO_SOCKET_STATE_TCP));
+ if (((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_LISTEN)) {
+ /* XXX TODO NOTE: to prevent the parent socket from trying to send, because this socket has no knowledge of dst IP !!! */
+ return pico_tcp_reply_rst(fr);
+ }
+
+ /***************************************************************************/
+ /* sending RST */
+ f = t->sock.net->alloc(t->sock.net, PICO_SIZE_TCPHDR + opt_len);
+
+ if (!f) {
+ return -1;
+ }
+
+ hdr_rcv = (struct pico_tcp_hdr *) fr->transport_hdr;
+
+ f->sock = &t->sock;
+ hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo;
+ hdr->flags = PICO_TCP_RST | PICO_TCP_ACK;
+ hdr->rwnd = short_be(t->wnd);
+ tcp_set_space(t);
+ tcp_add_options(t,f, PICO_TCP_RST | PICO_TCP_ACK, opt_len);
+ hdr->trans.sport = t->sock.local_port;
+ hdr->trans.dport = t->sock.remote_port;
+
+ /* non-synchronized state */
+ if (hdr_rcv->flags & PICO_TCP_ACK) {
+ hdr->seq = hdr_rcv->ack;
+ } else {
+ hdr->seq = 0U;
+ }
+
+ hdr->ack = hdr_rcv->seq + short_be(fr->payload_len);
+
+ t->rcv_ackd = t->rcv_nxt;
+ f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
+ hdr->rwnd = short_be(t->wnd);
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+
+ /* TCP: ENQUEUE to PROTO */
+ pico_enqueue(&tcp_out, f);
+
+ /***************************************************************************/
+
+ tcp_dbg("TCP SEND_RST (NON_SYNC) >>>>>>>>>>>>>>> DONE, ...\n");
+
+ return 0;
+}
+
+static void tcp_send_fin(struct pico_socket_tcp *t)
+{
+ struct pico_frame *f;
+ struct pico_tcp_hdr *hdr;
+ int opt_len = tcp_options_size(t, PICO_TCP_FIN);
+ f = t->sock.net->alloc(t->sock.net, PICO_SIZE_TCPHDR + opt_len);
+ if (!f) {
+ return;
+ }
+ f->sock = &t->sock;
+ hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo;
+ hdr->flags = PICO_TCP_FIN | PICO_TCP_ACK;
+ hdr->ack = long_be(t->rcv_nxt);
+ t->rcv_ackd = t->rcv_nxt;
+ hdr->rwnd = short_be(t->wnd);
+ tcp_set_space(t);
+ tcp_add_options(t,f, PICO_TCP_FIN, opt_len);
+ hdr->trans.sport = t->sock.local_port;
+ hdr->trans.dport = t->sock.remote_port;
+ hdr->seq = long_be(t->snd_nxt); /* XXX TODO check correct ?? --> snd_last? otherwise maybe data after FIN */
+
+ f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
+ hdr->rwnd = short_be(t->wnd);
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+ //tcp_dbg("SENDING FIN...\n");
+ /* TCP: ENQUEUE to PROTO ( Pure ACK ) */
+ pico_enqueue(&tcp_out, f);
+ t->snd_nxt++;
+}
+
+static void tcp_sack_prepare(struct pico_socket_tcp *t)
+{
+ struct pico_frame *pkt;
+ uint32_t left=0, right=0;
+ struct tcp_sack_block *sb;
+ int n = 0;
+ if (t->sacks) /* previous sacks are pending */
+ return;
+
+ pkt = first_segment(&t->tcpq_in);
+ while(n < 3) {
+ if (!pkt) {
+ if(left) {
+ sb = pico_zalloc(sizeof(struct tcp_sack_block));
+ if (!sb)
+ break;
+ sb->left = long_be(left);
+ sb->right = long_be(right);
+ n++;
+ sb->next = t->sacks;
+ t->sacks = sb;
+ left = 0;
+ right = 0;
+ }
+ break;
+ }
+ if ((SEQN(pkt) < t->rcv_nxt)) {
+ pkt = next_segment(&t->tcpq_in, pkt);
+ continue;
+ }
+ if (!left) {
+ left = SEQN(pkt);
+ right = SEQN(pkt) + pkt->payload_len;
+ pkt = next_segment(&t->tcpq_in, pkt);
+ continue;
+ }
+ if(SEQN(pkt) == (right + 1)) {
+ right += pkt->payload_len;
+ pkt = next_segment(&t->tcpq_in, pkt);
+ continue;
+ } else {
+ sb = pico_zalloc(sizeof(struct tcp_sack_block));
+ if (!sb)
+ break;
+ sb->left = long_be(left);
+ sb->right = long_be(right);
+ n++;
+ sb->next = t->sacks;
+ t->sacks = sb;
+ left = 0;
+ right = 0;
+ pkt = next_segment(&t->tcpq_in, pkt);
+ }
+ }
+}
+
+static int tcp_data_in(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+
+ if (((hdr->len & 0xf0) >> 2) <= f->transport_len) {
+ tcp_parse_options(f);
+ f->payload = f->transport_hdr + ((hdr->len & 0xf0) >>2);
+ f->payload_len = f->transport_len - ((hdr->len & 0xf0) >>2);
+ tcp_dbg("TCP> Received segment. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
+
+ if (seq_compare(SEQN(f), t->rcv_nxt) <= 0) {
+ struct pico_frame *nxt;
+ if (seq_compare(SEQN(f), t->rcv_nxt) == 0) { /* Exactly what we expected */
+ struct pico_frame *cpy = pico_frame_copy(f);
+ /* Enqueue: try to put into RCV buffer */
+ if(pico_enqueue_segment(&t->tcpq_in, cpy) <= 0) {
+ pico_frame_discard(cpy);
+ }
+ t->rcv_nxt = SEQN(f) + f->payload_len;
+ nxt = peek_segment(&t->tcpq_in, t->rcv_nxt);
+ while(nxt) {
+ tcp_dbg("scrolling rcv_nxt...%08x\n", t->rcv_nxt);
+ t->rcv_nxt += f->payload_len;
+ nxt = peek_segment(&t->tcpq_in, t->rcv_nxt);
+ }
+ t->sock.ev_pending |= PICO_SOCK_EV_RD;
+ t->rcv_nxt = SEQN(f) + f->payload_len;
+ } else {
+ tcp_dbg("TCP> lo segment. Uninteresting retransmission. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
+ }
+ } else {
+ tcp_dbg("TCP> hi segment. Possible packet loss. I'll dupack this. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
+ if (t->sack_ok) {
+ tcp_sack_prepare(t);
+ }
+ }
+ /* In either case, ack til recv_nxt. */
+ if ( ((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_CLOSE_WAIT) && ((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_SYN_SENT) && ((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_SYN_RECV)) {
+ //tcp_dbg("SENDACK CALLED FROM OUTSIDE tcp_synack, state %x\n",t->sock.state);
+ tcp_send_ack(t);
+ } else {
+ //tcp_dbg("SENDACK PREVENTED IN SYNSENT STATE\n");
+ }
+ return 0;
+ } else {
+ tcp_dbg("TCP: invalid data in pkt len, exp: %d, got %d\n", (hdr->len & 0xf0) >> 2, f->transport_len);
+ return -1;
+ }
+}
+
+static int tcp_ack_advance_una(struct pico_socket_tcp *t, struct pico_frame *f)
+{
+ int ret = release_all_until(&t->tcpq_out, ACKN(f));
+ if (ret > 0)
+ t->sock.ev_pending |= PICO_SOCK_EV_WR;
+ return ret;
+}
+
+static uint16_t time_diff(unsigned long a, unsigned long b)
+{
+ if (a >= b)
+ return (a - b);
+ else
+ return (b - a);
+}
+
+static void tcp_rtt(struct pico_socket_tcp *t, uint32_t rtt)
+{
+
+ uint32_t avg = t->avg_rtt;
+ uint32_t rvar = t->rttvar;
+ if (!avg) {
+ /* This follows RFC2988
+ * (2.2) When the first RTT measurement R is made, the host MUST set
+ *
+ * SRTT <- R
+ * RTTVAR <- R/2
+ * RTO <- SRTT + max (G, K*RTTVAR)
+ */
+ t->avg_rtt = rtt;
+ t->rttvar = rtt >> 1;
+ t->rto = t->avg_rtt + (t->rttvar << 4);
+ } else {
+ int var = (t->avg_rtt - rtt);
+ if (var < 0)
+ var = 0-var;
+ /* RFC2988, section (2.3). Alpha and beta are the ones suggested. */
+
+ /* First, evaluate a new value for the rttvar */
+ t->rttvar <<= 2;
+ t->rttvar -= rvar;
+ t->rttvar += var;
+ t->rttvar >>= 2;
+
+ /* Then, calculate the new avg_rtt */
+ t->avg_rtt <<= 3;
+ t->avg_rtt -= avg;
+ t->avg_rtt += rtt;
+ t->avg_rtt >>= 3;
+
+ /* Finally, assign a new value for the RTO, as specified in the RFC, with K=4 */
+ t->rto = t->avg_rtt + (t->rttvar << 2);
+ }
+ tcp_dbg(" -----=============== RTT CUR: %u AVG: %u RTTVAR: %u RTO: %u ======================----\n", rtt, t->avg_rtt, t->rttvar, t->rto);
+}
+
+static void tcp_congestion_control(struct pico_socket_tcp *t)
+{
+ if (t->x_mode > PICO_TCP_LOOKAHEAD)
+ return;
+ if (t->cwnd > t->tcpq_out.frames) {
+ tcp_dbg("Limited by app: %d\n", t->cwnd);
+ if (t->sock.wakeup)
+ t->sock.wakeup(PICO_SOCK_EV_WR, &t->sock);
+ return;
+ }
+ tcp_dbg("Doing congestion control\n");
+ if (t->cwnd < t->ssthresh) {
+ t->cwnd++;
+ } else {
+ t->cwnd_counter++;
+ if (t->cwnd_counter >= t->cwnd) {
+ t->cwnd++;
+ t->cwnd_counter = 0;
+ }
+ }
+ tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", pico_tick, t->cwnd, t->ssthresh, t->in_flight);
+}
+
+static void add_retransmission_timer(struct pico_socket_tcp *t, unsigned long next_ts);
+static void tcp_retrans_timeout(unsigned long val, void *sock)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *) sock;
+ struct pico_frame *f = NULL;
+ unsigned long limit = val - t->rto;
+ struct pico_tcp_hdr *hdr;
+ if( t->sock.net && ((t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_ESTABLISHED
+ || (t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_CLOSE_WAIT) )
+ {
+ tcp_dbg("TIMEOUT! backoff = %d\n", t->backoff);
+ /* was timer cancelled? */
+ if (t->timer_running == 0) {
+ add_retransmission_timer(t, 0);
+ return;
+ }
+ t->timer_running--;
+
+ f = first_segment(&t->tcpq_out);
+ while (f) {
+ if ((t->x_mode == PICO_TCP_WINDOW_FULL) ||
+ ((f->timestamp != 0) && (f->timestamp <= limit))) {
+ struct pico_frame *cpy;
+ hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+ tcp_dbg("TCP BLACKOUT> TIMED OUT (output) frame %08x, len= %d rto=%d Win full: %d frame flags: %04x\n", SEQN(f), f->payload_len, t->rto, t->x_mode == PICO_TCP_WINDOW_FULL, f->flags);
+ if ((t->x_mode != PICO_TCP_WINDOW_FULL) ) {
+ t->x_mode = PICO_TCP_BLACKOUT;
+ tcp_dbg("Mode: Blackout.\n");
+ t->cwnd = PICO_TCP_IW;
+ t->in_flight = 0;
+ }
+ f->timestamp = pico_tick;
+ tcp_add_options(t, f, 0, f->transport_len - f->payload_len - PICO_SIZE_TCPHDR);
+ hdr->rwnd = short_be(t->wnd);
+ hdr->flags |= PICO_TCP_PSH;
+ hdr->ack = long_be(t->rcv_nxt);
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+ /* TCP: ENQUEUE to PROTO ( retransmit )*/
+ cpy = pico_frame_copy(f);
+ if (pico_enqueue(&tcp_out, cpy) > 0) {
+ t->backoff++;
+ add_retransmission_timer(t, (t->rto << t->backoff) + pico_tick);
+ tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", pico_tick, t->cwnd, t->ssthresh, t->in_flight);
+ return;
+ } else {
+ add_retransmission_timer(t, (t->rto << t->backoff) + pico_tick);
+ pico_frame_discard(cpy);
+ }
+ }
+ f = next_segment(&t->tcpq_out, f);
+ }
+ t->backoff = 0;
+ add_retransmission_timer(t, 0);
+ if (t->tcpq_out.size < t->tcpq_out.max_size)
+ t->sock.ev_pending |= PICO_SOCK_EV_WR;
+ return;
+ }
+}
+
+static void add_retransmission_timer(struct pico_socket_tcp *t, unsigned long next_ts)
+{
+ struct pico_tree_node * index;
+
+ if (t->timer_running > 0)
+ return;
+
+ if (next_ts == 0) {
+ struct pico_frame *f;
+
+ pico_tree_foreach(index,&t->tcpq_out.pool){
+ f = index->keyValue;
+ if (((next_ts == 0) || (f->timestamp < next_ts)) && (f->timestamp > 0)) {
+ next_ts = f->timestamp;
+ }
+ }
+ }
+ if (next_ts > 0) {
+ if ((next_ts + t->rto) > pico_tick) {
+ pico_timer_add(next_ts + t->rto - pico_tick, tcp_retrans_timeout, t);
+ } else {
+ pico_timer_add(1, tcp_retrans_timeout, t);
+ }
+ t->timer_running++;
+ }
+}
+
+static int tcp_retrans(struct pico_socket_tcp *t, struct pico_frame *f)
+{
+ struct pico_frame *cpy;
+ struct pico_tcp_hdr *hdr;
+ if (f) {
+ hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+ tcp_dbg("TCP> RETRANS (by dupack) frame %08x, len= %d\n", SEQN(f), f->payload_len);
+ f->timestamp = pico_tick;
+ tcp_add_options(t, f, 0, f->transport_len - f->payload_len - PICO_SIZE_TCPHDR);
+ hdr->rwnd = short_be(t->wnd);
+ hdr->flags |= PICO_TCP_PSH;
+ hdr->ack = long_be(t->rcv_nxt);
+ hdr->crc = 0;
+ hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+ /* TCP: ENQUEUE to PROTO ( retransmit )*/
+ cpy = pico_frame_copy(f);
+ if (pico_enqueue(&tcp_out, cpy) > 0) {
+ t->in_flight++;
+ t->snd_last_out = SEQN(cpy);
+ add_retransmission_timer(t, pico_tick + t->rto);
+ } else {
+ pico_frame_discard(cpy);
+ }
+ return(f->payload_len);
+ }
+ return 0;
+}
+
+#ifdef TCP_ACK_DBG
+static void tcp_ack_dbg(struct pico_socket *s, struct pico_frame *f)
+{
+ uint32_t una, nxt, ack, cur;
+ struct pico_frame *una_f = NULL, *cur_f;
+ struct pico_tree_node *idx;
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ char info[64];
+ char tmp[64];
+ ack = ACKN(f);
+ nxt = t->snd_nxt;
+ tcp_dbg("===================================\n");
+ tcp_dbg("Queue out (%d/%d). ACKED=%08x\n", t->tcpq_out.size, t->tcpq_out.max_size, ack);
+
+ pico_tree_foreach(idx, &t->tcpq_out.pool) {
+ info[0] = 0;
+ cur_f = idx->keyValue;
+ cur = SEQN(cur_f);
+ if (!una_f) {
+ una_f = cur_f;
+ una = SEQN(una_f);
+ }
+
+ if (cur == nxt) {
+ strncpy(tmp, info, strlen(info));
+ snprintf(info,64, "%s SND_NXT", tmp);
+ }
+ if (cur == ack) {
+ strncpy(tmp, info, strlen(info));
+ snprintf(info,64, "%s ACK", tmp);
+ }
+ if (cur == una) {
+ strncpy(tmp, info, strlen(info));
+ snprintf(info,64, "%s SND_UNA", tmp);
+ }
+ if (cur == t->snd_last) {
+ strncpy(tmp, info, strlen(info));
+ snprintf(info,64, "%s SND_LAST", tmp);
+ }
+ tcp_dbg("%08x %d%s\n", cur, cur_f->payload_len, info);
+
+ }
+ tcp_dbg("SND_NXT is %08x, snd_LAST is %08x\n", nxt, t->snd_last);
+ tcp_dbg("===================================\n");
+ tcp_dbg("\n\n");
+}
+#endif
+
+static int tcp_ack(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_frame *f_new; /* use with Nagle to push to out queue */
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ uint32_t rtt = 0;
+ uint16_t acked = 0;
+ struct pico_frame *una = NULL;
+ if ((hdr->flags & PICO_TCP_ACK) == 0)
+ return -1;
+
+#ifdef TCP_ACK_DBG
+ tcp_ack_dbg(s,f);
+#endif
+
+ tcp_parse_options(f);
+ t->recv_wnd = short_be(hdr->rwnd);
+
+ acked = tcp_ack_advance_una(t, f);
+ una = first_segment(&t->tcpq_out);
+
+ if ((t->x_mode == PICO_TCP_BLACKOUT) ||
+ ((t->x_mode == PICO_TCP_WINDOW_FULL) && ((t->recv_wnd << t->recv_wnd_scale) > t->mss))) {
+ tcp_dbg("Re-entering look-ahead...\n\n\n");
+ t->x_mode = PICO_TCP_LOOKAHEAD;
+ t->backoff = 0;
+ }
+
+ /* One should be acked. */
+// if ((acked == 0) && (t->in_flight > 0))
+ if ((acked == 0) && (f->payload_len == 0) && (t->in_flight > 0))
+ t->in_flight--;
+ if (!una || acked > 0) {
+ t->x_mode = PICO_TCP_LOOKAHEAD;
+ tcp_dbg("Mode: Look-ahead. In flight: %d/%d buf: %d\n", t->in_flight, t->cwnd, t->tcpq_out.frames);
+ t->backoff = 0;
+
+ /* Do rtt/rttvar/rto calculations */
+ /* First, try with timestamps, using the value from options */
+ if(f && (f->timestamp != 0)) {
+ rtt = time_diff(pico_tick, f->timestamp);
+ if (rtt)
+ tcp_rtt(t, rtt);
+ } else if(una && (una->timestamp != 0)) {
+ /* If no timestamps are there, use conservatve estimation on the una */
+ rtt = time_diff(pico_tick, una->timestamp);
+ if (rtt)
+ tcp_rtt(t, rtt);
+ }
+
+ tcp_dbg("TCP ACK> FRESH ACK %08x (acked %d) Queue size: %u/%u frames: %u cwnd: %u in_flight: %u snd_una: %u\n", ACKN(f), acked, t->tcpq_out.size, t->tcpq_out.max_size, t->tcpq_out.frames, t->cwnd, t->in_flight, SEQN(una));
+ if (acked > t->in_flight) {
+ tcp_dbg("WARNING: in flight < 0\n");
+ t->in_flight = 0;
+ } else
+ t->in_flight -= (acked);
+
+ } else if ((t->snd_old_ack == ACKN(f)) && /* We've just seen this ack, and... */
+ ((0 == (hdr->flags & (PICO_TCP_PSH | PICO_TCP_SYN))) &&
+ (f->payload_len == 0)) && /* This is a pure ack, and... */
+ (ACKN(f) != t->snd_nxt)) /* There is something in flight awaiting to be acked... */
+ {
+ /* Process incoming duplicate ack. */
+ if (t->x_mode < PICO_TCP_RECOVER) {
+ t->x_mode++;
+ tcp_dbg("Mode: DUPACK %d, due to PURE ACK %0x, len = %d\n", t->x_mode, SEQN(f), f->payload_len);
+ tcp_dbg("ACK: %x - QUEUE: %x\n",ACKN(f), SEQN(first_segment(&t->tcpq_out)));
+ if (t->x_mode == PICO_TCP_RECOVER) { /* Switching mode */
+ t->snd_retry = SEQN(first_segment(&t->tcpq_out));
+ if (t->ssthresh > t->cwnd)
+ t->ssthresh >>=2;
+ else
+ t->ssthresh = (t->cwnd >> 1);
+ if (t->ssthresh < 2)
+ t->ssthresh = 2;
+ }
+ } else if (t->x_mode == PICO_TCP_RECOVER) {
+ tcp_dbg("TCP RECOVER> DUPACK! snd_una: %08x, snd_nxt: %08x, acked now: %08x\n", SEQN(first_segment(&t->tcpq_out)), t->snd_nxt, ACKN(f));
+ if (t->in_flight <= t->cwnd) {
+ struct pico_frame *nxt = peek_segment(&t->tcpq_out, t->snd_retry);
+ if (!nxt)
+ nxt = first_segment(&t->tcpq_out);
+
+ while (nxt && (nxt->flags & PICO_FRAME_FLAG_SACKED) && (nxt != first_segment(&t->tcpq_out))) {
+ tcp_dbg("Skipping %08x because it is sacked.\n", SEQN(nxt));
+ nxt = next_segment(&t->tcpq_out, nxt);
+ }
+
+ if (nxt && (seq_compare(SEQN(nxt), t->snd_nxt)) > 0)
+ nxt = NULL;
+ if (nxt && (seq_compare(SEQN(nxt), SEQN(first_segment(&t->tcpq_out))) > (t->recv_wnd << t->recv_wnd_scale)))
+ nxt = NULL;
+
+ if(!nxt)
+ nxt = first_segment(&t->tcpq_out);
+ if (nxt) {
+ tcp_retrans(t, peek_segment(&t->tcpq_out, t->snd_retry));
+ t->snd_retry = SEQN(nxt);
+ }
+ }
+
+ if (++t->cwnd_counter > 1) {
+ t->cwnd--;
+ if (t->cwnd < 2)
+ t->cwnd = 2;
+ t->cwnd_counter = 0;
+ }
+ } else {
+ tcp_dbg("DUPACK in mode %d \n", t->x_mode);
+
+ }
+ } /* End case duplicate ack detection */
+
+ /* Do congestion control */
+ tcp_congestion_control(t);
+ if ((acked > 0) && t->sock.wakeup) {
+ if (t->tcpq_out.size < t->tcpq_out.max_size)
+ t->sock.wakeup(PICO_SOCK_EV_WR, &(t->sock));
+ //t->sock.ev_pending |= PICO_SOCK_EV_WR;
+ }
+
+ /* if Nagle enabled, check if no unack'ed data and fill out queue (till window) */
+ if (IS_NAGLE_ENABLED((&(t->sock)))) {
+ while (!IS_TCP_HOLDQ_EMPTY(t) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= PICO_TCP_DEFAULT_MSS)) {
+ tcp_dbg_nagle("TCP_ACK - NAGLE add new segment\n");
+ f_new = pico_hold_segment_make(t);
+ if (f_new == NULL)
+ break; /* XXX corrupt !!! (or no memory) */
+ if (pico_enqueue_segment(&t->tcpq_out,f_new) <= 0)
+ // handle error
+ tcp_dbg_nagle("TCP_ACK - NAGLE FAILED to enqueue in out\n");
+ }
+ }
+
+ /* If some space was created, put a few segments out. */
+ tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", pico_tick, t->cwnd, t->ssthresh, t->in_flight);
+ if (t->x_mode == PICO_TCP_LOOKAHEAD) {
+ if ((t->cwnd >= t->in_flight) && (t->snd_nxt > t->snd_last_out)) {
+ pico_tcp_output(&t->sock, t->cwnd - t->in_flight);
+ }
+ }
+
+ t->snd_old_ack = ACKN(f);
+ return 0;
+}
+
+static int tcp_finwaitack(struct pico_socket *s, struct pico_frame *f)
+{
+ tcp_dbg("RECEIVED ACK IN FIN_WAIT1\n");
+ tcp_dbg("TCP> IN STATE FIN_WAIT2\n");
+
+ /* acking part */
+ tcp_ack(s,f);
+ /* update TCP state */
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_FIN_WAIT2;
+
+ return 0;
+}
+
+static void tcp_deltcb(unsigned long when, void *arg)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg;
+ IGNORE_PARAMETER(when);
+
+ if (TCPSTATE(&t->sock) == PICO_SOCKET_STATE_TCP_TIME_WAIT) {
+ tcp_dbg("TCP> state: time_wait, final timer expired, going to closed state\n");
+ /* update state */
+ (t->sock).state &= 0x00FFU;
+ (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
+ (t->sock).state &= 0xFF00U;
+ (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
+ /* call EV_FIN wakeup before deleting */
+ if (t->sock.wakeup) {
+ (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock));
+ }
+ /* delete socket */
+ pico_socket_del(&t->sock);
+ } else {
+ tcp_dbg("TCP> trying to go to closed, wrong state\n");
+ }
+}
+
+static int tcp_finwaitfin(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr);
+ tcp_dbg("TCP> received fin in FIN_WAIT2\n");
+ /* received FIN, increase ACK nr */
+ t->rcv_nxt = long_be(hdr->seq) + 1;
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT;
+ /* set SHUT_REMOTE */
+ s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
+ if (s->wakeup)
+ s->wakeup(PICO_SOCK_EV_CLOSE, s);
+ if (f->payload_len > 0) /* needed?? */
+ tcp_data_in(s,f);
+ /* send ACK */
+ tcp_send_ack(t);
+ /* set timer */
+ pico_timer_add(200, tcp_deltcb, t);
+ return 0;
+}
+
+static int tcp_closewaitack(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ tcp_dbg("TCP> received ack in CLOSING\n");
+ /* acking part */
+ tcp_ack(s,f);
+ /* update TCP state */
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT;
+ /* set timer */
+ pico_timer_add(200, tcp_deltcb, t);
+ return 0;
+}
+
+static int tcp_lastackwait(struct pico_socket *s, struct pico_frame *f)
+{
+ IGNORE_PARAMETER(f);
+ tcp_dbg("TCP> state: last_ack, received ack, to closed\n");
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_CLOSED;
+ s->state &= 0xFF00U;
+ s->state |= PICO_SOCKET_STATE_CLOSED;
+ /* call socket wakeup with EV_FIN */
+ if (s->wakeup)
+ s->wakeup(PICO_SOCK_EV_FIN, s);
+ /* delete socket */
+ pico_socket_del(s);
+ return 0;
+}
+
+static int tcp_syn(struct pico_socket *s, struct pico_frame *f)
+{
+ /* TODO: Check against backlog length */
+ struct pico_socket_tcp *new = (struct pico_socket_tcp *)pico_socket_clone(s);
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+ if (!new)
+ return -1;
+
+#ifdef PICO_TCP_SUPPORT_SOCKET_STATS
+ pico_timer_add(2000, sock_stats, s);
+#endif
+
+ new->sock.remote_port = ((struct pico_trans *)f->transport_hdr)->sport;
+#ifdef PICO_SUPPORT_IPV4
+ if (IS_IPV4(f)) {
+ new->sock.remote_addr.ip4.addr = ((struct pico_ipv4_hdr *)(f->net_hdr))->src.addr;
+ new->sock.local_addr.ip4.addr = ((struct pico_ipv4_hdr *)(f->net_hdr))->dst.addr;
+ }
+#endif
+#ifdef PICO_SUPPORT_IPV6
+ if (IS_IPV6(f)) {
+ new->sock.remote_addr.ip6 = ((struct pico_ipv6_hdr *)(f->net_hdr))->src;
+ new->sock.local_addr.ip6 = ((struct pico_ipv6_hdr *)(f->net_hdr))->dst;
+ }
+#endif
+
+ /* Set socket limits */
+ new->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ;
+ new->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ;
+ new->tcpq_hold.max_size = 2*PICO_TCP_DEFAULT_MSS;
+
+ f->sock = &new->sock;
+ tcp_parse_options(f);
+ new->mss = PICO_TCP_DEFAULT_MSS;
+ new->rcv_nxt = long_be(hdr->seq) + 1;
+ new->snd_nxt = long_be(pico_paws());
+ new->snd_last = new->snd_nxt;
+ new->cwnd = PICO_TCP_IW;
+ new->ssthresh = 40;
+ new->recv_wnd = short_be(hdr->rwnd);
+ new->jumbo = hdr->len & 0x07;
+ new->sock.parent = s;
+ new->sock.wakeup = s->wakeup;
+ /* Initialize timestamp values */
+ new->sock.state = PICO_SOCKET_STATE_BOUND | PICO_SOCKET_STATE_CONNECTED | PICO_SOCKET_STATE_TCP_SYN_RECV;
+ pico_socket_add(&new->sock);
+ tcp_send_synack(&new->sock);
+ tcp_dbg("SYNACK sent, socket added. snd_nxt is %08x\n", new->snd_nxt);
+ return 0;
+}
+
+static void tcp_set_init_point(struct pico_socket *s)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ t->rcv_processed = t->rcv_nxt;
+}
+
+static int tcp_synack(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+
+ if (ACKN(f) == (1 + t->snd_nxt)) {
+ t->rcv_nxt = long_be(hdr->seq);
+ t->rcv_processed = t->rcv_nxt + 1;
+ tcp_ack(s, f);
+
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_ESTABLISHED;
+ tcp_dbg("TCP> Established. State: %x\n", s->state);
+
+ if (s->wakeup)
+ s->wakeup(PICO_SOCK_EV_CONN, s);
+ s->ev_pending |= PICO_SOCK_EV_WR;
+
+ t->rcv_nxt++;
+ t->snd_nxt++;
+ tcp_send_ack(t); /* return ACK */
+
+ return 0;
+
+ } else {
+ tcp_dbg("TCP> Not established, RST sent.\n");
+ tcp_nosync_rst(s,f);
+ return 0;
+ }
+}
+
+static int tcp_first_ack(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ tcp_dbg("ACK in SYN_RECV: expecting %08x got %08x\n", t->snd_nxt, ACKN(f));
+ if (t->snd_nxt == ACKN(f)) {
+ tcp_set_init_point(s);
+ tcp_ack(s, f);
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_ESTABLISHED;
+ tcp_dbg("TCP: Established. State now: %04x\n", s->state);
+ if( !s->parent && s->wakeup) { /* If the socket has no parent, -> sending socket that has a sim_open */
+ tcp_dbg("FIRST ACK - No parent found -> sending socket\n");
+ s->wakeup(PICO_SOCK_EV_CONN, s);
+ }
+ if (s->parent && s->parent->wakeup) {
+ tcp_dbg("FIRST ACK - Parent found -> listening socket\n");
+ s->wakeup = s->parent->wakeup;
+ s->parent->wakeup(PICO_SOCK_EV_CONN, s->parent);
+ }
+ s->ev_pending |= PICO_SOCK_EV_WR;
+ tcp_dbg("%s: snd_nxt is now %08x\n", __FUNCTION__, t->snd_nxt);
+ return 0;
+ } else {
+ tcp_nosync_rst(s,f);
+ return 0;
+ }
+}
+
+static int tcp_closewait(struct pico_socket *s, struct pico_frame *f)
+{
+
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr);
+
+
+ if (f->payload_len > 0)
+ tcp_data_in(s,f);
+ if (f->flags & PICO_TCP_ACK)
+ tcp_ack(s,f);
+ if (seq_compare(SEQN(f), t->rcv_nxt) == 0) {
+ /* received FIN, increase ACK nr */
+ t->rcv_nxt = long_be(hdr->seq) + 1;
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_CLOSE_WAIT;
+ /* set SHUT_REMOTE */
+ s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
+ tcp_dbg("TCP> Close-wait\n");
+
+ if (s->wakeup){
+ if(f->payload_len>0){
+ struct pico_socket_tcp *_t = (struct pico_socket_tcp *)s;
+ _t->sock.ev_pending |=PICO_SOCK_EV_CLOSE;
+ }else
+ s->wakeup(PICO_SOCK_EV_CLOSE, s);
+ }
+ } else {
+ tcp_send_ack(t); /* return ACK */
+ }
+ return 0;
+}
+
+/*static int tcp_fin(struct pico_socket *s, struct pico_frame *f)
+{
+ return 0;
+}*/
+
+static int tcp_rcvfin(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ IGNORE_PARAMETER(f);
+ tcp_dbg("TCP> Received FIN in FIN_WAIT1\n");
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_CLOSING;
+ t->rcv_processed = t->rcv_nxt + 1;
+ t->rcv_nxt++;
+ /* send ACK */
+ tcp_send_ack(t);
+ return 0;
+}
+
+static int tcp_finack(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ IGNORE_PARAMETER(f);
+
+ tcp_dbg("TCP> ENTERED finack\n");
+ t->rcv_nxt++;
+ /* send ACK */
+ tcp_send_ack(t);
+
+ /* call socket wakeup with EV_FIN */
+ if (s->wakeup)
+ s->wakeup(PICO_SOCK_EV_FIN, s);
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT;
+ /* set SHUT_REMOTE */
+ s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
+ pico_timer_add(2000, tcp_deltcb, t);
+
+ return 0;
+}
+
+static int tcp_rst(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr);
+
+ tcp_dbg("TCP >>>>>>>>>>>>>> received RST <<<<<<<<<<<<<<<<<<<<\n");
+ if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_SYN_SENT) {
+ /* the RST is acceptable if the ACK field acknowledges the SYN */
+ if ((t->snd_nxt + 1) == ACKN(f)) { /* valid, got to closed state */
+ /* update state */
+ (t->sock).state &= 0x00FFU;
+ (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
+ (t->sock).state &= 0xFF00U;
+ (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
+
+ /* call EV_FIN wakeup before deleting */
+ if ((t->sock).wakeup)
+ (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock));
+
+ /* call EV_ERR wakeup before deleting */
+ pico_err = PICO_ERR_ECONNRESET;
+ if ((t->sock).wakeup)
+ (t->sock).wakeup(PICO_SOCK_EV_ERR, &(t->sock));
+
+ /* delete socket */
+ pico_socket_del(&t->sock);
+ } else { /* not valid, ignore */
+ tcp_dbg("TCP RST> IGNORE\n");
+ return 0;
+ }
+ } else { /* all other states */
+ /* all reset (RST) segments are validated by checking their SEQ-fields,
+ a reset is valid if its sequence number is in the window */
+ if ((long_be(hdr->seq) >= t->rcv_ackd) && (long_be(hdr->seq) <= ((short_be(hdr->rwnd)<<(t->wnd_scale)) + t->rcv_ackd))) {
+ if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_SYN_RECV) {
+ /* go to closed */
+ (t->sock).state &= 0x00FFU;
+ (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
+ (t->sock).state &= 0xFF00U;
+ (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
+ /* call EV_ERR wakeup */
+ pico_err = PICO_ERR_ECONNRESET;
+ if ((t->sock).wakeup)
+ (t->sock).wakeup(PICO_SOCK_EV_ERR, &(t->sock));
+ tcp_dbg("TCP RST> SOCKET BACK TO LISTEN\n");
+ pico_socket_del(s);
+ } else {
+ /* go to closed */
+ (t->sock).state &= 0x00FFU;
+ (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
+ (t->sock).state &= 0xFF00U;
+ (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
+
+ /* call EV_FIN wakeup before deleting */
+ if ((t->sock).wakeup)
+ (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock));
+ /* call EV_ERR wakeup before deleting */
+ pico_err = PICO_ERR_ECONNRESET;
+ if ((t->sock).wakeup)
+ (t->sock).wakeup(PICO_SOCK_EV_ERR, &(t->sock));
+
+ /* delete socket */
+ pico_socket_del(&t->sock);
+ }
+ } else { /* not valid, ignore */
+ tcp_dbg("TCP RST> IGNORE\n");
+ return 0;
+ }
+ }
+
+ return 0;
+}
+
+struct tcp_action_entry {
+ uint16_t tcpstate;
+ int (*syn)(struct pico_socket *s, struct pico_frame *f);
+ int (*synack)(struct pico_socket *s, struct pico_frame *f);
+ int (*ack)(struct pico_socket *s, struct pico_frame *f);
+ int (*data)(struct pico_socket *s, struct pico_frame *f);
+ int (*fin)(struct pico_socket *s, struct pico_frame *f);
+ int (*finack)(struct pico_socket *s, struct pico_frame *f);
+ int (*rst)(struct pico_socket *s, struct pico_frame *f);
+};
+
+static struct tcp_action_entry tcp_fsm[] = {
+ /* State syn synack ack data fin finack rst*/
+ { PICO_SOCKET_STATE_TCP_UNDEF, NULL, NULL, NULL, NULL, NULL, NULL, NULL },
+ { PICO_SOCKET_STATE_TCP_CLOSED, NULL, NULL, NULL, NULL, NULL, NULL, NULL },
+ { PICO_SOCKET_STATE_TCP_LISTEN, &tcp_syn, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, NULL },
+ { PICO_SOCKET_STATE_TCP_SYN_SENT, &tcp_nosync_rst, &tcp_synack, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_rst },
+ { PICO_SOCKET_STATE_TCP_SYN_RECV, NULL, &tcp_nosync_rst, &tcp_first_ack, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_rst },
+ { PICO_SOCKET_STATE_TCP_ESTABLISHED, NULL, &tcp_ack, &tcp_ack, &tcp_data_in, &tcp_closewait, &tcp_closewait, &tcp_rst },
+ { PICO_SOCKET_STATE_TCP_CLOSE_WAIT, NULL, &tcp_ack, &tcp_ack, &tcp_send_rst, &tcp_closewait, &tcp_closewait, &tcp_rst },
+ { PICO_SOCKET_STATE_TCP_LAST_ACK, NULL, &tcp_ack, &tcp_lastackwait, &tcp_send_rst, &tcp_send_rst, &tcp_send_rst, &tcp_rst },
+ { PICO_SOCKET_STATE_TCP_FIN_WAIT1, NULL, &tcp_ack, &tcp_finwaitack, &tcp_data_in, &tcp_rcvfin, &tcp_finack, &tcp_rst },
+ { PICO_SOCKET_STATE_TCP_FIN_WAIT2, NULL, &tcp_ack, &tcp_ack, &tcp_data_in, &tcp_finwaitfin, &tcp_finack, &tcp_rst },
+ { PICO_SOCKET_STATE_TCP_CLOSING, NULL, &tcp_ack, &tcp_closewaitack, &tcp_send_rst, &tcp_send_rst, &tcp_send_rst, &tcp_rst },
+ { PICO_SOCKET_STATE_TCP_TIME_WAIT, NULL, &tcp_ack, &tcp_send_rst, &tcp_send_rst, &tcp_send_rst, &tcp_send_rst, &tcp_rst }
+};
+
+/*
+ NOTE: in SYN-RECV receiving syn when cloned by default (see yellow pos-it), should send reset.
+*/
+
+int pico_tcp_input(struct pico_socket *s, struct pico_frame *f)
+{
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr);
+ int ret = 0;
+ uint8_t flags = hdr->flags;
+ struct tcp_action_entry *action = &tcp_fsm[s->state >> 8];
+
+ f->payload = (f->transport_hdr + ((hdr->len & 0xf0) >> 2));
+ f->payload_len = f->transport_len - ((hdr->len & 0xf0) >> 2);
+
+ tcp_dbg("[%lu] TCP> [tcp input] socket: %p state: %d <-- local port:%d remote port: %d seq: %08x ack: %08x flags: %02x = t_len: %d, hdr: %u payload: %d\n", pico_tick,
+ s, s->state >> 8, short_be(hdr->trans.dport), short_be(hdr->trans.sport), SEQN(f), ACKN(f), hdr->flags, f->transport_len, (hdr->len & 0xf0) >> 2, f->payload_len );
+
+ /* This copy of the frame has the current socket as owner */
+ f->sock = s;
+ s->timestamp = PICO_TIME_MS();
+ /* Those are not supported at this time. */
+ flags &= ~(PICO_TCP_CWR | PICO_TCP_URG | PICO_TCP_ECN);
+ if (flags == PICO_TCP_SYN) {
+ if (action->syn)
+ action->syn(s,f);
+ } else if (flags == (PICO_TCP_SYN | PICO_TCP_ACK)) {
+ if (action->synack)
+ action->synack(s,f);
+ } else {
+ if ((flags == PICO_TCP_ACK) || (flags == (PICO_TCP_ACK | PICO_TCP_PSH))) {
+ if (action->ack) {
+ action->ack(s,f);
+ }
+ }
+ if (f->payload_len > 0 && !(s->state & PICO_SOCKET_STATE_CLOSED) && !TCP_IS_STATE(s,PICO_SOCKET_STATE_TCP_LISTEN))
+ {
+ ret = f->payload_len;
+ if (action->data)
+ action->data(s,f);
+ }
+ if (flags == PICO_TCP_FIN) {
+ if (action->fin)
+ action->fin(s,f);
+ }
+ if ((flags == (PICO_TCP_FIN | PICO_TCP_ACK)) || (flags == (PICO_TCP_FIN | PICO_TCP_ACK | PICO_TCP_PSH))) {
+ if (action->finack)
+ action->finack(s,f);
+ }
+ if (flags & PICO_TCP_RST) {
+ if (action->rst)
+ action->rst(s,f);
+ }
+ }
+
+//discard:
+ pico_frame_discard(f);
+ return ret;
+}
+
+static void tcp_send_keepalive(unsigned long when, void *_t)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)_t;
+ IGNORE_PARAMETER(when);
+ tcp_dbg("Sending keepalive (%d), [State = %d]...\n", t->backoff,t->sock.state );
+ if( t->sock.net && ((t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_ESTABLISHED) )
+ {
+ tcp_send_ack(t);
+
+ if (t->keepalive_timer_running > 0) {
+ t->keepalive_timer_running--;
+ }
+
+ if (t->keepalive_timer_running == 0) {
+ t->keepalive_timer_running++;
+ tcp_dbg("[Self] Adding timer(retransmit keepalive)\n");
+ pico_timer_add(t->rto << (++t->backoff), tcp_send_keepalive, t);
+ }
+ }
+}
+
+void zombie_timer(unsigned long time, void *param)
+{
+ struct tcp_port_pair * ports = (struct tcp_port_pair *)param;
+ IGNORE_PARAMETER(time);
+ if(ports)
+ {
+ struct pico_socket_tcp * t = (struct pico_socket_tcp *)pico_sockets_find(ports->local,ports->remote);
+ if(t)
+ {
+ (t->sock).state &= 0x00FFU;
+ (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
+ (t->sock).state &= 0xFF00U;
+ (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
+ tcp_dbg("Deleting zombie socket %p\n",param);
+ pico_socket_del(&t->sock);
+ }
+ pico_free(ports);
+ }
+}
+
+int pico_tcp_output(struct pico_socket *s, int loop_score)
+{
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+ struct pico_frame *f, *una;
+ struct pico_tcp_hdr *hdr;
+ int sent = 0;
+
+ una = first_segment(&t->tcpq_out);
+
+ f = peek_segment(&t->tcpq_out, t->snd_nxt);
+ while((f) && (t->cwnd >= t->in_flight)) {
+ hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+ f->timestamp = pico_tick;
+ tcp_add_options(t, f, hdr->flags, tcp_options_size(t, hdr->flags));
+ if (seq_compare(SEQN(f) + f->payload_len, SEQN(una) + (t->recv_wnd << t->recv_wnd_scale)) > 0) {
+ t->cwnd = t->in_flight;
+ if (t->cwnd < 1)
+ t->cwnd = 1;
+ if (t->x_mode != PICO_TCP_WINDOW_FULL) {
+ tcp_dbg("TCP> RIGHT SIZING (rwnd: %d, frame len: %d\n",t->recv_wnd << t->recv_wnd_scale, f->payload_len);
+ tcp_dbg("In window full...\n");
+ t->snd_nxt = SEQN(una); /* XXX prevent out-of-order-packets ! */ /*DLA re-enabled.*/
+ t->snd_retry = SEQN(una); /* XXX replace by retry pointer? */
+
+ /* Alternative to the line above: (better performance, but seems to lock anyway with larger buffers)
+ if (seq_compare(t->snd_nxt, SEQN(una)) > 0)
+ t->snd_nxt -= f->payload_len;
+ */
+
+ t->x_mode = PICO_TCP_WINDOW_FULL;
+ if (t->keepalive_timer_running == 0) {
+ tcp_dbg("[Window full] Adding timer(send keepalive)\n");
+ tcp_send_keepalive(0, t);
+ }
+ }
+ break;
+ }
+ tcp_dbg("TCP> DEQUEUED (for output) frame %08x, acks %08x len= %d, remaining frames %d\n", SEQN(f), ACKN(f), f->payload_len,t->tcpq_out.frames);
+ tcp_send(t, f);
+ sent++;
+ loop_score--;
+ t->snd_last_out = SEQN(f);
+ if (loop_score < 1)
+ break;
+ if (f->payload_len > 0) {
+ f = next_segment(&t->tcpq_out, f);
+ } else {
+ f = NULL;
+ }
+ }
+ if (sent > 0) {
+ if (t->rto < PICO_TCP_RTO_MIN)
+ t->rto = PICO_TCP_RTO_MIN;
+ //if (s->wakeup)
+ // t->sock.wakeup(PICO_SOCK_EV_WR, &t->sock);
+ add_retransmission_timer(t, pico_tick + t->rto);
+ } else {
+ /* Nothing to transmit. */
+ }
+
+ if ((t->tcpq_out.frames == 0) && (s->state & PICO_SOCKET_STATE_SHUT_LOCAL)) { /* if no more packets in queue, XXX replacled !f by tcpq check */
+ if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED) {
+ tcp_dbg("TCP> buffer empty, shutdown established ...\n");
+ /* send fin if queue empty and in state shut local (write) */
+ tcp_send_fin(t);
+ /* change tcp state to FIN_WAIT1 */
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_FIN_WAIT1;
+ } else if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_CLOSE_WAIT) {
+ struct tcp_port_pair *pair = (struct tcp_port_pair *)pico_zalloc(sizeof(struct tcp_port_pair));
+ pair->local = s->local_port;
+ pair->remote = s->remote_port;
+ /* send fin if queue empty and in state shut local (write) */
+ tcp_send_fin(t);
+ /* change tcp state to LAST_ACK */
+ s->state &= 0x00FFU;
+ s->state |= PICO_SOCKET_STATE_TCP_LAST_ACK;
+ tcp_dbg("TCP> STATE: LAST_ACK.\n");
+ // start zombie timer
+ pico_timer_add(PICO_TCP_ZOMBIE_TO,&zombie_timer,(void *)pair);
+ }
+ }
+ return loop_score;
+}
+
+/* function to make new segment from hold queue with specific size (mss) */
+static struct pico_frame * pico_hold_segment_make(struct pico_socket_tcp *t)
+{
+ struct pico_frame *f_temp,*f_new;
+ struct pico_socket *s = (struct pico_socket *) &t->sock;
+ struct pico_tcp_hdr *hdr;
+ int total_len = 0, total_payload_len = 0;
+ int off = 0, test = 0;
+
+ off = pico_tcp_overhead(s);
+
+ /* init with first frame in hold queue */
+ f_temp = first_segment(&t->tcpq_hold);
+ total_len = f_temp->payload_len;
+ f_temp = next_segment(&t->tcpq_hold, f_temp);
+
+ /* check till total_len <= MSS */
+ while ((f_temp != NULL) && ((total_len+f_temp->payload_len) <= PICO_TCP_DEFAULT_MSS)) {
+ total_len += f_temp->payload_len;
+ f_temp = next_segment(&t->tcpq_hold, f_temp);
+ if (f_temp == NULL)
+ break;
+ }
+ /* alloc new frame with payload size = off + total_len */
+ f_new = pico_socket_frame_alloc(s, off + total_len);
+ if (!f_new) {
+ pico_err = PICO_ERR_ENOMEM;
+ return f_new;
+ }
+
+ hdr = (struct pico_tcp_hdr *) f_new->transport_hdr;
+ /* init new frame */
+ f_new->payload += off;
+ f_new->payload_len -= off;
+ f_new->sock = s;
+
+ f_temp = first_segment(&t->tcpq_hold);
+ hdr->seq = ((struct pico_tcp_hdr *)(f_temp->transport_hdr))->seq; /* get sequence number of first frame */
+ hdr->trans.sport = t->sock.local_port;
+ hdr->trans.dport = t->sock.remote_port;
+
+ /* check till total_payload_len <= MSS */
+ while ((f_temp != NULL) && ((total_payload_len + f_temp->payload_len) <= PICO_TCP_DEFAULT_MSS)) {
+ /* cpy data and discard frame */
+ test++;
+ memcpy(f_new->payload + total_payload_len, f_temp->payload, f_temp->payload_len);
+ total_payload_len += f_temp->payload_len;
+ pico_discard_segment(&t->tcpq_hold, f_temp);
+ f_temp = first_segment(&t->tcpq_hold);
+ }
+
+ hdr->len = (f_new->payload - f_new->transport_hdr) << 2 | t->jumbo;
+
+ tcp_dbg_nagle("NAGLE make - joined %d segments, len %d bytes\n",test,total_payload_len);
+
+ return f_new;
+}
+
+/* original behavior kept when Nagle disabled;
+ Nagle algorithm added here, keeping hold frame queue instead of eg linked list of data */
+int pico_tcp_push(struct pico_protocol *self, struct pico_frame *f)
+{
+ struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+ struct pico_socket_tcp *t = (struct pico_socket_tcp *) f->sock;
+ struct pico_frame *f_new;
+ int total_len = 0;
+ IGNORE_PARAMETER(self);
+ pico_err = PICO_ERR_NOERR;
+ hdr->trans.sport = t->sock.local_port;
+ hdr->trans.dport = t->sock.remote_port;
+ hdr->seq = long_be(t->snd_last + 1);
+ hdr->len = (f->payload - f->transport_hdr) << 2 | t->jumbo;
+
+ if ((uint32_t)f->payload_len > (uint32_t)(t->tcpq_out.max_size - t->tcpq_out.size))
+ t->sock.ev_pending &= (~PICO_SOCK_EV_WR);
+
+ /***************************************************************************/
+
+ if (!IS_NAGLE_ENABLED((&(t->sock)))) {
+ /* TCP_NODELAY enabled, original behavior */
+ if (pico_enqueue_segment(&t->tcpq_out,f) > 0) {
+ tcp_dbg_nagle("TCP_PUSH - NO NAGLE - Pushing segment %08x, len %08x to socket %p\n", t->snd_last + 1, f->payload_len, t);
+ t->snd_last += f->payload_len;
+ return f->payload_len;
+ } else {
+ tcp_dbg("Enqueue failed.\n");
+ return 0;
+ }
+ }
+ /***************************************************************************/
+ else {
+ /* Nagle's algorithm enabled, check if ready to send, or put frame in hold queue */
+ if (IS_TCP_IDLE(t) && IS_TCP_HOLDQ_EMPTY(t)) { /* opt 1. send frame */
+ if (pico_enqueue_segment(&t->tcpq_out,f) > 0) {
+ tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushing segment %08x, len %08x to socket %p\n", t->snd_last + 1, f->payload_len, t);
+ t->snd_last += f->payload_len;
+ return f->payload_len;
+ } else {
+ tcp_dbg("Enqueue failed.\n");
+ return 0;
+ }
+ } else { /* opt 2. hold data back */
+ total_len = f->payload_len + t->tcpq_hold.size;
+ if ((total_len >= PICO_TCP_DEFAULT_MSS) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= PICO_TCP_DEFAULT_MSS)) {/* TODO check mss socket */
+ /* IF enough data in hold (>mss) AND space in out queue (>mss) */
+ /* add current frame in hold and make new segment */
+ if (pico_enqueue_segment(&t->tcpq_hold,f) > 0 ) {
+ tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushed into hold, make new (enqueued frames out %d)\n",t->tcpq_out.frames);
+ t->snd_last += f->payload_len; /* XXX WATCH OUT */
+ f_new = pico_hold_segment_make(t);
+ } else {
+ tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue hold failed 1\n");
+ return 0;
+ }
+ /* and put new frame in out queue */
+ if ((f_new != NULL) && (pico_enqueue_segment(&t->tcpq_out,f_new) > 0)) {
+ return f_new->payload_len;
+ } else {
+ tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue out failed, f_new = %p\n",f_new);
+ return -1; /* XXX something seriously wrong */
+ }
+ } else {
+ /* ELSE put frame in hold queue */
+ if (pico_enqueue_segment(&t->tcpq_hold,f) > 0) {
+ tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushed into hold (enqueued frames out %d)\n",t->tcpq_out.frames);
+ t->snd_last += f->payload_len; /* XXX WATCH OUT */
+ return f->payload_len;
+ } else {
+ pico_err = PICO_ERR_EAGAIN;
+ tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue hold failed 2\n");
+ return 0;
+ }
+ }
+ }
+ }
+ /***************************************************************************/
+}
+
+inline static void tcp_discard_all_segments(struct pico_tcp_queue *tq)
+{
+ struct pico_tree_node *index = NULL, *index_safe = NULL;
+ LOCK(Mutex);
+ pico_tree_foreach_safe(index,&tq->pool,index_safe)
+ {
+ struct pico_frame *f = index->keyValue;
+ if(!f)
+ break;
+ pico_tree_delete(&tq->pool,f);
+ pico_frame_discard(f);
+ }
+ tq->frames = 0;
+ tq->size = 0;
+ UNLOCK(Mutex);
+}
+
+void pico_tcp_cleanup_queues(struct pico_socket *sck)
+{
+ struct pico_socket_tcp * tcp = (struct pico_socket_tcp *)sck;
+ tcp_discard_all_segments(&tcp->tcpq_in);
+ tcp_discard_all_segments(&tcp->tcpq_out);
+ tcp_discard_all_segments(&tcp->tcpq_hold);
+}
+
+#endif //PICO_SUPPORT_TCP
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_tcp.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,99 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_TCP
+#define _INCLUDE_PICO_TCP
+#include "pico_addressing.h"
+#include "pico_protocol.h"
+#include "pico_socket.h"
+
+extern struct pico_protocol pico_proto_tcp;
+
+struct __attribute__((packed)) pico_tcp_hdr {
+ struct pico_trans trans;
+ uint32_t seq;
+ uint32_t ack;
+ uint8_t len;
+ uint8_t flags;
+ uint16_t rwnd;
+ uint16_t crc;
+ uint16_t urgent;
+};
+
+struct __attribute__((packed)) tcp_pseudo_hdr_ipv4
+{
+ struct pico_ip4 src;
+ struct pico_ip4 dst;
+ uint16_t tcp_len;
+ uint8_t res;
+ uint8_t proto;
+};
+
+#define PICO_TCPHDR_SIZE 20
+#define PICO_SIZE_TCPOPT_SYN 20
+#define PICO_SIZE_TCPHDR (sizeof(struct pico_tcp_hdr))
+
+#define PICO_TCP_DEFAULT_MSS 1444
+
+
+
+/* TCP options */
+#define PICO_TCP_OPTION_END 0x00
+#define PICO_TCPOPTLEN_END 1
+#define PICO_TCP_OPTION_NOOP 0x01
+#define PICO_TCPOPTLEN_NOOP 1
+#define PICO_TCP_OPTION_MSS 0x02
+#define PICO_TCPOPTLEN_MSS 4
+#define PICO_TCP_OPTION_WS 0x03
+#define PICO_TCPOPTLEN_WS 3
+#define PICO_TCP_OPTION_SACK_OK 0x04
+#define PICO_TCPOPTLEN_SACK_OK 2
+#define PICO_TCP_OPTION_SACK 0x05
+#define PICO_TCPOPTLEN_SACK 2 /* Plus the block */
+#define PICO_TCP_OPTION_TIMESTAMP 0x08
+#define PICO_TCPOPTLEN_TIMESTAMP 10
+
+/* TCP flags */
+#define PICO_TCP_FIN 0x01
+#define PICO_TCP_SYN 0x02
+#define PICO_TCP_RST 0x04
+#define PICO_TCP_PSH 0x08
+#define PICO_TCP_ACK 0x10
+#define PICO_TCP_URG 0x20
+#define PICO_TCP_ECN 0x40
+#define PICO_TCP_CWR 0x80
+
+
+
+struct __attribute__((packed)) pico_tcp_option
+{
+ uint8_t kind;
+ uint8_t len;
+#if 0
+ union {
+ uint16_t mss;
+ uint8_t wshift;
+ struct {
+ uint32_t tsval;
+ uint32_t tsecr;
+ } timestamp;
+ } data;
+#endif
+};
+
+struct pico_socket *pico_tcp_open(void);
+int pico_tcp_read(struct pico_socket *s, void *buf, int len);
+int pico_tcp_initconn(struct pico_socket *s);
+int pico_tcp_input(struct pico_socket *s, struct pico_frame *f);
+uint16_t pico_tcp_checksum_ipv4(struct pico_frame *f);
+int pico_tcp_overhead(struct pico_socket *s);
+int pico_tcp_output(struct pico_socket *s, int loop_score);
+int pico_tcp_queue_in_is_empty(struct pico_socket *s);
+int pico_tcp_reply_rst(struct pico_frame *f);
+void pico_tcp_cleanup_queues(struct pico_socket *sck);
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_udp.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,177 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_udp.h"
+#include "pico_config.h"
+#include "pico_eth.h"
+#include "pico_socket.h"
+#include "pico_stack.h"
+
+
+/* Queues */
+static struct pico_queue udp_in = {0};
+static struct pico_queue udp_out = {0};
+
+
+/* Functions */
+
+uint16_t pico_udp_checksum_ipv4(struct pico_frame *f)
+{
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ struct pico_udp_hdr *udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ struct pico_socket *s = f->sock;
+ struct pico_ipv4_pseudo_hdr pseudo;
+
+ if (s) {
+ /* Case of outgoing frame */
+ //dbg("UDP CRC: on outgoing frame\n");
+ pseudo.src.addr = s->local_addr.ip4.addr;
+ pseudo.dst.addr = s->remote_addr.ip4.addr;
+ } else {
+ /* Case of incomming frame */
+ //dbg("UDP CRC: on incomming frame\n");
+ pseudo.src.addr = hdr->src.addr;
+ pseudo.dst.addr = hdr->dst.addr;
+ }
+ pseudo.zeros = 0;
+ pseudo.proto = PICO_PROTO_UDP;
+ pseudo.len = short_be(f->transport_len);
+
+ return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv4_pseudo_hdr), udp_hdr, f->transport_len);
+}
+
+
+static int pico_udp_process_out(struct pico_protocol *self, struct pico_frame *f)
+{
+ IGNORE_PARAMETER(self);
+ return pico_network_send(f);
+}
+
+static int pico_udp_push(struct pico_protocol *self, struct pico_frame *f)
+{
+ struct pico_udp_hdr *hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info;
+
+ /* this (fragmented) frame should contain a transport header */
+ if (f->transport_hdr != f->payload) {
+ hdr->trans.sport = f->sock->local_port;
+ if (remote_duple) {
+ hdr->trans.dport = remote_duple->remote_port;
+ } else {
+ hdr->trans.dport = f->sock->remote_port;
+ }
+ hdr->len = short_be(f->transport_len);
+ /* do not perform CRC validation. If you want to, a system needs to be
+ implemented to calculate the CRC over the total payload of a
+ fragmented payload */
+ hdr->crc = 0;
+ }
+
+ if (pico_enqueue(self->q_out, f) > 0) {
+ return f->payload_len;
+ } else {
+ return 0;
+ }
+}
+
+/* Interface: protocol definition */
+struct pico_protocol pico_proto_udp = {
+ .name = "udp",
+ .proto_number = PICO_PROTO_UDP,
+ .layer = PICO_LAYER_TRANSPORT,
+ .process_in = pico_transport_process_in,
+ .process_out = pico_udp_process_out,
+ .push = pico_udp_push,
+ .q_in = &udp_in,
+ .q_out = &udp_out,
+};
+
+
+#define PICO_UDP_MODE_UNICAST 0x01
+#define PICO_UDP_MODE_MULTICAST 0x02
+#define PICO_UDP_MODE_BROADCAST 0xFF
+
+struct pico_socket_udp
+{
+ struct pico_socket sock;
+ int mode;
+#ifdef PICO_SUPPORT_MCAST
+ uint8_t mc_ttl; /* Multicasting TTL */
+#endif
+};
+
+#ifdef PICO_SUPPORT_MCAST
+int pico_udp_set_mc_ttl(struct pico_socket *s, uint8_t ttl)
+{
+ struct pico_socket_udp *u;
+ if(!s) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ u = (struct pico_socket_udp *) s;
+ u->mc_ttl = ttl;
+ return 0;
+}
+
+int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl)
+{
+ struct pico_socket_udp *u;
+ if(!s)
+ return -1;
+ u = (struct pico_socket_udp *) s;
+ *ttl = u->mc_ttl;
+ return 0;
+}
+#endif /* PICO_SUPPORT_MCAST */
+
+struct pico_socket *pico_udp_open(void)
+{
+ struct pico_socket_udp *u = pico_zalloc(sizeof(struct pico_socket_udp));
+ if (!u)
+ return NULL;
+ u->mode = PICO_UDP_MODE_UNICAST;
+
+#ifdef PICO_SUPPORT_MCAST
+ u->mc_ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
+ /* enable multicast loopback by default */
+ u->sock.opt_flags |= (1 << PICO_SOCKET_OPT_MULTICAST_LOOP);
+#endif
+
+ return &u->sock;
+}
+
+int pico_udp_recv(struct pico_socket *s, void *buf, int len, void *src, uint16_t *port)
+{
+ struct pico_frame *f = pico_queue_peek(&s->q_in);
+ if (f) {
+ f->payload = f->transport_hdr + sizeof(struct pico_udp_hdr);
+ f->payload_len = f->transport_len - sizeof(struct pico_udp_hdr);
+// dbg("expected: %d, got: %d\n", len, f->payload_len);
+ if (src)
+ pico_store_network_origin(src, f);
+ if (port) {
+ struct pico_trans *hdr = (struct pico_trans *)f->transport_hdr;
+ *port = hdr->sport;
+ }
+ if (f->payload_len > len) {
+ memcpy(buf, f->payload, len);
+ f->payload += len;
+ f->payload_len -= len;
+ return len;
+ } else {
+ int ret = f->payload_len;
+ memcpy(buf, f->payload, f->payload_len);
+ f = pico_dequeue(&s->q_in);
+ pico_frame_discard(f);
+ return ret;
+ }
+ } else return 0;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_udp.h Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,42 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+*********************************************************************/
+#ifndef _INCLUDE_PICO_UDP
+#define _INCLUDE_PICO_UDP
+#include "pico_addressing.h"
+#include "pico_protocol.h"
+
+extern struct pico_protocol pico_proto_udp;
+
+struct __attribute__((packed)) pico_udp_hdr {
+ struct pico_trans trans;
+ uint16_t len;
+ uint16_t crc;
+};
+#define PICO_UDPHDR_SIZE 8
+
+struct pico_socket *pico_udp_open(void);
+int pico_udp_recv(struct pico_socket *s, void *buf, int len, void *src, uint16_t *port);
+uint16_t pico_udp_checksum_ipv4(struct pico_frame *f);
+
+#ifdef PICO_SUPPORT_MCAST
+int pico_udp_set_mc_ttl(struct pico_socket *s, uint8_t ttl);
+int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl);
+#else
+static inline int pico_udp_set_mc_ttl(struct pico_socket *s, uint8_t ttl)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+static inline int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+#endif /* PICO_SUPPORT_MCAST */
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_zmq.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,472 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+#include "pico_stack.h"
+#include "pico_config.h"
+#include "pico_ipv4.h"
+#include "pico_socket.h"
+#include "pico_zmq.h"
+
+#define MY_VERSION 1u
+
+
+enum zmq_state {
+ ST_OPEN = 0,
+ ST_CONNECTED,
+ ST_SIGNATURE,
+ ST_VERSION,
+ ST_GREETING,
+ ST_RDY,
+ ST_BUSY
+};
+
+enum zmq_role {
+ ROLE_NONE = 0,
+ ROLE_PUBLISHER,
+ ROLE_SUBSCRIBER
+};
+
+struct __attribute__((packed)) zmq_msg {
+ uint8_t flags;
+ uint8_t len;
+ char txt[0];
+};
+
+struct zmq_socket;
+
+struct zmq_connector {
+ struct pico_socket *sock;
+ enum zmq_state state;
+ ZMQ parent;
+ enum zmq_role role;
+ uint8_t bytes_received;
+ struct zmq_connector *next;
+};
+
+struct zmq_socket {
+ struct pico_socket *sock;
+ void (*ready)(ZMQ z);
+ enum zmq_state state;
+ struct zmq_connector *subs;
+ enum zmq_role role;
+};
+
+static int zmq_socket_cmp(void *ka, void *kb)
+{
+ ZMQ a = ka;
+ ZMQ b = kb;
+ if (a->sock < b->sock)
+ return -1;
+ if (b->sock < a->sock)
+ return 1;
+ return 0;
+}
+PICO_TREE_DECLARE(zmq_sockets, zmq_socket_cmp);
+
+static inline ZMQ ZMTP(struct pico_socket *s)
+{
+ struct zmq_socket tst = { .sock = s };
+ return (pico_tree_findKey(&zmq_sockets, &tst));
+}
+
+static inline struct zmq_connector *find_subscriber(struct pico_socket *s)
+{
+ ZMQ search;
+ struct pico_tree_node *idx;
+ struct zmq_connector *el;
+ pico_tree_foreach(idx, &zmq_sockets) {
+ search = idx->keyValue;
+ el = search->subs;
+ while(el) {
+ if (el->sock == s)
+ return el;
+ el = el->next;
+ }
+ }
+ return NULL;
+}
+
+
+static void zmq_connector_add(ZMQ z, struct zmq_connector *zc)
+{
+ zc->next = z->subs;
+ z->subs = zc;
+ zc->parent = z;
+ dbg("Added connector %p, sock is %p\n", zc, zc->sock);
+}
+
+static void zmq_connector_del(struct zmq_connector *zc)
+{
+ ZMQ z = zc->parent;
+ if(z) {
+ struct zmq_connector *el = z->subs, *prev = NULL; /* el = pointer to linked list */
+ while(el) {
+ if (el == zc) { /* did we find the connector that we want to delete? */
+ if (prev) /* was there a previous list item? */
+ prev->next = zc->next; /* link the linked list again */
+ else
+ z->subs = zc->next; /* we were at the beginning of the list */
+ break;
+ }
+ prev = el;
+ el = el->next;
+ }
+ }
+ pico_socket_close(zc->sock);
+ pico_free(zc);
+}
+
+static void zmq_check_state(ZMQ z)
+{
+ struct zmq_connector *c = z->subs;
+ enum zmq_state default_state, option_state;
+ if ((z->state != ST_RDY) && (z->state != ST_BUSY))
+ return;
+ if (z->role == ROLE_SUBSCRIBER) {
+ default_state = ST_RDY;
+ option_state = ST_BUSY;
+ } else {
+ default_state = ST_BUSY;
+ option_state = ST_RDY;
+ }
+ z->state = default_state;
+ while(c) {
+ if (c->state == option_state) {
+ z->state = option_state;
+ return;
+ }
+ c = c->next;
+ }
+}
+
+
+static void zmq_hs_connected(struct zmq_connector *z)
+{
+ /* v2 signature */
+ uint8_t my_signature[14] = {0xff, 0, 0, 0, 0, 0, 0, 0, 1, 0x7f, 1, 1, 0, 0};
+
+// uint8_t my_ver[2] = {MY_VERSION, 0};
+// uint8_t my_greeting[52] = {'N','U','L','L', 0};
+
+ pico_socket_write(z->sock, my_signature, 14);
+// pico_socket_write(z->sock, my_ver, 2);
+
+// if (MY_VERSION > 2)
+// pico_socket_write(z->sock, my_greeting, 52);
+
+ z->state = ST_SIGNATURE;
+// z->state = ST_RDY;
+}
+
+static void zmq_hs_signature(struct zmq_connector *zc)
+{
+ uint8_t incoming[20];
+ int ret;
+
+ ret = pico_socket_read(zc->sock, incoming, 14);
+ if (zc->bytes_received == 0 && ret > 0 && incoming[0] != 0xFF) {
+ //dbg("Received invalid signature: [0]!=0xFF\n");
+ zmq_connector_del(zc);
+ }
+ zc->bytes_received += ret;
+ if (zc->bytes_received < 14) {
+ //dbg("Waiting for the rest of the sig - got %u bytes\n",zc->bytes_received);
+ return;
+ }
+
+ //dbg("Valid signature received. len = %d, first byte: %02x\n", ret, incoming[0]);
+ zc->state = ST_RDY;
+}
+
+static void zmq_hs_version(struct zmq_connector *zc)
+{
+ uint8_t incoming[20];
+ int ret;
+ ret = pico_socket_read(zc->sock, incoming, 2);
+ if (ret < 0) {
+ dbg("Cannot exchange valid version information. Read returned -1\n");
+ zmq_connector_del(zc);
+ return;
+ }
+ if (ret == 0)
+ return;
+/* Version check?
+ if (incoming[0] != 3) {
+ dbg("Version %d.x not supported by this publisher\n", incoming[0]);
+ zmq_connector_del(zc);
+ return;
+ }
+ dbg("Subscriber is using version 3. Good!\n");
+*/
+ dbg("Subscriber is using version %d. Good!\n", incoming[0]);
+ if (incoming[0] == 3)
+ zc->state = ST_GREETING;
+ else
+ zc->state = ST_RDY;
+}
+
+static void zmq_hs_greeting(struct zmq_connector *zc)
+{
+ uint8_t incoming[64];
+ int ret;
+ ret = pico_socket_read(zc->sock, incoming, 64);
+ dbg("zmq_socket_read in greeting returned %d\n", ret);
+ if (ret == 0)
+ return;
+ if (ret < 0) {
+ dbg("Cannot retrieve valid greeting\n");
+ zmq_connector_del(zc);
+ return;
+ }
+ zc->state = ST_RDY;
+ zmq_check_state(zc->parent);
+ dbg("Paired. Sending Ready.\n");
+ pico_socket_write(zc->sock, "READY ",8);
+}
+
+static void zmq_hs_rdy(struct zmq_connector *zc)
+{
+ int ret;
+ uint8_t incoming[258];
+ if (zc->role == ROLE_SUBSCRIBER)
+ return;
+ ret = pico_socket_read(zc->sock, incoming, 258);
+ dbg("Got %d bytes from subscriber whilst in rdy state.\n", ret);
+}
+
+static void zmq_hs_busy(struct zmq_connector *zc)
+{
+ int was_busy = 0;
+ if (zc->parent->state == ST_BUSY)
+ was_busy = 1;
+ zmq_check_state(zc->parent);
+ if (was_busy && (zc->parent->state == ST_RDY) && zc->parent->ready)
+ zc->parent->ready(zc->parent);
+}
+
+static void(*zmq_hs_cb[])(struct zmq_connector *) = {
+ NULL,
+ zmq_hs_connected,
+ zmq_hs_signature,
+ zmq_hs_version,
+ zmq_hs_greeting,
+ zmq_hs_rdy,
+ zmq_hs_busy
+};
+
+
+static void cb_tcp0mq(uint16_t ev, struct pico_socket *s)
+{
+ struct pico_ip4 orig;
+ uint16_t port;
+ char peer[30];
+ struct zmq_connector *z_a, *zc;
+ ZMQ z = ZMTP(s);
+
+ /* Publisher. Accepting new subscribers */
+ if (z) {
+ if (ev & PICO_SOCK_EV_CONN) {
+ z_a = pico_zalloc(sizeof(struct zmq_socket));
+ if (z_a == NULL)
+ return;
+
+ z_a->sock = pico_socket_accept(s, &orig, &port);
+ pico_ipv4_to_string(peer, orig.addr);
+ dbg("tcp0mq> Connection requested by %s:%u.\n", peer, short_be(port));
+ if (z->state == ST_OPEN) {
+ dbg("tcp0mq> Accepted connection! New subscriber on sock %p.\n",z_a->sock);
+ zmq_connector_add(z, z_a);
+ z_a->role = ROLE_PUBLISHER;
+ z_a->state = ST_CONNECTED;
+ zmq_hs_connected(z_a);
+ } else {
+ dbg("tcp0mq> Server busy, connection rejected\n");
+ pico_socket_close(z_a->sock);
+ }
+ }
+ return;
+ }
+
+ zc = find_subscriber(s);
+ if (!zc) {
+ dbg("Cannot find subscriber with socket %p, ev = %d!\n", s, ev);
+// pico_socket_close(s);
+ return;
+ }
+
+ if ((ev & PICO_SOCK_EV_CONN) && zc->role == ROLE_SUBSCRIBER && zc->state == ST_OPEN)
+ {
+ zc->state = ST_CONNECTED;
+ zmq_hs_connected(zc);
+ }
+
+
+ if (ev & PICO_SOCK_EV_RD) {
+ if (zmq_hs_cb[zc->state])
+ zmq_hs_cb[zc->state](zc);
+ }
+
+ if ((ev & PICO_SOCK_EV_WR) && zc->parent && (zc->parent->role == ROLE_PUBLISHER) && (zc->state == ST_BUSY)) {
+ if (zmq_hs_cb[zc->state])
+ zmq_hs_cb[zc->state](zc);
+ }
+
+
+ if (ev & PICO_SOCK_EV_FIN) {
+ dbg("tcp0mq> Connection closed.\n");
+ zmq_connector_del(zc);
+ }
+
+ if (ev & PICO_SOCK_EV_ERR) {
+ dbg("tcp0mq> Socket Error received: %s. Bailing out.\n", strerror(pico_err));
+ zmq_connector_del(zc);
+ }
+
+ if (ev & PICO_SOCK_EV_CLOSE) {
+ dbg("tcp0mq> event close\n");
+ zmq_connector_del(zc);
+ }
+
+}
+
+ZMQ zmq_subscriber(void (*cb)(ZMQ z))
+{
+ ZMQ z = pico_zalloc(sizeof(struct zmq_socket));
+ if (!z) {
+ pico_err = PICO_ERR_ENOMEM;
+ return NULL;
+ }
+ z->state = ST_BUSY;
+ z->ready = cb;
+ z->role = ROLE_SUBSCRIBER;
+ pico_tree_insert(&zmq_sockets, z);
+ return z;
+}
+
+int zmq_connect(ZMQ z, char *address, uint16_t port)
+{
+ struct pico_ip4 ip = {0};
+ struct zmq_connector *z_c;
+ if (pico_string_to_ipv4(address, &ip.addr) < 0) {
+ dbg("FIXME!! I need to synchronize with the dns client to get to my publisher :(\n");
+ return -1;
+ }
+
+ z_c = pico_zalloc(sizeof(struct zmq_connector));
+ if (!z_c)
+ return -1;
+ z_c->role = ROLE_SUBSCRIBER;
+ z_c->state = ST_OPEN;
+ z_c->sock = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_TCP, &cb_tcp0mq);
+ if (!z_c->sock) {
+ pico_free(z_c);
+ return -1;
+ }
+ if (pico_socket_connect(z_c->sock, &ip, short_be(port)) < 0)
+ return -1;
+ zmq_connector_add(z, z_c);
+ return 0;
+}
+
+ZMQ zmq_publisher(uint16_t _port, void (*cb)(ZMQ z))
+{
+ struct pico_socket *s;
+ struct pico_ip4 inaddr_any = {0};
+ uint16_t port = short_be(_port);
+ ZMQ z = NULL;
+ s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_TCP, &cb_tcp0mq);
+ if (!s)
+ return NULL;
+
+ dbg("zmq_publisher: BIND\n");
+ if (pico_socket_bind(s, &inaddr_any, &port)!= 0) {
+ dbg("zmq publisher: BIND failed\n");
+ return NULL;
+ }
+ if (pico_socket_listen(s, 2) != 0) {
+ dbg("zmq publisher: LISTEN failed\n");
+ return NULL;
+ }
+ dbg("zmq_publisher: Active and bound to local port %d\n", short_be(port));
+
+ z = pico_zalloc(sizeof(struct zmq_socket));
+ if (!z) {
+ pico_socket_close(s);
+ pico_err = PICO_ERR_ENOMEM;
+ return NULL;
+ }
+ z->sock = s;
+ z->state = ST_OPEN;
+ z->ready = cb;
+ z->role = ROLE_PUBLISHER;
+ z->subs = NULL;
+ pico_tree_insert(&zmq_sockets, z);
+ dbg("zmq publisher created.\n");
+ return z;
+}
+
+int zmq_send(ZMQ z, char *txt, int len)
+{
+ struct zmq_msg *msg;
+ struct zmq_connector *c = z->subs;
+ int ret = 0;
+
+ if (!c)
+ {
+ dbg("no subscribers, bailing out\n");
+ return 0; /* Need at least one subscriber */
+ }
+ msg = pico_zalloc(len + 2);
+ msg->flags = 4;
+ msg->len = (uint8_t) len;
+ memcpy(msg->txt, txt, len);
+
+ while (c) {
+ dbg("write to %u\n",c->state);
+ if ((ST_RDY == c->state) && (pico_socket_write(c->sock, msg, len + 2) > 0))
+ ret++;
+ c = c->next;
+ }
+ pico_free(msg);
+ return ret;
+}
+
+int zmq_recv(ZMQ z, char *txt)
+{
+ int ret;
+ struct zmq_msg msg;
+ struct zmq_connector *nxt, *c = z->subs;
+ if (z->state != ST_RDY)
+ return 0;
+ while (c) {
+ nxt = c->next;
+ ret = pico_socket_read(c->sock, &msg, 2);
+ if (ret < 0) {
+ dbg("Error reading!\n");
+ zmq_connector_del(c);
+ } else if (ret < 2) {
+ c->state = ST_BUSY;
+ } else {
+ return pico_socket_read(c->sock, txt, msg.len);
+ }
+ c = nxt;
+ }
+ zmq_check_state(z);
+ return 0;
+}
+
+void zmq_close(ZMQ z)
+{
+ struct zmq_connector *nxt, *c = z->subs;
+ while(c) {
+ nxt = c->next;
+ zmq_connector_del(c);
+ c = nxt;
+ }
+ pico_socket_close(z->sock);
+ pico_free(z);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_zmq.h Thu Sep 19 13:26:14 2013 +0000 @@ -0,0 +1,14 @@ +#ifndef __PICO_ZMQ_H +#define __PICO_ZMQ_H + +struct zmq_socket; +typedef struct zmq_socket *ZMQ; + +ZMQ zmq_publisher(uint16_t _port, void (*cb)(ZMQ z)); +ZMQ zmq_subscriber(void (*cb)(ZMQ z)); +int zmq_connect(ZMQ z, char *address, uint16_t port); +int __attribute__((unused)) zmq_send(ZMQ z, char *txt, int len); +int zmq_recv(ZMQ z, char *txt); +void zmq_close(ZMQ z); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_arp.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,320 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_config.h"
+#include "pico_arp.h"
+#include "pico_tree.h"
+#include "pico_ipv4.h"
+#include "pico_device.h"
+#include "pico_stack.h"
+
+const uint8_t PICO_ETHADDR_ALL[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+#define PICO_ARP_TIMEOUT 600000
+#define PICO_ARP_RETRY 300
+
+#ifdef DEBUG_ARP
+ #define arp_dbg dbg
+#else
+ #define arp_dbg(...) do{}while(0)
+#endif
+
+static struct pico_queue pending;
+static int pending_timer_on = 0;
+
+void check_pending(unsigned long now, void *_unused)
+{
+ struct pico_frame *f = pico_dequeue(&pending);
+ IGNORE_PARAMETER(now);
+ IGNORE_PARAMETER(_unused);
+ if (!f) {
+ pending_timer_on = 0;
+ return;
+ }
+ if(pico_ethernet_send(f) > 0)
+ pico_frame_discard(f);
+ pico_timer_add(PICO_ARP_RETRY, &check_pending, NULL);
+}
+
+
+struct
+__attribute__ ((__packed__))
+pico_arp_hdr
+{
+ uint16_t htype;
+ uint16_t ptype;
+ uint8_t hsize;
+ uint8_t psize;
+ uint16_t opcode;
+ uint8_t s_mac[PICO_SIZE_ETH];
+ struct pico_ip4 src;
+ uint8_t d_mac[PICO_SIZE_ETH];
+ struct pico_ip4 dst;
+};
+
+
+#define PICO_SIZE_ARPHDR ((sizeof(struct pico_arp_hdr)))
+
+/* Arp Entries for the tables. */
+struct pico_arp {
+/* CAREFUL MAN! ARP entry MUST begin with a pico_eth structure,
+ * due to in-place casting!!! */
+ struct pico_eth eth;
+ struct pico_ip4 ipv4;
+ int arp_status;
+ uint32_t timestamp;
+ struct pico_device *dev;
+};
+
+
+
+/*****************/
+/** ARP TREE **/
+/*****************/
+
+/* Routing destination */
+
+static int arp_compare(void * ka, void * kb)
+{
+ struct pico_arp *a = ka, *b = kb;
+ if (a->ipv4.addr < b->ipv4.addr)
+ return -1;
+ else if (a->ipv4.addr > b->ipv4.addr)
+ return 1;
+ return 0;
+}
+
+PICO_TREE_DECLARE(arp_tree, arp_compare);
+
+/*********************/
+/** END ARP TREE **/
+/*********************/
+
+struct pico_eth *pico_arp_lookup(struct pico_ip4 *dst)
+{
+ struct pico_arp search, *found;
+ search.ipv4.addr = dst->addr;
+ found = pico_tree_findKey(&arp_tree,&search);
+ if (found && (found->arp_status != PICO_ARP_STATUS_STALE))
+ return &found->eth;
+ return NULL;
+}
+
+struct pico_ip4 *pico_arp_reverse_lookup(struct pico_eth *dst)
+{
+ struct pico_arp* search;
+ struct pico_tree_node * index;
+ pico_tree_foreach(index,&arp_tree){
+ search = index->keyValue;
+ if(memcmp(&(search->eth.addr), &dst->addr, 6) == 0)
+ return &search->ipv4;
+ }
+ return NULL;
+}
+
+struct pico_eth *pico_arp_get(struct pico_frame *f) {
+ struct pico_eth *a4;
+ struct pico_ip4 gateway;
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ struct pico_ipv4_link *l;
+
+ l = pico_ipv4_link_get(&hdr->dst);
+ if(l){
+ //address belongs to ourself
+ return &l->dev->eth->mac;
+ }
+
+ gateway = pico_ipv4_route_get_gateway(&hdr->dst);
+ /* check if dst is local (gateway = 0), or if to use gateway */
+ if (gateway.addr != 0)
+ a4 = pico_arp_lookup(&gateway); /* check if gateway ip mac in cache */
+ else
+ a4 = pico_arp_lookup(&hdr->dst); /* check if local ip mac in cache */
+ if (!a4) {
+ if (++f->failure_count < 4) {
+ dbg ("================= ARP REQUIRED: %d =============\n\n", f->failure_count);
+ /* check if dst is local (gateway = 0), or if to use gateway */
+ if (gateway.addr != 0)
+ pico_arp_query(f->dev, &gateway); /* arp to gateway */
+ else
+ pico_arp_query(f->dev, &hdr->dst); /* arp to dst */
+
+ pico_enqueue(&pending, f);
+ if (!pending_timer_on) {
+ pending_timer_on++;
+ pico_timer_add(PICO_ARP_RETRY, &check_pending, NULL);
+ }
+ } else {
+ dbg("ARP: Destination Unreachable\n");
+ pico_notify_dest_unreachable(f);
+ pico_frame_discard(f);
+ }
+ }
+ return a4;
+}
+
+#ifdef DEBUG_ARP
+void dbg_arp(void)
+{
+ struct pico_arp *a;
+ struct pico_tree_node * index;
+
+ pico_tree_foreach(index,&arp_tree) {
+ a = index->keyValue;
+ arp_dbg("ARP to %08x, mac: %02x:%02x:%02x:%02x:%02x:%02x\n", a->ipv4.addr,a->eth.addr[0],a->eth.addr[1],a->eth.addr[2],a->eth.addr[3],a->eth.addr[4],a->eth.addr[5] );
+ }
+}
+#endif
+
+void arp_expire(unsigned long now, void *_stale)
+{
+ struct pico_arp *stale = (struct pico_arp *) _stale;
+ IGNORE_PARAMETER(now);
+ stale->arp_status = PICO_ARP_STATUS_STALE;
+ arp_dbg("ARP: Setting arp_status to STALE\n");
+ pico_arp_query(stale->dev, &stale->ipv4);
+
+}
+
+void pico_arp_add_entry(struct pico_arp *entry)
+{
+ entry->arp_status = PICO_ARP_STATUS_REACHABLE;
+ entry->timestamp = PICO_TIME();
+
+ pico_tree_insert(&arp_tree,entry);
+ arp_dbg("ARP ## reachable.\n");
+ pico_timer_add(PICO_ARP_TIMEOUT, arp_expire, entry);
+}
+
+int pico_arp_create_entry(uint8_t* hwaddr, struct pico_ip4 ipv4, struct pico_device* dev)
+{
+ struct pico_arp* arp = pico_zalloc(sizeof(struct pico_arp));
+ if(!arp){
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ memcpy(arp->eth.addr, hwaddr, 6);
+ arp->ipv4.addr = ipv4.addr;
+ arp->dev = dev;
+
+ pico_arp_add_entry(arp);
+
+ return 0;
+}
+
+int pico_arp_receive(struct pico_frame *f)
+{
+ struct pico_arp_hdr *hdr;
+ struct pico_arp search, *found, *new = NULL;
+ int ret = -1;
+ hdr = (struct pico_arp_hdr *) f->net_hdr;
+
+ if (!hdr)
+ goto end;
+
+
+ /* Populate a new arp entry */
+ search.ipv4.addr = hdr->src.addr;
+ memcpy(search.eth.addr, hdr->s_mac, PICO_SIZE_ETH);
+
+ /* Search for already existing entry */
+
+ found = pico_tree_findKey(&arp_tree,&search);
+ if (!found) {
+ new = pico_zalloc(sizeof(struct pico_arp));
+ if (!new)
+ goto end;
+ new->ipv4.addr = hdr->src.addr;
+ }
+ else if (found->arp_status == PICO_ARP_STATUS_STALE) {
+ /* Replace if stale */
+ new = found;
+
+ pico_tree_delete(&arp_tree,new);
+ }
+
+ ret = 0;
+
+ if (new) {
+ memcpy(new->eth.addr, hdr->s_mac, PICO_SIZE_ETH);
+ new->dev = f->dev;
+ pico_arp_add_entry(new);
+ }
+
+ if (hdr->opcode == PICO_ARP_REQUEST) {
+ struct pico_ip4 me;
+ struct pico_eth_hdr *eh = (struct pico_eth_hdr *)f->datalink_hdr;
+ struct pico_device *link_dev;
+ me.addr = hdr->dst.addr;
+
+ link_dev = pico_ipv4_link_find(&me);
+ if (link_dev != f->dev)
+ goto end;
+
+ hdr->opcode = PICO_ARP_REPLY;
+ memcpy(hdr->d_mac, hdr->s_mac, PICO_SIZE_ETH);
+ memcpy(hdr->s_mac, f->dev->eth->mac.addr, PICO_SIZE_ETH);
+ hdr->dst.addr = hdr->src.addr;
+ hdr->src.addr = me.addr;
+
+ /* Prepare eth header for arp reply */
+ memcpy(eh->daddr, eh->saddr, PICO_SIZE_ETH);
+ memcpy(eh->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH);
+ f->start = f->datalink_hdr;
+ f->len = PICO_SIZE_ETHHDR + PICO_SIZE_ARPHDR;
+ f->dev->send(f->dev, f->start, f->len);
+ }
+
+#ifdef DEBUG_ARG
+ dbg_arp();
+#endif
+
+end:
+ pico_frame_discard(f);
+ return ret;
+}
+
+int pico_arp_query(struct pico_device *dev, struct pico_ip4 *dst)
+{
+ struct pico_frame *q = pico_frame_alloc(PICO_SIZE_ETHHDR + PICO_SIZE_ARPHDR);
+ struct pico_eth_hdr *eh;
+ struct pico_arp_hdr *ah;
+ struct pico_ip4 *src;
+ int ret;
+
+ src = pico_ipv4_source_find(dst);
+ if (!src)
+ return -1;
+
+ arp_dbg("QUERY: %08x\n", dst->addr);
+
+ if (!q)
+ return -1;
+ eh = (struct pico_eth_hdr *)q->start;
+ ah = (struct pico_arp_hdr *) (q->start + PICO_SIZE_ETHHDR);
+
+ /* Fill eth header */
+ memcpy(eh->saddr, dev->eth->mac.addr, PICO_SIZE_ETH);
+ memcpy(eh->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH);
+ eh->proto = PICO_IDETH_ARP;
+
+ /* Fill arp header */
+ ah->htype = PICO_ARP_HTYPE_ETH;
+ ah->ptype = PICO_IDETH_IPV4;
+ ah->hsize = PICO_SIZE_ETH;
+ ah->psize = PICO_SIZE_IP4;
+ ah->opcode = PICO_ARP_REQUEST;
+ memcpy(ah->s_mac, dev->eth->mac.addr, PICO_SIZE_ETH);
+ ah->src.addr = src->addr;
+ ah->dst.addr = dst->addr;
+ arp_dbg("Sending arp query.\n");
+ ret = dev->send(dev, q->start, q->len);
+ pico_frame_discard(q);
+ return ret;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_device.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,246 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_config.h"
+#include "pico_device.h"
+#include "pico_stack.h"
+#include "pico_protocol.h"
+#include "pico_tree.h"
+
+
+static int pico_dev_cmp(void *ka, void *kb)
+{
+ struct pico_device *a = ka, *b = kb;
+ if (a->hash < b->hash)
+ return -1;
+ if (a->hash > b->hash)
+ return 1;
+ return 0;
+}
+
+PICO_TREE_DECLARE(Device_tree,pico_dev_cmp);
+
+int pico_device_init(struct pico_device *dev, const char *name, uint8_t *mac)
+{
+ int len = strlen(name);
+ if(len>MAX_DEVICE_NAME)
+ len = MAX_DEVICE_NAME;
+ memcpy(dev->name, name, len);
+ dev->hash = pico_hash(dev->name);
+
+ pico_tree_insert(&Device_tree,dev);
+ dev->q_in = pico_zalloc(sizeof(struct pico_queue));
+ dev->q_out = pico_zalloc(sizeof(struct pico_queue));
+
+ if (mac) {
+ dev->eth = pico_zalloc(sizeof(struct pico_ethdev));
+ memcpy(dev->eth->mac.addr, mac, PICO_SIZE_ETH);
+ } else {
+ dev->eth = NULL;
+ }
+
+ if (!dev->q_in || !dev->q_out || (mac && !dev->eth))
+ return -1;
+ return 0;
+}
+
+void pico_device_destroy(struct pico_device *dev)
+{
+ if (dev->destroy)
+ dev->destroy(dev);
+
+ if (dev->q_in) {
+ pico_queue_empty(dev->q_in);
+ pico_free(dev->q_in);
+ }
+ if (dev->q_out) {
+ pico_queue_empty(dev->q_out);
+ pico_free(dev->q_out);
+ }
+
+ if (dev->eth)
+ pico_free(dev->eth);
+
+ pico_tree_delete(&Device_tree,dev);
+ pico_free(dev);
+}
+
+static int devloop(struct pico_device *dev, int loop_score, int direction)
+{
+ struct pico_frame *f;
+
+ /* If device supports interrupts, read the value of the condition and trigger the dsr */
+ if ((dev->__serving_interrupt) && (dev->dsr)) {
+ /* call dsr routine */
+ loop_score = dev->dsr(dev, loop_score);
+ }
+
+ /* If device supports polling, give control. Loop score is managed internally,
+ * remaining loop points are returned. */
+ if (dev->poll) {
+ loop_score = dev->poll(dev, loop_score);
+ }
+
+ if (direction == PICO_LOOP_DIR_OUT) {
+
+ while(loop_score > 0) {
+ if (dev->q_out->frames <= 0)
+ break;
+
+ /* Device dequeue + send */
+ f = pico_dequeue(dev->q_out);
+ if (f) {
+ if (dev->eth) {
+ int ret = pico_ethernet_send(f);
+ if (0 == ret) {
+ loop_score--;
+ continue;
+ } if (ret < 0) {
+ if (!pico_source_is_local(f)) {
+ dbg("Destination unreachable -------> SEND ICMP\n");
+ pico_notify_dest_unreachable(f);
+ } else {
+ dbg("Destination unreachable -------> LOCAL\n");
+ }
+ pico_frame_discard(f);
+ continue;
+ }
+ } else {
+ dev->send(dev, f->start, f->len);
+ }
+ pico_frame_discard(f);
+ loop_score--;
+ }
+ }
+
+ } else if (direction == PICO_LOOP_DIR_IN) {
+
+ while(loop_score > 0) {
+ if (dev->q_in->frames <= 0)
+ break;
+
+ /* Receive */
+ f = pico_dequeue(dev->q_in);
+ if (f) {
+ if (dev->eth) {
+ f->datalink_hdr = f->buffer;
+ pico_ethernet_receive(f);
+ } else {
+ f->net_hdr = f->buffer;
+ pico_network_receive(f);
+ }
+ loop_score--;
+ }
+ }
+ }
+
+ return loop_score;
+}
+
+
+#define DEV_LOOP_MIN 16
+
+int pico_devices_loop(int loop_score, int direction)
+{
+ struct pico_device *start;
+ static struct pico_device *next = NULL, *next_in = NULL, *next_out = NULL;
+ static struct pico_tree_node * next_node, * in_node, * out_node;
+
+ if (next_in == NULL) {
+ in_node = pico_tree_firstNode(Device_tree.root);
+ next_in = in_node->keyValue;
+ }
+ if (next_out == NULL) {
+ out_node = pico_tree_firstNode(Device_tree.root);
+ next_out = out_node->keyValue;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ next_node = in_node;
+ next = next_in;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ next_node = out_node;
+ next = next_out;
+ }
+
+ /* init start node */
+ start = next;
+
+ /* round-robin all devices, break if traversed all devices */
+ while (loop_score > DEV_LOOP_MIN && next != NULL) {
+ loop_score = devloop(next, loop_score, direction);
+
+ next_node = pico_tree_next(next_node);
+ next = next_node->keyValue;
+
+ if (next == NULL)
+ {
+ next_node = pico_tree_firstNode(Device_tree.root);
+ next = next_node->keyValue;
+ }
+ if (next == start)
+ break;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ in_node = next_node;
+ next_in = next;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ out_node = next_node;
+ next_out = next;
+ }
+
+ return loop_score;
+}
+
+struct pico_device* pico_get_device(const char* name)
+{
+ struct pico_device *dev;
+ struct pico_tree_node * index;
+ pico_tree_foreach(index, &Device_tree){
+ dev = index->keyValue;
+ if(strcmp(name, dev->name) == 0)
+ return dev;
+ }
+ return NULL;
+}
+
+int pico_device_broadcast(struct pico_frame * f)
+{
+ struct pico_tree_node * index;
+ int ret = -1;
+
+ pico_tree_foreach(index,&Device_tree)
+ {
+ struct pico_device * dev = index->keyValue;
+ if(dev != f->dev)
+ {
+ struct pico_frame * copy = pico_frame_copy(f);
+
+ if(!copy)
+ return -1;
+ copy->dev = dev;
+ copy->dev->send(copy->dev, copy->start, copy->len);
+ pico_frame_discard(copy);
+ }
+ else
+ {
+ ret = f->dev->send(f->dev, f->start, f->len);
+ }
+ }
+
+ return ret;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_frame.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,201 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_config.h"
+#include "pico_frame.h"
+
+#ifdef PICO_SUPPORT_DEBUG_MEMORY
+static int n_frames_allocated;
+#endif
+
+/** frame alloc/dealloc/copy **/
+void pico_frame_discard(struct pico_frame *f)
+{
+ (*f->usage_count)--;
+ if (*f->usage_count <= 0) {
+ pico_free(f->usage_count);
+#ifdef PICO_SUPPORT_DEBUG_MEMORY
+ dbg("Discarded buffer @%p, caller: %p\n", f->buffer, __builtin_return_address(3));
+ dbg("DEBUG MEMORY: %d frames in use.\n", --n_frames_allocated);
+#endif
+ pico_free(f->buffer);
+ if (f->info)
+ pico_free(f->info);
+ }
+#ifdef PICO_SUPPORT_DEBUG_MEMORY
+ else {
+ dbg("Removed frame @%p(copy), usage count now: %d\n", f, *f->usage_count);
+ }
+#endif
+ pico_free(f);
+}
+
+struct pico_frame *pico_frame_copy(struct pico_frame *f)
+{
+ struct pico_frame *new = pico_zalloc(sizeof(struct pico_frame));
+ if (!new)
+ return NULL;
+ memcpy(new, f, sizeof(struct pico_frame));
+ *(new->usage_count) += 1;
+#ifdef PICO_SUPPORT_DEBUG_MEMORY
+ dbg("Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count);
+#endif
+ new->next = NULL;
+ return new;
+}
+
+
+struct pico_frame *pico_frame_alloc(int size)
+{
+ struct pico_frame *p = pico_zalloc(sizeof(struct pico_frame));
+ if (!p)
+ return NULL;
+ p->buffer = pico_zalloc(size);
+ if (!p->buffer) {
+ pico_free(p);
+ return NULL;
+ }
+ p->usage_count = pico_zalloc(sizeof(uint32_t));
+ if (!p->usage_count) {
+ pico_free(p->buffer);
+ pico_free(p);
+ return NULL;
+ }
+ p->buffer_len = size;
+
+
+ /* By default, frame content is the full buffer. */
+ p->start = p->buffer;
+ p->len = p->buffer_len;
+ *p->usage_count = 1;
+#ifdef PICO_SUPPORT_DEBUG_MEMORY
+ dbg("Allocated buffer @%p, len= %d caller: %p\n", p->buffer, p->buffer_len, __builtin_return_address(2));
+ dbg("DEBUG MEMORY: %d frames in use.\n", ++n_frames_allocated);
+#endif
+ return p;
+}
+
+struct pico_frame *pico_frame_deepcopy(struct pico_frame *f)
+{
+ struct pico_frame *new = pico_frame_alloc(f->buffer_len);
+ int addr_diff;
+ unsigned char *buf;
+ uint32_t *uc;
+ if (!new)
+ return NULL;
+
+ /* Save the two key pointers... */
+ buf = new->buffer;
+ uc = new->usage_count;
+
+ /* Overwrite all fields with originals */
+ memcpy(new, f, sizeof(struct pico_frame));
+
+ /* ...restore the two key pointers */
+ new->buffer = buf;
+ new->usage_count = uc;
+
+ /* Update in-buffer pointers with offset */
+ addr_diff = (int)(new->buffer - f->buffer);
+ new->net_hdr += addr_diff;
+ new->transport_hdr += addr_diff;
+ new->app_hdr += addr_diff;
+ new->start += addr_diff;
+ new->payload += addr_diff;
+
+#ifdef PICO_SUPPORT_DEBUG_MEMORY
+ dbg("Deep-Copied frame @%p, into %p, usage count now: %d\n", f, new, *new->usage_count);
+#endif
+ new->next = NULL;
+ return new;
+}
+
+/**
+ * Calculate checksum of a given string
+ */
+uint16_t pico_checksum(void *inbuf, int len)
+{
+ uint8_t *buf = (uint8_t *) inbuf;
+ uint16_t tmp = 0;
+ uint32_t sum = 0;
+ int i = 0;
+
+ for(i=0; i < len; i++) {
+ if (i%2) {
+ sum += buf[i];
+ } else {
+ tmp = buf[i];
+ sum += (tmp << 8);
+ }
+ }
+
+ while (sum >> 16) { /* a second carry is possible! */
+ sum = (sum & 0x0000FFFF) + (sum >> 16);
+ }
+ return (uint16_t) (~sum);
+}
+
+uint16_t pico_dualbuffer_checksum(void *inbuf1, int len1, void *inbuf2, int len2)
+{
+ uint8_t *b1 = (uint8_t *) inbuf1;
+ uint8_t *b2 = (uint8_t *) inbuf2;
+ uint16_t tmp = 0;
+ uint32_t sum = 0;
+ int i = 0, j = 0;
+
+ for(i=0; i < len1; i++) {
+ if (j%2) {
+ sum += b1[i];
+ } else {
+ tmp = b1[i];
+ sum += (tmp << 8);
+ }
+ j++;
+ }
+
+ j = 0; /* j has to be reset if len1 is odd */
+ for(i=0; i < len2; i++) {
+ if (j%2) {
+ sum += b2[i];
+ } else {
+ tmp = b2[i];
+ sum += (tmp << 8);
+ }
+ j++;
+ }
+ while (sum >> 16) { /* a second carry is possible! */
+ sum = (sum & 0x0000FFFF) + (sum >> 16);
+ }
+ return (uint16_t) (~sum);
+}
+
+uint16_t pico_dualbuffer_checksum_broken(void *inbuf1, int len1, void *inbuf2, int len2)
+{
+ uint16_t *b1 = (uint16_t *) inbuf1;
+ uint16_t *b2 = (uint16_t *) inbuf2;
+ uint32_t sum = 0;
+ int i=0, j=0;
+ for(i=0; i<(len1>>1); i++){
+ sum += short_be(b1[i]);
+ j++;
+ }
+ for(i=0; i<(len2>>1); i++){
+ sum += short_be(b2[i]);
+ j++;
+ }
+ sum = (sum & 0xFFFF) + (sum >> 16);
+ sum += (sum >> 16);
+
+ // Take the bitwise complement of sum
+ sum = ~sum;
+ return (uint16_t) (sum) ;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_protocol.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,350 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_protocol.h"
+#include "pico_tree.h"
+
+static int pico_proto_cmp(void *ka, void *kb)
+{
+ struct pico_protocol *a = ka, *b=kb;
+ if (a->hash < b->hash)
+ return -1;
+ if (a->hash > b->hash)
+ return 1;
+ return 0;
+}
+
+PICO_TREE_DECLARE(Datalink_proto_tree,pico_proto_cmp);
+PICO_TREE_DECLARE(Network_proto_tree,pico_proto_cmp);
+PICO_TREE_DECLARE(Transport_proto_tree,pico_proto_cmp);
+PICO_TREE_DECLARE(Socket_proto_tree,pico_proto_cmp);
+
+static int proto_loop(struct pico_protocol *proto, int loop_score, int direction)
+{
+ struct pico_frame *f;
+
+ if (direction == PICO_LOOP_DIR_IN) {
+
+ while(loop_score >0) {
+ if (proto->q_in->frames <= 0)
+ break;
+
+ f = pico_dequeue(proto->q_in);
+ if ((f) &&(proto->process_in(proto, f) > 0)) {
+ loop_score--;
+ }
+ }
+
+ } else if (direction == PICO_LOOP_DIR_OUT) {
+
+ while(loop_score >0) {
+ if (proto->q_out->frames <= 0)
+ break;
+
+ f = pico_dequeue(proto->q_out);
+ if ((f) &&(proto->process_out(proto, f) > 0)) {
+ loop_score--;
+ }
+ }
+ }
+
+ return loop_score;
+}
+
+#define DL_LOOP_MIN 1
+
+int pico_protocol_datalink_loop(int loop_score, int direction)
+{
+ struct pico_protocol *start;
+ static struct pico_protocol *next = NULL, *next_in = NULL, *next_out = NULL;
+ static struct pico_tree_node * next_node, * in_node, * out_node;
+
+ if (next_in == NULL) {
+ in_node = pico_tree_firstNode(Datalink_proto_tree.root);
+ if (in_node)
+ next_in = in_node->keyValue;
+ }
+ if (next_out == NULL) {
+ out_node = pico_tree_firstNode(Datalink_proto_tree.root);
+ if (out_node)
+ next_out = out_node->keyValue;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ next_node = in_node;
+ next = next_in;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ next_node = out_node;
+ next = next_out;
+ }
+
+ /* init start node */
+ start = next;
+
+ /* round-robin all datalink protocols, break if traversed all protocols */
+ while (loop_score > DL_LOOP_MIN && next != NULL) {
+ loop_score = proto_loop(next, loop_score, direction);
+
+ //next = RB_NEXT(pico_protocol_tree, &Datalink_proto_tree, next);
+ next_node = pico_tree_next(next_node);
+ next = next_node->keyValue;
+
+ if (next == NULL)
+ {
+ next_node = pico_tree_firstNode(Datalink_proto_tree.root);
+ next = next_node->keyValue;
+ }
+ if (next == start)
+ break;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ in_node = next_node;
+ next_in = next;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ out_node = next_node;
+ next_out = next;
+ }
+
+ return loop_score;
+}
+
+
+#define NW_LOOP_MIN 1
+
+int pico_protocol_network_loop(int loop_score, int direction)
+{
+ struct pico_protocol *start;
+ static struct pico_protocol *next = NULL, *next_in = NULL, *next_out = NULL;
+ static struct pico_tree_node * next_node, * in_node, * out_node;
+
+ if (next_in == NULL) {
+ in_node = pico_tree_firstNode(Network_proto_tree.root);
+ if (in_node)
+ next_in = in_node->keyValue;
+ }
+ if (next_out == NULL) {
+ out_node = pico_tree_firstNode(Network_proto_tree.root);
+ if (out_node)
+ next_out = out_node->keyValue;
+ }
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ next_node = in_node;
+ next = next_in;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ next_node = out_node;
+ next = next_out;
+ }
+
+ /* init start node */
+ start = next;
+
+ /* round-robin all network protocols, break if traversed all protocols */
+ while (loop_score > NW_LOOP_MIN && next != NULL) {
+ loop_score = proto_loop(next, loop_score, direction);
+
+ next_node = pico_tree_next(next_node);
+ next = next_node->keyValue;
+
+ if (next == NULL)
+ {
+ next_node = pico_tree_firstNode(Network_proto_tree.root);
+ next = next_node->keyValue;
+ }
+ if (next == start)
+ break;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ in_node = next_node;
+ next_in = next;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ out_node = next_node;
+ next_out = next;
+ }
+
+ return loop_score;
+}
+
+#define TP_LOOP_MIN 1
+
+int pico_protocol_transport_loop(int loop_score, int direction)
+{
+ struct pico_protocol *start;
+ static struct pico_protocol *next = NULL, *next_in = NULL, *next_out = NULL;
+ static struct pico_tree_node * next_node, * in_node, * out_node;
+
+ if (next_in == NULL) {
+ in_node = pico_tree_firstNode(Transport_proto_tree.root);
+ if (in_node)
+ next_in = in_node->keyValue;
+ }
+ if (next_out == NULL) {
+ out_node = pico_tree_firstNode(Transport_proto_tree.root);
+ if (out_node)
+ next_out = out_node->keyValue;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ next_node = in_node;
+ next = next_in;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ next_node = out_node;
+ next = next_out;
+ }
+
+ /* init start node */
+ start = next;
+
+ /* round-robin all transport protocols, break if traversed all protocols */
+ while (loop_score > DL_LOOP_MIN && next != NULL) {
+ loop_score = proto_loop(next, loop_score, direction);
+
+ //next = RB_NEXT(pico_protocol_tree, &Transport_proto_tree, next);
+ next_node = pico_tree_next(next_node);
+ next = next_node->keyValue;
+
+ if (next == NULL)
+ {
+ next_node = pico_tree_firstNode(Transport_proto_tree.root);
+ next = next_node->keyValue;
+ }
+ if (next == start)
+ break;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ in_node = next_node;
+ next_in = next;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ out_node = next_node;
+ next_out = next;
+ }
+
+ return loop_score;
+}
+
+
+#define SOCK_LOOP_MIN 1
+
+int pico_protocol_socket_loop(int loop_score, int direction)
+{
+ struct pico_protocol *start;
+ static struct pico_protocol *next = NULL, *next_in = NULL, *next_out = NULL;
+ static struct pico_tree_node * next_node, * in_node, * out_node;
+
+ if (next_in == NULL) {
+ in_node = pico_tree_firstNode(Socket_proto_tree.root);
+ if(in_node)
+ next_in = in_node->keyValue;
+ }
+ if (next_out == NULL) {
+ out_node = pico_tree_firstNode(Socket_proto_tree.root);
+ if(out_node)
+ next_out = out_node->keyValue;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ next_node = in_node;
+ next = next_in;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ next_node = out_node;
+ next = next_out;
+ }
+
+ /* init start node */
+ start = next;
+
+ /* round-robin all transport protocols, break if traversed all protocols */
+ while (loop_score > SOCK_LOOP_MIN && next != NULL) {
+ loop_score = proto_loop(next, loop_score,direction);
+
+ next_node = pico_tree_next(next_node);
+ next = next_node->keyValue;
+
+ if (next == NULL)
+ {
+ next_node = pico_tree_firstNode(next_node);
+ next = next_node->keyValue;
+ }
+ if (next == start)
+ break;
+ }
+
+ if (direction == PICO_LOOP_DIR_IN)
+ {
+ in_node = next_node;
+ next_in = next;
+ }
+ else if (direction == PICO_LOOP_DIR_OUT)
+ {
+ out_node = next_node;
+ next_out = next;
+ }
+
+ return loop_score;
+}
+
+int pico_protocols_loop(int loop_score)
+{
+/*
+ loop_score = pico_protocol_datalink_loop(loop_score);
+ loop_score = pico_protocol_network_loop(loop_score);
+ loop_score = pico_protocol_transport_loop(loop_score);
+ loop_score = pico_protocol_socket_loop(loop_score);
+*/
+ return loop_score;
+}
+
+void pico_protocol_init(struct pico_protocol *p)
+{
+ if (!p)
+ return;
+
+ p->hash = pico_hash(p->name);
+ switch (p->layer) {
+ case PICO_LAYER_DATALINK:
+ pico_tree_insert(&Datalink_proto_tree, p);
+ break;
+ case PICO_LAYER_NETWORK:
+ pico_tree_insert(&Network_proto_tree,p);
+ break;
+ case PICO_LAYER_TRANSPORT:
+ pico_tree_insert(&Transport_proto_tree,p);
+ break;
+ case PICO_LAYER_SOCKET:
+ pico_tree_insert(&Socket_proto_tree,p);
+ break;
+ }
+ dbg("Protocol %s registered (layer: %d).\n", p->name, p->layer);
+
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_socket.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,2342 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_config.h"
+#include "pico_queue.h"
+#include "pico_socket.h"
+#include "pico_ipv4.h"
+#include "pico_ipv6.h"
+#include "pico_udp.h"
+#include "pico_tcp.h"
+#include "pico_stack.h"
+#include "pico_icmp4.h"
+#include "pico_nat.h"
+#include "pico_tree.h"
+#include "pico_device.h"
+
+#if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6)
+#if defined (PICO_SUPPORT_TCP) || defined (PICO_SUPPORT_UDP)
+
+
+#define PROTO(s) ((s)->proto->proto_number)
+#define PICO_SOCKET4_MTU 1480 /* Ethernet MTU(1500) - IP header size(20) */
+#define PICO_SOCKET6_MTU 1460 /* Ethernet MTU(1500) - IP header size(40) */
+#define TCP_STATE(s) (s->state & PICO_SOCKET_STATE_TCP)
+
+#ifdef PICO_SUPPORT_MUTEX
+static void * Mutex = NULL;
+#define LOCK(x) {\
+ if (x == NULL) \
+ x = pico_mutex_init(); \
+ pico_mutex_lock(x); \
+}
+#define UNLOCK(x) pico_mutex_unlock(x);
+
+#else
+#define LOCK(x) do{}while(0)
+#define UNLOCK(x) do{}while(0)
+#endif
+
+
+#define PROTO(s) ((s)->proto->proto_number)
+
+#ifdef PICO_SUPPORT_TCP
+# define IS_NAGLE_ENABLED(s) (!(!(!(s->opt_flags & (1 << PICO_SOCKET_OPT_TCPNODELAY)))))
+#endif
+
+#define PICO_SOCKET_MTU 1480 /* Ethernet MTU(1500) - IP header size(20) */
+
+#ifdef PICO_SUPPORT_IPV4
+# define IS_SOCK_IPV4(s) ((s->net == &pico_proto_ipv4))
+#else
+# define IS_SOCK_IPV4(s) (0)
+#endif
+
+#ifdef PICO_SUPPORT_IPV6
+# define IS_SOCK_IPV6(s) ((s->net == &pico_proto_ipv6))
+#else
+# define IS_SOCK_IPV6(s) (0)
+#endif
+
+#ifdef PICO_SUPPORT_IPFRAG
+# define frag_dbg(...) do{}while(0)
+#endif
+
+#ifdef PICO_SUPPORT_MCAST
+# define so_mcast_dbg(...) do{}while(0) /* ip_mcast_dbg in pico_ipv4.c */
+#endif
+
+static struct pico_sockport *sp_udp = NULL ,*sp_tcp = NULL;
+
+struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, int len);
+
+static int socket_cmp(void * ka, void * kb)
+{
+ struct pico_socket *a = ka, *b = kb;
+ int a_is_ip6 = is_sock_ipv6(a);
+ int b_is_ip6 = is_sock_ipv6(b);
+
+ int diff;
+
+ /* First, order by network ver */
+ if (a_is_ip6 < b_is_ip6)
+ return -1;
+ if (a_is_ip6 > b_is_ip6)
+ return 1;
+
+ /* If either socket is PICO_IPV4_INADDR_ANY mode, skip local address comparison */
+
+ /* At this point, sort by local host */
+
+ if (0) {
+#ifdef PICO_SUPPORT_IPV6
+ } else if (a_is_ip6) {
+ if ((memcmp(a->local_addr.ip6.addr, PICO_IP6_ANY, PICO_SIZE_IP6)==0) || memcmp((b->local_addr.ip6.addr, PICO_IP6_ANY, PICO_SIZE_IP6) == 0))
+ diff = 0;
+ else
+ diff = memcmp(a->local_addr.ip6.addr, b->local_addr.ip6.addr, PICO_SIZE_IP6);
+#endif
+ } else {
+ if ((a->local_addr.ip4.addr == PICO_IP4_ANY) || (b->local_addr.ip4.addr == PICO_IP4_ANY))
+ diff = 0;
+ else
+ diff = a->local_addr.ip4.addr - b->local_addr.ip4.addr;
+ }
+
+ if (diff)
+ return diff;
+
+
+ /* Sort by remote host */
+ if (a_is_ip6)
+ diff = memcmp(a->remote_addr.ip6.addr, b->remote_addr.ip6.addr, PICO_SIZE_IP6);
+ else
+ diff = a->remote_addr.ip4.addr - b->remote_addr.ip4.addr;
+
+ if (diff)
+ return diff;
+
+ /* And finally by remote port. The two sockets are coincident if the quad is the same. */
+ return b->remote_port - a->remote_port;
+}
+
+struct pico_sockport
+{
+ struct pico_tree socks; // how you make the connection ?
+ uint16_t number;
+ uint16_t proto;
+};
+
+#define INIT_SOCKPORT { {&LEAF , socket_cmp}, 0, 0 }
+
+int sockport_cmp(void * ka, void * kb)
+{
+ struct pico_sockport *a = ka, *b = kb;
+ if (a->number < b->number)
+ return -1;
+ if (a->number > b->number)
+ return 1;
+ return 0;
+}
+
+PICO_TREE_DECLARE(UDPTable,sockport_cmp);
+PICO_TREE_DECLARE(TCPTable,sockport_cmp);
+
+#ifdef PICO_SUPPORT_MCAST
+/* socket
+ * |
+ * MCASTListen
+ * | | |
+ * ------------ | ------------
+ * | | |
+ * MCASTSources MCASTSources MCASTSources
+ * | | | | | | | | | | | |
+ * S S S S S S S S S S S S
+ *
+ * MCASTListen: RBTree(mcast_link, mcast_group)
+ * MCASTSources: RBTree(source)
+ */
+struct pico_mcast_listen
+{
+ uint8_t filter_mode;
+ struct pico_ip4 mcast_link;
+ struct pico_ip4 mcast_group;
+ struct pico_tree MCASTSources;
+};
+
+static int mcast_listen_cmp(void *ka, void *kb)
+{
+ struct pico_mcast_listen *a = ka, *b = kb;
+ if (a->mcast_group.addr < b->mcast_group.addr)
+ return -1;
+ if (a->mcast_group.addr > b->mcast_group.addr)
+ return 1;
+
+ if (a->mcast_link.addr < b->mcast_link.addr)
+ return -1;
+ if (a->mcast_link.addr > b->mcast_link.addr)
+ return 1;
+
+ return 0;
+}
+
+static int mcast_sources_cmp(void *ka, void *kb)
+{
+ struct pico_ip4 *a = ka, *b = kb;
+ if (a->addr < b->addr)
+ return -1;
+ if (a->addr > b->addr)
+ return 1;
+ return 0;
+}
+
+static int mcast_socket_cmp(void *ka, void *kb)
+{
+ struct pico_socket *a = ka, *b = kb;
+ if (a < b)
+ return -1;
+ if (a > b)
+ return 1;
+ return 0;
+}
+/* gather all multicast sockets to hasten filter aggregation */
+PICO_TREE_DECLARE(MCASTSockets, mcast_socket_cmp);
+
+static int mcast_filter_cmp(void *ka, void *kb)
+{
+ struct pico_ip4 *a = ka, *b = kb;
+ if (a->addr < b->addr)
+ return -1;
+ if (a->addr > b->addr)
+ return 1;
+ return 0;
+}
+/* gather sources to be filtered */
+PICO_TREE_DECLARE(MCASTFilter, mcast_filter_cmp);
+
+/* MCASTFilter will be empty if no socket is listening on mcast_group on mcast_link anymore */
+static int pico_socket_aggregate_mcastfilters(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group)
+{
+ uint8_t filter_mode = PICO_IP_MULTICAST_INCLUDE;
+ struct pico_mcast_listen *listen = NULL, ltest = {0};
+ struct pico_ip4 *source = NULL;
+ struct pico_socket *mcast_sock = NULL;
+ struct pico_tree_node *index = NULL, *_tmp = NULL, *index2 = NULL, *_tmp2 = NULL;
+
+ ltest.mcast_link = *mcast_link;
+ ltest.mcast_group = *mcast_group;
+
+ /* cleanup old filter */
+ pico_tree_foreach_safe(index, &MCASTFilter, _tmp)
+ {
+ pico_tree_delete(&MCASTFilter, index->keyValue);
+ }
+
+ /* construct new filter */
+ pico_tree_foreach(index, &MCASTSockets)
+ {
+ mcast_sock = index->keyValue;
+ listen = pico_tree_findKey(mcast_sock->MCASTListen, <est);
+ if (listen) {
+ /* aggregate filter */
+ switch(filter_mode)
+ {
+ case PICO_IP_MULTICAST_INCLUDE:
+ switch (listen->filter_mode)
+ {
+ case PICO_IP_MULTICAST_INCLUDE:
+ /* filter = summation of INCLUDEs */
+ /* mode stays INCLUDE, add all sources to filter */
+ pico_tree_foreach(index2, &listen->MCASTSources)
+ {
+ source = index2->keyValue;
+ pico_tree_insert(&MCASTFilter, source);
+ }
+ break;
+
+ case PICO_IP_MULTICAST_EXCLUDE:
+ /* filter = EXCLUDE - INCLUDE */
+ /* delete from the interface INCLUDE filter any source NOT in the socket EXCLUDE filter */
+ pico_tree_foreach_safe(index2, &MCASTFilter, _tmp2)
+ {
+ source = pico_tree_findKey(&listen->MCASTSources, index2->keyValue);
+ if (!source)
+ pico_tree_delete(&MCASTFilter, index2->keyValue);
+ }
+ /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */
+ filter_mode = PICO_IP_MULTICAST_EXCLUDE;
+ /* add to the interface EXCLUDE filter any socket source NOT in the former interface INCLUDE filter */
+ pico_tree_foreach(index2, &listen->MCASTSources)
+ {
+ source = pico_tree_insert(&MCASTFilter, index2->keyValue);
+ if (source)
+ pico_tree_delete(&MCASTFilter, source);
+ }
+ break;
+
+ default:
+ return -1;
+ }
+ break;
+
+ case PICO_IP_MULTICAST_EXCLUDE:
+ switch (listen->filter_mode)
+ {
+ case PICO_IP_MULTICAST_INCLUDE:
+ /* filter = EXCLUDE - INCLUDE */
+ /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */
+ /* remove from the interface EXCLUDE filter any source in the socket INCLUDE filter */
+ pico_tree_foreach(index2, &listen->MCASTSources)
+ {
+ source = pico_tree_findKey(&MCASTFilter, index2->keyValue);
+ if (source)
+ pico_tree_delete(&MCASTFilter, source);
+ }
+ break;
+
+ case PICO_IP_MULTICAST_EXCLUDE:
+ /* filter = intersection of EXCLUDEs */
+ /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */
+ /* remove from the interface EXCLUDE filter any source not in the socket EXCLUDE filter */
+ pico_tree_foreach_safe(index2, &MCASTFilter, _tmp2)
+ {
+ source = pico_tree_findKey(&listen->MCASTSources, index2->keyValue);
+ if (!source)
+ pico_tree_delete(&MCASTFilter, index2->keyValue);
+ }
+ break;
+
+ default:
+ return -1;
+ }
+ break;
+
+ default:
+ return -1;
+ }
+ }
+ }
+ return filter_mode;
+}
+
+static int pico_socket_mcast_filter(struct pico_socket *s, struct pico_ip4 *mcast_group, struct pico_ip4 *src)
+{
+ struct pico_ipv4_link *mcast_link = NULL;
+ struct pico_mcast_listen *listen = NULL, ltest = {0};
+ struct pico_tree_node *index = NULL;
+
+ /* no multicast enabled on socket */
+ if (!s->MCASTListen)
+ return 0;
+
+ mcast_link = pico_ipv4_link_get(&s->local_addr.ip4);
+ if (!mcast_link)
+ return -1;
+
+ ltest.mcast_link.addr = mcast_link->address.addr;
+ ltest.mcast_group = *mcast_group;
+ listen = pico_tree_findKey(s->MCASTListen, <est);
+ if (!listen)
+ return -1;
+
+ /* perform source filtering */
+ switch (listen->filter_mode)
+ {
+ case PICO_IP_MULTICAST_INCLUDE:
+ pico_tree_foreach(index, &listen->MCASTSources)
+ {
+ if (src->addr == ((struct pico_ip4 *)index->keyValue)->addr) {
+ so_mcast_dbg("MCAST: IP %08X in included socket source list\n", src->addr);
+ return 0;
+ }
+ }
+ so_mcast_dbg("MCAST: IP %08X NOT in included socket source list\n", src->addr);
+ return -1;
+ break;
+
+ case PICO_IP_MULTICAST_EXCLUDE:
+ pico_tree_foreach(index, &listen->MCASTSources)
+ {
+ if (src->addr == ((struct pico_ip4 *)index->keyValue)->addr) {
+ so_mcast_dbg("MCAST: IP %08X in excluded socket source list\n", src->addr);
+ return -1;
+ }
+ }
+ so_mcast_dbg("MCAST: IP %08X NOT in excluded socket source list\n", src->addr);
+ return 0;
+ break;
+
+ default:
+ return -1;
+ break;
+ }
+ return -1;
+}
+
+static inline struct pico_ipv4_link *pico_socket_setoption_mcastargs_validation(struct pico_ip_mreq *mreq, struct pico_ip_mreq_source *mreq_source)
+{
+ struct pico_ipv4_link *mcast_link = NULL;
+
+ if (!mreq && !mreq_source)
+ return NULL;
+
+ if (mreq) {
+ if (!mreq->mcast_group_addr.addr)
+ return NULL;
+ if (pico_ipv4_is_unicast(mreq->mcast_group_addr.addr))
+ return NULL;
+
+ if (!mreq->mcast_link_addr.addr) {
+ mcast_link = pico_ipv4_get_default_mcastlink();
+ if (!mcast_link)
+ return NULL;
+ } else {
+ mcast_link = pico_ipv4_link_get(&mreq->mcast_link_addr);
+ if (!mcast_link)
+ return NULL;
+ }
+ }
+ if (mreq_source) {
+ if (!mreq_source->mcast_group_addr.addr)
+ return NULL;
+ if (pico_ipv4_is_unicast(mreq_source->mcast_group_addr.addr))
+ return NULL;
+
+ if (!mreq_source->mcast_source_addr.addr)
+ return NULL;
+ if (!pico_ipv4_is_unicast(mreq_source->mcast_source_addr.addr))
+ return NULL;
+
+ if (!mreq_source->mcast_link_addr.addr) {
+ mcast_link = pico_ipv4_get_default_mcastlink();
+ if (!mcast_link)
+ return NULL;
+ } else {
+ mcast_link = pico_ipv4_link_get(&mreq_source->mcast_link_addr);
+ if (!mcast_link)
+ return NULL;
+ }
+ }
+ return mcast_link;
+}
+#else
+static int pico_socket_mcast_filter(struct pico_socket *s, struct pico_ip4 *mcast_group, struct pico_ip4 *src) {
+ return 0;
+}
+#endif /* PICO_SUPPORT_MCAST */
+
+static struct pico_sockport *pico_get_sockport(uint16_t proto, uint16_t port)
+{
+ struct pico_sockport test = INIT_SOCKPORT;
+ test.number = port;
+
+ if (proto == PICO_PROTO_UDP)
+ return pico_tree_findKey(&UDPTable,&test);
+
+ else if (proto == PICO_PROTO_TCP)
+ return pico_tree_findKey(&TCPTable,&test);
+
+ else return NULL;
+}
+
+int pico_is_port_free(uint16_t proto, uint16_t port, void *addr, void *net)
+{
+ struct pico_sockport *sp;
+ struct pico_ip4 ip;
+ sp = pico_get_sockport(proto, port);
+
+ if (!net)
+ net = &pico_proto_ipv4;
+
+ /** IPv6 (wip) ***/
+ if (net != &pico_proto_ipv4) {
+ dbg("IPV6!!!!!\n");
+ return (!sp);
+ }
+
+ /* IPv4 */
+#ifdef PICO_SUPPORT_NAT
+ if (pico_ipv4_nat_find(port, NULL, 0, proto)) {
+ dbg("In use by nat....\n");
+ return 0;
+ }
+#endif
+ if (addr)
+ ip.addr = ((struct pico_ip4 *)addr)->addr;
+ else
+ ip.addr = PICO_IPV4_INADDR_ANY;
+
+ if (ip.addr == PICO_IPV4_INADDR_ANY) {
+ if (!sp) return 1;
+ else {
+ dbg("In use, and asked for ANY\n");
+ return 0;
+ }
+ }
+ if (sp) {
+ struct pico_ip4 *s_local;
+ struct pico_tree_node *idx;
+ struct pico_socket *s;
+ pico_tree_foreach(idx, &sp->socks) {
+ s = idx->keyValue;
+ if (s->net == &pico_proto_ipv4) {
+ s_local = (struct pico_ip4*) &s->local_addr;
+ if ((s_local->addr == PICO_IPV4_INADDR_ANY) || (s_local->addr == ip.addr))
+ return 0;
+ }
+ }
+ }
+ return 1;
+}
+
+static int pico_check_socket(struct pico_socket *s)
+{
+ struct pico_sockport *test;
+ struct pico_socket *found;
+ struct pico_tree_node * index;
+
+ test = pico_get_sockport(PROTO(s), s->local_port);
+
+ if (!test) {
+ return -1;
+ }
+
+ pico_tree_foreach(index,&test->socks){
+ found = index->keyValue;
+ if (s == found) {
+ return 0;
+ }
+ }
+
+ return -1;
+}
+
+struct pico_socket* pico_sockets_find(uint16_t local,uint16_t remote)
+{
+ struct pico_socket * sock = NULL;
+ struct pico_tree_node *index = NULL;
+ struct pico_sockport *sp = NULL;
+
+ sp = pico_get_sockport(PICO_PROTO_TCP,local);
+ if(sp)
+ {
+ pico_tree_foreach(index,&sp->socks)
+ {
+ if( ((struct pico_socket *)index->keyValue)->remote_port == remote)
+ {
+ sock = (struct pico_socket *)index->keyValue;
+ break;
+ }
+ }
+ }
+
+ return sock;
+}
+
+
+int pico_socket_add(struct pico_socket *s)
+{
+ struct pico_sockport *sp = pico_get_sockport(PROTO(s), s->local_port);
+ LOCK(Mutex);
+ if (!sp) {
+ //dbg("Creating sockport..%04x\n", s->local_port); /* In comment due to spam during test */
+ sp = pico_zalloc(sizeof(struct pico_sockport));
+
+ if (!sp) {
+ pico_err = PICO_ERR_ENOMEM;
+ UNLOCK(Mutex);
+ return -1;
+ }
+ sp->proto = PROTO(s);
+ sp->number = s->local_port;
+ sp->socks.root = &LEAF;
+ sp->socks.compare = socket_cmp;
+
+ if (PROTO(s) == PICO_PROTO_UDP)
+ {
+ pico_tree_insert(&UDPTable,sp);
+ }
+ else if (PROTO(s) == PICO_PROTO_TCP)
+ {
+ pico_tree_insert(&TCPTable,sp);
+ }
+ }
+
+ pico_tree_insert(&sp->socks,s);
+ s->state |= PICO_SOCKET_STATE_BOUND;
+ UNLOCK(Mutex);
+#if DEBUG_SOCKET_TREE
+ {
+ struct pico_tree_node * index;
+ //RB_FOREACH(s, socket_tree, &sp->socks) {
+ pico_tree_foreach(index,&sp->socks){
+ s = index->keyValue;
+ dbg(">>>> List Socket lc=%hu rm=%hu\n", short_be(s->local_port), short_be(s->remote_port));
+ }
+
+ }
+#endif
+ return 0;
+}
+
+static void socket_clean_queues(struct pico_socket *sock)
+{
+ struct pico_frame * f_in = pico_dequeue(&sock->q_in);
+ struct pico_frame * f_out = pico_dequeue(&sock->q_out);
+ while(f_in || f_out)
+ {
+ if(f_in)
+ {
+ pico_frame_discard(f_in);
+ f_in = pico_dequeue(&sock->q_in);
+ }
+
+ if(f_out)
+ {
+ pico_frame_discard(f_out);
+ f_out = pico_dequeue(&sock->q_out);
+ }
+ }
+ // for tcp sockets go further and clean the sockets inside queue
+ if(sock->proto == &pico_proto_tcp)
+ pico_tcp_cleanup_queues(sock);
+}
+
+static void socket_garbage_collect(unsigned long now, void *arg)
+{
+ struct pico_socket *s = (struct pico_socket *) arg;
+ IGNORE_PARAMETER(now);
+ socket_clean_queues(s);
+ pico_free(s);
+}
+
+int pico_socket_del(struct pico_socket *s)
+{
+ struct pico_sockport *sp = pico_get_sockport(PROTO(s), s->local_port);
+
+ if (!sp) {
+ pico_err = PICO_ERR_ENXIO;
+ return -1;
+ }
+
+ LOCK(Mutex);
+ pico_tree_delete(&sp->socks,s);
+ s->net = NULL;
+ if(pico_tree_empty(&sp->socks)){
+ if (PROTO(s) == PICO_PROTO_UDP)
+ {
+ pico_tree_delete(&UDPTable,sp);
+ }
+ else if (PROTO(s) == PICO_PROTO_TCP)
+ {
+ pico_tree_delete(&TCPTable,sp);
+ }
+
+ if(sp_tcp == sp) sp_tcp = NULL;
+
+ if(sp_udp == sp) sp_udp = NULL;
+
+ pico_free(sp);
+
+ }
+
+#ifdef PICO_SUPPORT_MCAST
+ do {
+ int filter_mode;
+ struct pico_tree_node *index = NULL, *_tmp = NULL, *index2 = NULL, *_tmp2 = NULL;
+ struct pico_mcast_listen *listen = NULL;
+ struct pico_ip4 *source = NULL;
+ if (s->MCASTListen) {
+ pico_tree_delete(&MCASTSockets, s);
+ pico_tree_foreach_safe(index, s->MCASTListen, _tmp)
+ {
+ listen = index->keyValue;
+ pico_tree_foreach_safe(index2, &listen->MCASTSources, _tmp2)
+ {
+ source = index->keyValue;
+ pico_tree_delete(&listen->MCASTSources, source);
+ pico_free(source);
+ }
+ filter_mode = pico_socket_aggregate_mcastfilters(&listen->mcast_link, &listen->mcast_group);
+ if (filter_mode >= 0)
+ pico_ipv4_mcast_leave(&listen->mcast_link, &listen->mcast_group, 1, filter_mode, &MCASTFilter);
+ pico_tree_delete(s->MCASTListen, listen);
+ pico_free(listen);
+ }
+ pico_free(s->MCASTListen);
+ }
+ } while (0);
+#endif
+
+ s->state = PICO_SOCKET_STATE_CLOSED;
+ pico_timer_add(3000, socket_garbage_collect, s);
+ UNLOCK(Mutex);
+ return 0;
+}
+
+static int pico_socket_alter_state(struct pico_socket *s, uint16_t more_states, uint16_t less_states, uint16_t tcp_state)
+{
+ struct pico_sockport *sp;
+ if (more_states & PICO_SOCKET_STATE_BOUND)
+ return pico_socket_add(s);
+
+ if (less_states & PICO_SOCKET_STATE_BOUND)
+ return pico_socket_del(s);
+
+ sp = pico_get_sockport(PROTO(s), s->local_port);
+ if (!sp) {
+ pico_err = PICO_ERR_ENXIO;
+ return -1;
+ }
+
+ s->state |= more_states;
+ s->state &= (~less_states);
+ if (tcp_state) {
+ s->state &= 0x00FF;
+ s->state |= tcp_state;
+ }
+
+ return 0;
+}
+
+static int pico_socket_deliver(struct pico_protocol *p, struct pico_frame *f, uint16_t localport)
+{
+ struct pico_frame *cpy = NULL;
+ struct pico_sockport *sp = NULL;
+ struct pico_socket *s = NULL, *found = NULL;
+ struct pico_tree_node *index = NULL;
+ struct pico_tree_node *_tmp;
+ struct pico_trans *tr = (struct pico_trans *) f->transport_hdr;
+ #ifdef PICO_SUPPORT_IPV4
+ struct pico_ipv4_hdr *ip4hdr;
+ #endif
+ #ifdef PICO_SUPPORT_IPV6
+ struct pico_ipv6_hdr *ip6hdr;
+ #endif
+
+ if (!tr)
+ return -1;
+
+ sp = pico_get_sockport(p->proto_number, localport);
+
+ if (!sp) {
+ dbg("No such port %d\n",short_be(localport));
+ return -1;
+ }
+
+ #ifdef PICO_SUPPORT_TCP
+ if (p->proto_number == PICO_PROTO_TCP) {
+ pico_tree_foreach_safe(index,&sp->socks, _tmp){
+ s = index->keyValue;
+ /* 4-tuple identification of socket (port-IP) */
+ #ifdef PICO_SUPPORT_IPV4
+ if (IS_IPV4(f)) {
+ struct pico_ip4 s_local, s_remote, p_src, p_dst;
+ ip4hdr = (struct pico_ipv4_hdr*)(f->net_hdr);
+ s_local.addr = s->local_addr.ip4.addr;
+ s_remote.addr = s->remote_addr.ip4.addr;
+ p_src.addr = ip4hdr->src.addr;
+ p_dst.addr = ip4hdr->dst.addr;
+ if ( (s->remote_port == tr->sport) && /* remote port check */
+ (s_remote.addr == p_src.addr) && /* remote addr check */
+ ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr))) { /* Either local socket is ANY, or matches dst */
+ found = s;
+ break;
+ } else if ( (s->remote_port == 0) && /* not connected... listening */
+ ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr))) { /* Either local socket is ANY, or matches dst */
+ /* listen socket */
+ found = s;
+ }
+ }
+ #endif
+ #ifdef PICO_SUPPORT_IPV6 /* XXX TODO make compare for ipv6 addresses */
+ if (IS_IPV6(f)) {
+ ip6hdr = (struct pico_ipv6_hdr*)(f->net_hdr);
+ if ( (s->remote_port == localport) ) { // && (((struct pico_ip6) s->remote_addr.ip6).addr == ((struct pico_ip6)(ip6hdr->src)).addr) ) {
+ found = s;
+ break;
+ } else if (s->remote_port == 0) {
+ /* listen socket */
+ found = s;
+ }
+ }
+ #endif
+ } /* FOREACH */
+ if (found != NULL) {
+ pico_tcp_input(found,f);
+ if ((found->ev_pending) && found->wakeup) {
+ found->wakeup(found->ev_pending, found);
+ }
+ return 0;
+ } else {
+ dbg("SOCKET> mmm something wrong (prob sockport)\n");
+ return -1;
+ }
+ } /* TCP CASE */
+#endif
+
+#ifdef PICO_SUPPORT_UDP
+ if (p->proto_number == PICO_PROTO_UDP) {
+ pico_tree_foreach_safe(index,&sp->socks, _tmp){
+ s = index->keyValue;
+ if (IS_IPV4(f)) { /* IPV4 */
+ struct pico_ip4 s_local, p_dst;
+ ip4hdr = (struct pico_ipv4_hdr*)(f->net_hdr);
+ s_local.addr = s->local_addr.ip4.addr;
+ p_dst.addr = ip4hdr->dst.addr;
+ if ((pico_ipv4_is_broadcast(p_dst.addr)) || pico_ipv4_is_multicast(p_dst.addr)) {
+ struct pico_device *dev = pico_ipv4_link_find(&s->local_addr.ip4);
+ if (pico_ipv4_is_multicast(p_dst.addr) && (pico_socket_mcast_filter(s, &ip4hdr->dst, &ip4hdr->src) < 0))
+ return -1;
+ if ((s_local.addr == PICO_IPV4_INADDR_ANY) || /* If our local ip is ANY, or.. */
+ (dev == f->dev) ) { /* the source of the bcast packet is a neighbor... */
+ cpy = pico_frame_copy(f);
+ if (!cpy)
+ return -1;
+ if (pico_enqueue(&s->q_in, cpy) > 0) {
+ if (s->wakeup)
+ s->wakeup(PICO_SOCK_EV_RD, s);
+ }
+ }
+ } else if ((s_local.addr == PICO_IPV4_INADDR_ANY) || (s_local.addr == p_dst.addr))
+ { /* Either local socket is ANY, or matches dst */
+ cpy = pico_frame_copy(f);
+ if (!cpy)
+ return -1;
+ if (pico_enqueue(&s->q_in, cpy) > 0) {
+ if (s->wakeup)
+ s->wakeup(PICO_SOCK_EV_RD, s);
+ }
+ }
+ } else {
+ /*... IPv6 */
+ }
+ } /* FOREACH */
+ pico_frame_discard(f);
+ if (s)
+ return 0;
+ else
+ return -1;
+ }
+#endif
+ return -1;
+}
+
+struct pico_socket *pico_socket_open(uint16_t net, uint16_t proto, void (*wakeup)(uint16_t ev, struct pico_socket *))
+{
+
+ struct pico_socket *s = NULL;
+
+#ifdef PICO_SUPPORT_UDP
+ if (proto == PICO_PROTO_UDP) {
+ s = pico_udp_open();
+ s->proto = &pico_proto_udp;
+ }
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+ if (proto == PICO_PROTO_TCP) {
+ s = pico_tcp_open();
+ s->proto = &pico_proto_tcp;
+ /*check if Nagle enabled */
+ if (!IS_NAGLE_ENABLED(s))
+ dbg("ERROR Nagle should be enabled here\n\n");
+ }
+#endif
+
+ if (!s) {
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return NULL;
+ }
+
+#ifdef PICO_SUPPORT_IPV4
+ if (net == PICO_PROTO_IPV4)
+ s->net = &pico_proto_ipv4;
+#endif
+
+#ifdef PICO_SUPPORT_IPV6
+ if (net == PICO_PROTO_IPV6)
+ s->net = &pico_proto_ipv6;
+#endif
+
+ s->q_in.max_size = PICO_DEFAULT_SOCKETQ;
+ s->q_out.max_size = PICO_DEFAULT_SOCKETQ;
+ s->wakeup = wakeup;
+
+ if (!s->net) {
+ pico_free(s);
+ pico_err = PICO_ERR_ENETUNREACH;
+ return NULL;
+ }
+ return s;
+}
+
+
+struct pico_socket *pico_socket_clone(struct pico_socket *facsimile)
+{
+ struct pico_socket *s = NULL;
+
+#ifdef PICO_SUPPORT_UDP
+ if (facsimile->proto->proto_number == PICO_PROTO_UDP) {
+ s = pico_udp_open();
+ s->proto = &pico_proto_udp;
+ }
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+ if (facsimile->proto->proto_number == PICO_PROTO_TCP) {
+ s = pico_tcp_open();
+ s->proto = &pico_proto_tcp;
+ }
+#endif
+
+ if (!s) {
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return NULL;
+ }
+ s->local_port = facsimile->local_port;
+ s->remote_port = facsimile->remote_port;
+ s->state = facsimile->state;
+
+#ifdef PICO_SUPPORT_IPV4
+ if (facsimile->net == &pico_proto_ipv4) {
+ s->net = &pico_proto_ipv4;
+ memcpy(&s->local_addr, &facsimile->local_addr, sizeof(struct pico_ip4));
+ memcpy(&s->remote_addr, &facsimile->remote_addr, sizeof(struct pico_ip4));
+ }
+#endif
+
+#ifdef PICO_SUPPORT_IPV6
+ if (net == &pico_proto_ipv6) {
+ s->net = &pico_proto_ipv6;
+ memcpy(&s->local_addr, &facsimile->local_addr, sizeof(struct pico_ip6));
+ memcpy(&s->remote_addr, &facsimile->remote_addr, sizeof(struct pico_ip6));
+ }
+#endif
+ s->q_in.max_size = PICO_DEFAULT_SOCKETQ;
+ s->q_out.max_size = PICO_DEFAULT_SOCKETQ;
+ s->wakeup = NULL;
+ if (!s->net) {
+ pico_free(s);
+ pico_err = PICO_ERR_ENETUNREACH;
+ return NULL;
+ }
+ return s;
+}
+
+int pico_socket_read(struct pico_socket *s, void *buf, int len)
+{
+ if (!s || buf == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ /* check if exists in tree */
+ /* See task #178 */
+ if (pico_check_socket(s) != 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+
+ if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) {
+ pico_err = PICO_ERR_EIO;
+ return -1;
+ }
+#ifdef PICO_SUPPORT_UDP
+ if (PROTO(s) == PICO_PROTO_UDP)
+ return pico_udp_recv(s, buf, len, NULL, NULL);
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+ if (PROTO(s) == PICO_PROTO_TCP){
+ /* check if in shutdown state and if no more data in tcpq_in */
+ if ((s->state & PICO_SOCKET_STATE_SHUT_REMOTE) && pico_tcp_queue_in_is_empty(s) ) {
+ pico_err = PICO_ERR_ESHUTDOWN;
+ return -1;
+ } else {
+ return pico_tcp_read(s, buf, len);
+ }
+ }
+#endif
+ return 0;
+}
+
+int pico_socket_write(struct pico_socket *s, const void *buf, int len)
+{
+ if (!s || buf == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ /* check if exists in tree */
+ /* See task #178 */
+ if (pico_check_socket(s) != 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+
+ if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) {
+ pico_err = PICO_ERR_EIO;
+ return -1;
+ }
+ if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) {
+ pico_err = PICO_ERR_ENOTCONN;
+ return -1;
+ } else if (s->state & PICO_SOCKET_STATE_SHUT_LOCAL) { /* check if in shutdown state */
+ pico_err = PICO_ERR_ESHUTDOWN;
+ return -1;
+ } else {
+ return pico_socket_sendto(s, buf, len, &s->remote_addr, s->remote_port);
+ }
+}
+
+uint16_t pico_socket_high_port(uint16_t proto)
+{
+ uint16_t port;
+ if (0 ||
+#ifdef PICO_SUPPORT_TCP
+ (proto == PICO_PROTO_TCP) ||
+#endif
+#ifdef PICO_SUPPORT_TCP
+ (proto == PICO_PROTO_UDP) ||
+#endif
+ 0) {
+ do {
+ uint32_t rand = pico_rand();
+ port = (uint16_t) (rand & 0xFFFFU);
+ port = (uint16_t)(port % (65535 - 1024)) + 1024U;
+ if (pico_is_port_free(proto, port, NULL, NULL)) {
+ return short_be(port);
+ }
+ } while(1);
+ }
+ else return 0U;
+}
+
+
+int pico_socket_sendto(struct pico_socket *s, const void *buf, const int len, void *dst, uint16_t remote_port)
+{
+ struct pico_frame *f;
+ struct pico_remote_duple *remote_duple = NULL;
+ int socket_mtu = PICO_SOCKET4_MTU;
+ int header_offset = 0;
+ int total_payload_written = 0;
+#ifdef PICO_SUPPORT_IPV4
+ struct pico_ip4 *src4;
+#endif
+
+#ifdef PICO_SUPPORT_IPV6
+ struct pico_ip6 *src6;
+#endif
+ if (len == 0) {
+ return 0;
+ } else if (len < 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if (buf == NULL || s == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if (!dst || !remote_port) {
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ }
+
+ if ((s->state & PICO_SOCKET_STATE_CONNECTED) != 0) {
+ if (remote_port != s->remote_port) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+
+#ifdef PICO_SUPPORT_IPV4
+ if (IS_SOCK_IPV4(s)) {
+ socket_mtu = PICO_SOCKET4_MTU;
+ if ((s->state & PICO_SOCKET_STATE_CONNECTED)) {
+ if (s->remote_addr.ip4.addr != ((struct pico_ip4 *)dst)->addr ) {
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ }
+ } else {
+ src4 = pico_ipv4_source_find(dst);
+ if (!src4) {
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ return -1;
+ }
+ if (src4->addr != PICO_IPV4_INADDR_ANY)
+ s->local_addr.ip4.addr = src4->addr;
+# ifdef PICO_SUPPORT_UDP
+ /* socket remote info could change in a consecutive call, make persistent */
+ if (PROTO(s) == PICO_PROTO_UDP) {
+ remote_duple = pico_zalloc(sizeof(struct pico_remote_duple));
+ remote_duple->remote_addr.ip4.addr = ((struct pico_ip4 *)dst)->addr;
+ remote_duple->remote_port = remote_port;
+ }
+# endif
+ }
+ #endif
+ } else if (IS_SOCK_IPV6(s)) {
+ socket_mtu = PICO_SOCKET6_MTU;
+ #ifdef PICO_SUPPORT_IPV6
+ if (s->state & PICO_SOCKET_STATE_CONNECTED) {
+ if (memcmp(&s->remote_addr, dst, PICO_SIZE_IP6))
+ return -1;
+ } else {
+ src6 = pico_ipv6_source_find(dst);
+ if (!src6) {
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ return -1;
+ }
+ memcpy(&s->local_addr, src6, PICO_SIZE_IP6);
+ memcpy(&s->remote_addr, dst, PICO_SIZE_IP6);
+# ifdef PICO_SUPPORT_UDP
+ if (PROTO(s) == PICO_PROTO_UDP) {
+ remote_duple = pico_zalloc(sizeof(struct pico_remote_duple));
+ remote_duple->remote_addr.ip6.addr = ((struct pico_ip6 *)dst)->addr;
+ remote_duple->remote_port = remote_port;
+ }
+# endif
+ }
+#endif
+ }
+
+ if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) {
+ s->local_port = pico_socket_high_port(s->proto->proto_number);
+ if (s->local_port == 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+ if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) {
+ s->remote_port = remote_port;
+ }
+
+#ifdef PICO_SUPPORT_TCP
+ if (PROTO(s) == PICO_PROTO_TCP)
+ header_offset = pico_tcp_overhead(s);
+#endif
+
+#ifdef PICO_SUPPORT_UDP
+ if (PROTO(s) == PICO_PROTO_UDP)
+ header_offset = sizeof(struct pico_udp_hdr);
+#endif
+
+ while (total_payload_written < len) {
+ int transport_len = (len - total_payload_written) + header_offset;
+ if (transport_len > socket_mtu)
+ transport_len = socket_mtu;
+ #ifdef PICO_SUPPORT_IPFRAG
+ else {
+ if (total_payload_written)
+ transport_len -= header_offset; /* last fragment, do not allocate memory for transport header */
+ }
+#endif /* PICO_SUPPORT_IPFRAG */
+
+ f = pico_socket_frame_alloc(s, transport_len);
+ if (!f) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ f->payload += header_offset;
+ f->payload_len -= header_offset;
+ f->sock = s;
+ if (remote_duple) {
+ f->info = pico_zalloc(sizeof(struct pico_remote_duple));
+ memcpy(f->info, remote_duple, sizeof(struct pico_remote_duple));
+ }
+
+ #ifdef PICO_SUPPORT_IPFRAG
+ #ifdef PICO_SUPPORT_UDP
+ if (PROTO(s) == PICO_PROTO_UDP && ((len + header_offset) > socket_mtu)) {
+ /* hacking way to identify fragmentation frames: payload != transport_hdr -> first frame */
+ if (!total_payload_written) {
+ frag_dbg("FRAG: first fragmented frame %p | len = %u offset = 0\n", f, f->payload_len);
+ /* transport header length field contains total length + header length */
+ f->transport_len = len + header_offset;
+ f->frag = short_be(PICO_IPV4_MOREFRAG);
+ } else {
+ /* no transport header in fragmented IP */
+ f->payload = f->transport_hdr;
+ f->payload_len += header_offset;
+ /* set offset in octets */
+ f->frag = short_be((total_payload_written + header_offset) / 8);
+ if (total_payload_written + f->payload_len < len) {
+ frag_dbg("FRAG: intermediate fragmented frame %p | len = %u offset = %u\n", f, f->payload_len, short_be(f->frag));
+ f->frag |= short_be(PICO_IPV4_MOREFRAG);
+ } else {
+ frag_dbg("FRAG: last fragmented frame %p | len = %u offset = %u\n", f, f->payload_len, short_be(f->frag));
+ f->frag &= short_be(PICO_IPV4_FRAG_MASK);
+ }
+ }
+ } else {
+ f->frag = short_be(PICO_IPV4_DONTFRAG);
+ }
+# endif /* PICO_SUPPORT_UDP */
+#endif /* PICO_SUPPORT_IPFRAG */
+
+ if (f->payload_len <= 0) {
+ pico_frame_discard(f);
+ if (remote_duple)
+ pico_free(remote_duple);
+ return total_payload_written;
+ }
+
+ memcpy(f->payload, buf + total_payload_written, f->payload_len);
+ //dbg("Pushing segment, hdr len: %d, payload_len: %d\n", header_offset, f->payload_len);
+
+ if (s->proto->push(s->proto, f) > 0) {
+ total_payload_written += f->payload_len;
+ } else {
+ pico_frame_discard(f);
+ pico_err = PICO_ERR_EAGAIN;
+ break;
+ }
+ }
+ if (remote_duple)
+ pico_free(remote_duple);
+ return total_payload_written;
+}
+
+int pico_socket_send(struct pico_socket *s, const void *buf, int len)
+{
+ if (!s || buf == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ /* check if exists in tree */
+ /* See task #178 */
+ if (pico_check_socket(s) != 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+
+ if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) {
+ pico_err = PICO_ERR_ENOTCONN;
+ return -1;
+ }
+ return pico_socket_sendto(s, buf, len, &s->remote_addr, s->remote_port);
+}
+
+int pico_socket_recvfrom(struct pico_socket *s, void *buf, int len, void *orig, uint16_t *remote_port)
+{
+ if (!s || buf == NULL) { /// || orig == NULL || remote_port == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ /* check if exists in tree */
+ if (pico_check_socket(s) != 0) {
+ pico_err = PICO_ERR_EINVAL;
+ /* See task #178 */
+ return -1;
+ }
+ }
+
+ if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) {
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ }
+#ifdef PICO_SUPPORT_UDP
+ if (PROTO(s) == PICO_PROTO_UDP) {
+ return pico_udp_recv(s, buf, len, orig, remote_port);
+ }
+#endif
+#ifdef PICO_SUPPORT_TCP
+ if (PROTO(s) == PICO_PROTO_TCP) {
+ /* check if in shutdown state and if tcpq_in empty */
+ if ((s->state & PICO_SOCKET_STATE_SHUT_REMOTE) && pico_tcp_queue_in_is_empty(s)) {
+ pico_err = PICO_ERR_ESHUTDOWN;
+ return -1;
+ } else {
+ //dbg("socket tcp recv\n");
+ return pico_tcp_read(s, buf, len);
+ }
+ }
+#endif
+ //dbg("socket return 0\n");
+ return 0;
+}
+
+int pico_socket_recv(struct pico_socket *s, void *buf, int len)
+{
+ return pico_socket_recvfrom(s, buf, len, NULL, NULL);
+}
+
+
+int pico_socket_bind(struct pico_socket *s, void *local_addr, uint16_t *port)
+{
+ if (!s || !local_addr || !port) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if (!is_sock_ipv6(s)) {
+ struct pico_ip4 *ip = (struct pico_ip4 *)local_addr;
+ if (ip->addr != PICO_IPV4_INADDR_ANY) {
+ if (!pico_ipv4_link_find(local_addr)) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+ } else {
+ /*... IPv6 */
+ }
+
+
+ /* When given port = 0, get a random high port to bind to. */
+ if (*port == 0) {
+ *port = pico_socket_high_port(PROTO(s));
+ if (*port == 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+
+ if (pico_is_port_free(PROTO(s), *port, local_addr, s->net) == 0) {
+ pico_err = PICO_ERR_EADDRINUSE;
+ return -1;
+ }
+ s->local_port = *port;
+
+ if (is_sock_ipv6(s)) {
+ struct pico_ip6 *ip = (struct pico_ip6 *) local_addr;
+ memcpy(s->local_addr.ip6.addr, ip, PICO_SIZE_IP6);
+ /* XXX: port ipv4 functionality to ipv6 */
+ /* Check for port already in use */
+ if (pico_is_port_free(PROTO(s), *port, &local_addr, s->net)) {
+ pico_err = PICO_ERR_EADDRINUSE;
+ return -1;
+ }
+ } else if (is_sock_ipv4(s)) {
+ struct pico_ip4 *ip = (struct pico_ip4 *) local_addr;
+ s->local_addr.ip4.addr = ip->addr;
+ }
+ return pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0);
+}
+
+int pico_socket_connect(struct pico_socket *s, const void *remote_addr, uint16_t remote_port)
+{
+ int ret = -1;
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ if (!s || remote_addr == NULL || remote_port == 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ s->remote_port = remote_port;
+
+ if (s->local_port == 0) {
+ s->local_port = pico_socket_high_port(PROTO(s));
+ if (!s->local_port) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+
+ if (is_sock_ipv6(s)) {
+ const struct pico_ip6 *ip = (const struct pico_ip6 *) remote_addr;
+ memcpy(s->remote_addr.ip6.addr, ip, PICO_SIZE_IP6);
+ } else if (is_sock_ipv4(s)) {
+ const struct pico_ip4 *local, *ip = (const struct pico_ip4 *) remote_addr;
+ s->remote_addr.ip4.addr = ip->addr;
+ local = pico_ipv4_source_find(ip);
+ if (local) {
+ s->local_addr.ip4.addr = local->addr;
+ } else {
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ return -1;
+ }
+ }
+
+ pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0);
+
+#ifdef PICO_SUPPORT_UDP
+ if (PROTO(s) == PICO_PROTO_UDP) {
+ pico_socket_alter_state(s, PICO_SOCKET_STATE_CONNECTED, 0, 0);
+ pico_err = PICO_ERR_NOERR;
+ ret = 0;
+ }
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+ if (PROTO(s) == PICO_PROTO_TCP) {
+ if (pico_tcp_initconn(s) == 0) {
+ pico_socket_alter_state(s, PICO_SOCKET_STATE_CONNECTED | PICO_SOCKET_STATE_TCP_SYN_SENT, 0, 0);
+ pico_err = PICO_ERR_NOERR;
+ ret = 0;
+ } else {
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ }
+ }
+#endif
+
+ return ret;
+}
+
+#ifdef PICO_SUPPORT_TCP
+
+int pico_socket_listen(struct pico_socket *s, int backlog)
+{
+ if (!s || backlog < 1) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ /* check if exists in tree */
+ /* See task #178 */
+ if (pico_check_socket(s) != 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+
+ if (PROTO(s) == PICO_PROTO_UDP) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) {
+ pico_err = PICO_ERR_EISCONN;
+ return -1;
+ }
+
+ if (backlog < 1) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if (PROTO(s) == PICO_PROTO_TCP)
+ pico_socket_alter_state(s, PICO_SOCKET_STATE_TCP_SYN_SENT, 0, PICO_SOCKET_STATE_TCP_LISTEN);
+ s->max_backlog = backlog;
+
+ return 0;
+}
+
+struct pico_socket *pico_socket_accept(struct pico_socket *s, void *orig, uint16_t *port)
+{
+ if (!s || !orig || !port) {
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+ }
+
+ pico_err = PICO_ERR_EINVAL;
+
+ if ((s->state & PICO_SOCKET_STATE_BOUND) == 0) {
+ return NULL;
+ }
+
+ if (PROTO(s) == PICO_PROTO_UDP) {
+ return NULL;
+ }
+
+ if (TCPSTATE(s) == PICO_SOCKET_STATE_TCP_LISTEN) {
+ struct pico_sockport *sp = pico_get_sockport(PICO_PROTO_TCP, s->local_port);
+ struct pico_socket *found;
+ /* If at this point no incoming connection socket is found,
+ * the accept call is valid, but no connection is established yet.
+ */
+ pico_err = PICO_ERR_EAGAIN;
+ if (sp) {
+ struct pico_tree_node * index;
+ //RB_FOREACH(found, socket_tree, &sp->socks) {
+ pico_tree_foreach(index,&sp->socks){
+ found = index->keyValue;
+ if (s == found->parent) {
+ found->parent = NULL;
+ pico_err = PICO_ERR_NOERR;
+ memcpy(orig, &found->remote_addr, sizeof(struct pico_ip4));
+ *port = found->remote_port;
+ return found;
+ }
+ }
+ }
+ }
+ return NULL;
+}
+
+#else
+
+int pico_socket_listen(struct pico_socket *s, int backlog)
+{
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+}
+
+struct pico_socket *pico_socket_accept(struct pico_socket *s, void *orig, uint16_t *local_port)
+{
+ pico_err = PICO_ERR_EINVAL;
+ return NULL;
+}
+
+#endif
+
+#define PICO_SOCKET_SETOPT_EN(socket,index) (socket->opt_flags |= (1 << index))
+#define PICO_SOCKET_SETOPT_DIS(socket,index) (socket->opt_flags &= ~(1 << index))
+
+int pico_socket_setoption(struct pico_socket *s, int option, void *value) // XXX no check against proto (vs setsockopt) or implicit by socket?
+{
+ if (s == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ pico_err = PICO_ERR_NOERR;
+
+ switch (option)
+ {
+#ifdef PICO_SUPPORT_TCP
+ case PICO_TCP_NODELAY:
+ if (!value) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (s->proto->proto_number == PICO_PROTO_TCP) {
+ int *val = (int*)value;
+ if (*val > 0) {
+ dbg("setsockopt: Nagle algorithm disabled.\n");
+ PICO_SOCKET_SETOPT_EN(s,PICO_SOCKET_OPT_TCPNODELAY);
+ } else {
+ dbg("setsockopt: Nagle algorithm enabled.\n");
+ PICO_SOCKET_SETOPT_DIS(s,PICO_SOCKET_OPT_TCPNODELAY);
+ }
+ } else {
+ pico_err = PICO_ERR_EINVAL;
+ }
+ break;
+#endif
+
+
+#ifdef PICO_SUPPORT_MCAST
+ case PICO_IP_MULTICAST_IF:
+ pico_err = PICO_ERR_EOPNOTSUPP;
+ return -1;
+ break;
+
+ case PICO_IP_MULTICAST_TTL:
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ return pico_udp_set_mc_ttl(s, *((uint8_t *) value));
+ }
+ break;
+
+ case PICO_IP_MULTICAST_LOOP:
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ switch (*(uint8_t *) value)
+ {
+ case 0:
+ /* do not loop back multicast datagram */
+ PICO_SOCKET_SETOPT_DIS(s,PICO_SOCKET_OPT_MULTICAST_LOOP);
+ break;
+
+ case 1:
+ /* do loop back multicast datagram */
+ PICO_SOCKET_SETOPT_EN(s,PICO_SOCKET_OPT_MULTICAST_LOOP);
+ break;
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ }
+ break;
+
+ case PICO_IP_ADD_MEMBERSHIP:
+ /* EXCLUDE mode */
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ int filter_mode;
+ struct pico_ip_mreq *mreq = NULL;
+ struct pico_mcast_listen *listen = NULL, ltest = {0};
+ struct pico_ipv4_link *mcast_link = NULL;
+
+ if (!value) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ mreq = (struct pico_ip_mreq *) value;
+ mcast_link = pico_socket_setoption_mcastargs_validation(mreq, NULL);
+ if (!mcast_link) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (!mreq->mcast_link_addr.addr)
+ mreq->mcast_link_addr.addr = mcast_link->address.addr;
+
+ if (!s->MCASTListen) { /* No RBTree allocated yet */
+ s->MCASTListen = pico_zalloc(sizeof(struct pico_tree));
+ if (!s->MCASTListen) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ s->MCASTListen->root = &LEAF;
+ s->MCASTListen->compare = mcast_listen_cmp;
+ }
+ ltest.mcast_link = mreq->mcast_link_addr;
+ ltest.mcast_group = mreq->mcast_group_addr;
+ listen = pico_tree_findKey(s->MCASTListen, <est);
+ if (listen) {
+ if (listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) {
+ so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ so_mcast_dbg("pico_socket_setoption: ERROR duplicate PICO_IP_ADD_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ } else {
+ listen = pico_zalloc(sizeof(struct pico_mcast_listen));
+ if (!listen) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ listen->filter_mode = PICO_IP_MULTICAST_EXCLUDE;
+ listen->mcast_link = mreq->mcast_link_addr;
+ listen->mcast_group = mreq->mcast_group_addr;
+ listen->MCASTSources.root = &LEAF;
+ listen->MCASTSources.compare = mcast_sources_cmp;
+ pico_tree_insert(s->MCASTListen, listen);
+ }
+
+ pico_tree_insert(&MCASTSockets, s);
+ filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr);
+ if (filter_mode < 0)
+ return -1;
+
+ return pico_ipv4_mcast_join(&mreq->mcast_link_addr, &mreq->mcast_group_addr, 1, filter_mode, &MCASTFilter);
+ }
+ break;
+
+ case PICO_IP_DROP_MEMBERSHIP:
+ /* EXCLUDE mode */
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ int filter_mode = 0;
+ struct pico_ip_mreq *mreq = NULL;
+ struct pico_mcast_listen *listen = NULL, ltest = {0};
+ struct pico_ip4 *source = NULL;
+ struct pico_ipv4_link *mcast_link = NULL;
+ struct pico_tree_node *index, *_tmp;
+
+ if (!value) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ mreq = (struct pico_ip_mreq *) value;
+ mcast_link = pico_socket_setoption_mcastargs_validation(mreq, NULL);
+ if (!mcast_link) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (!mreq->mcast_link_addr.addr)
+ mreq->mcast_link_addr.addr = mcast_link->address.addr;
+
+ if (!s->MCASTListen) { /* No RBTree allocated yet */
+ so_mcast_dbg("socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before any PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ ltest.mcast_link = mreq->mcast_link_addr;
+ ltest.mcast_group = mreq->mcast_group_addr;
+ listen = pico_tree_findKey(s->MCASTListen, <est);
+ if (!listen) {
+ so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ } else {
+ pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp)
+ {
+ source = index->keyValue;
+ pico_tree_delete(&listen->MCASTSources, source);
+ pico_free(source);
+ }
+ pico_tree_delete(s->MCASTListen, listen);
+ pico_free(listen);
+ if (pico_tree_empty(s->MCASTListen)) {
+ pico_free(s->MCASTListen);
+ s->MCASTListen = NULL;
+ pico_tree_delete(&MCASTSockets, s);
+ }
+ }
+
+ filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr);
+ if (filter_mode < 0)
+ return -1;
+
+ return pico_ipv4_mcast_leave(&mreq->mcast_link_addr, &mreq->mcast_group_addr, 1, filter_mode, &MCASTFilter);
+ }
+ break;
+
+ case PICO_IP_UNBLOCK_SOURCE:
+ /* EXCLUDE mode */
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ int filter_mode = 0;
+ struct pico_ip_mreq_source *mreq = NULL;
+ struct pico_mcast_listen *listen = NULL, ltest = {0};
+ struct pico_ip4 *source = NULL, stest = {0};
+ struct pico_ipv4_link *mcast_link = NULL;
+
+ if (!value) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ mreq = (struct pico_ip_mreq_source *) value;
+ mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq);
+ if (!mcast_link) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (!mreq->mcast_link_addr.addr)
+ mreq->mcast_link_addr.addr = mcast_link->address.addr;
+
+ if (!s->MCASTListen) { /* No RBTree allocated yet */
+ so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before any PICO_IP_ADD_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ ltest.mcast_link = mreq->mcast_link_addr;
+ ltest.mcast_group = mreq->mcast_group_addr;
+ listen = pico_tree_findKey(s->MCASTListen, <est);
+ if (!listen) {
+ so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ if (listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) {
+ so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ stest.addr = mreq->mcast_source_addr.addr;
+ source = pico_tree_findKey(&listen->MCASTSources, &stest);
+ if (!source) {
+ so_mcast_dbg("pico_socket_setoption: ERROR address to unblock not in source list\n");
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ } else {
+ pico_tree_delete(&listen->MCASTSources, source);
+ pico_free(source);
+ }
+ }
+
+ filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr);
+ if (filter_mode < 0)
+ return -1;
+
+ return pico_ipv4_mcast_leave(&mreq->mcast_link_addr, &mreq->mcast_group_addr, 0, filter_mode, &MCASTFilter);
+ }
+ break;
+
+ case PICO_IP_BLOCK_SOURCE:
+ /* EXCLUDE mode */
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ int filter_mode = 0;
+ struct pico_ip_mreq_source *mreq = NULL;
+ struct pico_mcast_listen *listen = NULL, ltest = {0};
+ struct pico_ip4 *source = NULL, stest = {0};
+ struct pico_ipv4_link *mcast_link = NULL;
+
+ if (!value) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ mreq = (struct pico_ip_mreq_source *) value;
+ mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq);
+ if (!mcast_link) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (!mreq->mcast_link_addr.addr)
+ mreq->mcast_link_addr.addr = mcast_link->address.addr;
+
+ if (!s->MCASTListen) { /* No RBTree allocated yet */
+ so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before any PICO_IP_ADD_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ ltest.mcast_link = mreq->mcast_link_addr;
+ ltest.mcast_group = mreq->mcast_group_addr;
+ listen = pico_tree_findKey(s->MCASTListen, <est);
+ if (!listen) {
+ so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ if (listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) {
+ so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ stest.addr = mreq->mcast_source_addr.addr;
+ source = pico_tree_findKey(&listen->MCASTSources, &stest);
+ if (source) {
+ so_mcast_dbg("pico_socket_setoption: ERROR address to block already in source list\n");
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ } else {
+ source = pico_zalloc(sizeof(struct pico_ip4));
+ if (!source) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ source->addr = mreq->mcast_source_addr.addr;
+ pico_tree_insert(&listen->MCASTSources, source);
+ }
+ }
+
+ filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr);
+ if (filter_mode < 0)
+ return -1;
+
+ return pico_ipv4_mcast_join(&mreq->mcast_link_addr, &mreq->mcast_group_addr, 0, filter_mode, &MCASTFilter);
+ }
+ break;
+
+ case PICO_IP_ADD_SOURCE_MEMBERSHIP:
+ /* INCLUDE mode */
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ int filter_mode = 0, reference_count = 0;
+ struct pico_ip_mreq_source *mreq = NULL;
+ struct pico_mcast_listen *listen = NULL, ltest = {0};
+ struct pico_ip4 *source = NULL, stest = {0};
+ struct pico_ipv4_link *mcast_link = NULL;
+
+ if (!value) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ mreq = (struct pico_ip_mreq_source *) value;
+ mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq);
+ if (!mcast_link) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (!mreq->mcast_link_addr.addr)
+ mreq->mcast_link_addr.addr = mcast_link->address.addr;
+
+ if (!s->MCASTListen) { /* No RBTree allocated yet */
+ s->MCASTListen = pico_zalloc(sizeof(struct pico_tree));
+ if (!s->MCASTListen) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ s->MCASTListen->root = &LEAF;
+ s->MCASTListen->compare = mcast_listen_cmp;
+ }
+ ltest.mcast_link = mreq->mcast_link_addr;
+ ltest.mcast_group = mreq->mcast_group_addr;
+ listen = pico_tree_findKey(s->MCASTListen, <est);
+ if (listen) {
+ if (listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) {
+ so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ stest.addr = mreq->mcast_source_addr.addr;
+ source = pico_tree_findKey(&listen->MCASTSources, &stest);
+ if (source) {
+ so_mcast_dbg("pico_socket_setoption: ERROR source address to allow already in source list\n");
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ } else {
+ source = pico_zalloc(sizeof(struct pico_ip4));
+ if (!source) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ source->addr = mreq->mcast_source_addr.addr;
+ pico_tree_insert(&listen->MCASTSources, source);
+ }
+ } else {
+ listen = pico_zalloc(sizeof(struct pico_mcast_listen));
+ if (!listen) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ listen->filter_mode = PICO_IP_MULTICAST_INCLUDE;
+ listen->mcast_link = mreq->mcast_link_addr;
+ listen->mcast_group = mreq->mcast_group_addr;
+ listen->MCASTSources.root = &LEAF;
+ listen->MCASTSources.compare = mcast_sources_cmp;
+ source = pico_zalloc(sizeof(struct pico_ip4));
+ if (!source) {
+ pico_free(listen);
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ source->addr = mreq->mcast_source_addr.addr;
+ pico_tree_insert(&listen->MCASTSources, source);
+ pico_tree_insert(s->MCASTListen, listen);
+ reference_count = 1;
+ }
+
+ pico_tree_insert(&MCASTSockets, s);
+ filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr);
+ if (filter_mode < 0)
+ return -1;
+
+ return pico_ipv4_mcast_join(&mreq->mcast_link_addr, &mreq->mcast_group_addr, reference_count, filter_mode, &MCASTFilter);
+ }
+ break;
+
+ case PICO_IP_DROP_SOURCE_MEMBERSHIP:
+ /* INCLUDE mode */
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ int filter_mode = 0, reference_count = 0;
+ struct pico_ip_mreq_source *mreq = NULL;
+ struct pico_mcast_listen *listen = NULL, ltest = {0};
+ struct pico_ip4 *source = NULL, stest = {0};
+ struct pico_ipv4_link *mcast_link = NULL;
+
+ if (!value) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ mreq = (struct pico_ip_mreq_source *) value;
+ mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq);
+ if (!mcast_link) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ if (!mreq->mcast_link_addr.addr)
+ mreq->mcast_link_addr.addr = mcast_link->address.addr;
+
+ if (!s->MCASTListen) { /* No RBTree allocated yet */
+ so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before any PICO_IP_ADD_SOURCE_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ ltest.mcast_link = mreq->mcast_link_addr;
+ ltest.mcast_group = mreq->mcast_group_addr;
+ listen = pico_tree_findKey(s->MCASTListen, <est);
+ if (!listen) {
+ so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before PICO_IP_ADD_SOURCE_MEMBERSHIP\n");
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ } else {
+ if (listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) {
+ so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ stest.addr = mreq->mcast_source_addr.addr;
+ source = pico_tree_findKey(&listen->MCASTSources, &stest);
+ if (!source) {
+ so_mcast_dbg("pico_socket_setoption: ERROR address to drop not in source list\n");
+ pico_err = PICO_ERR_EADDRNOTAVAIL;
+ return -1;
+ } else {
+ pico_tree_delete(&listen->MCASTSources, source);
+ pico_free(source);
+ if (pico_tree_empty(&listen->MCASTSources)) { /* 1 if empty, 0 otherwise */
+ reference_count = 1;
+ pico_tree_delete(s->MCASTListen, listen);
+ pico_free(listen);
+ if (pico_tree_empty(s->MCASTListen)) {
+ pico_free(s->MCASTListen);
+ s->MCASTListen = NULL;
+ pico_tree_delete(&MCASTSockets, s);
+ }
+ }
+ }
+ }
+
+ filter_mode = pico_socket_aggregate_mcastfilters(&mcast_link->address, &mreq->mcast_group_addr);
+ if (filter_mode < 0)
+ return -1;
+
+ return pico_ipv4_mcast_leave(&mreq->mcast_link_addr, &mreq->mcast_group_addr, reference_count, filter_mode, &MCASTFilter);
+ }
+ break;
+#endif /* PICO_SUPPORT_MCAST */
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if (pico_err != PICO_ERR_NOERR)
+ return -1;
+ else
+ return 0;
+}
+
+#define PICO_SOCKET_GETOPT(socket,index) ((socket->opt_flags & (1 << index)) != 0)
+
+int pico_socket_getoption(struct pico_socket *s, int option, void *value)
+{
+ if (!s || !value) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ switch (option)
+ {
+#ifdef PICO_SUPPORT_TCP
+ case PICO_TCP_NODELAY:
+ if (s->proto->proto_number == PICO_PROTO_TCP)
+ /* state of the NODELAY option */
+ *(int *)value = PICO_SOCKET_GETOPT(s,PICO_SOCKET_OPT_TCPNODELAY);
+ else
+ *(int *)value = 0;
+ break;
+#endif
+
+#ifdef PICO_SUPPORT_MCAST
+ case PICO_IP_MULTICAST_IF:
+ pico_err = PICO_ERR_EOPNOTSUPP;
+ return -1;
+ break;
+
+ case PICO_IP_MULTICAST_TTL:
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ pico_udp_get_mc_ttl(s, (uint8_t *) value);
+ } else {
+ *(uint8_t *)value = 0;
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ break;
+
+ case PICO_IP_MULTICAST_LOOP:
+ if (s->proto->proto_number == PICO_PROTO_UDP) {
+ *(uint8_t *)value = PICO_SOCKET_GETOPT(s,PICO_SOCKET_OPT_MULTICAST_LOOP);
+ } else {
+ *(uint8_t *)value = 0;
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ break;
+#endif /* PICO_SUPPORT_MCAST */
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ return 0;
+}
+
+
+int pico_socket_shutdown(struct pico_socket *s, int mode)
+{
+ if (!s) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ } else {
+ /* check if exists in tree */
+ /* See task #178 */
+ if (pico_check_socket(s) != 0) {
+ pico_free(s); /* close socket after bind or connect failed */
+ return 0;
+ }
+ }
+
+#ifdef PICO_SUPPORT_UDP
+ if (PROTO(s) == PICO_PROTO_UDP) {
+ if (mode & PICO_SHUT_RDWR)
+ pico_socket_alter_state(s, PICO_SOCKET_STATE_CLOSED, PICO_SOCKET_STATE_CLOSING |PICO_SOCKET_STATE_BOUND | PICO_SOCKET_STATE_CONNECTED, 0);
+ else if (mode & PICO_SHUT_RD)
+ pico_socket_alter_state(s, 0, PICO_SOCKET_STATE_BOUND, 0);
+ }
+#endif
+#ifdef PICO_SUPPORT_TCP
+ if (PROTO(s) == PICO_PROTO_TCP) {
+ if(mode & PICO_SHUT_RDWR)
+ pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_LOCAL | PICO_SOCKET_STATE_SHUT_REMOTE, 0, 0);
+ else if (mode & PICO_SHUT_WR)
+ pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_LOCAL, 0, 0);
+ else if (mode & PICO_SHUT_RD)
+ pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_REMOTE, 0, 0);
+
+ }
+#endif
+ return 0;
+}
+
+int pico_socket_close(struct pico_socket *s)
+{
+ return pico_socket_shutdown(s, PICO_SHUT_RDWR);
+}
+
+#ifdef PICO_SUPPORT_CRC
+static inline int pico_transport_crc_check(struct pico_frame *f)
+{
+ struct pico_ipv4_hdr *net_hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ struct pico_udp_hdr *udp_hdr = NULL;
+ uint16_t checksum_invalid = 1;
+
+ switch (net_hdr->proto)
+ {
+ case PICO_PROTO_TCP:
+ checksum_invalid = short_be(pico_tcp_checksum_ipv4(f));
+ //dbg("TCP CRC validation == %u\n", checksum_invalid);
+ if (checksum_invalid) {
+ //dbg("TCP CRC: validation failed!\n");
+ pico_frame_discard(f);
+ return 0;
+ }
+ break;
+
+ case PICO_PROTO_UDP:
+ udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ if (short_be(udp_hdr->crc)) {
+ checksum_invalid = short_be(pico_udp_checksum_ipv4(f));
+ //dbg("UDP CRC validation == %u\n", checksum_invalid);
+ if (checksum_invalid) {
+ //dbg("UDP CRC: validation failed!\n");
+ pico_frame_discard(f);
+ return 0;
+ }
+ }
+ break;
+
+ default:
+ // Do nothing
+ break;
+ }
+ return 1;
+}
+#else
+static inline int pico_transport_crc_check(struct pico_frame *f)
+{
+ IGNORE_PARAMETER(f);
+ return 1;
+}
+#endif /* PICO_SUPPORT_CRC */
+
+int pico_transport_process_in(struct pico_protocol *self, struct pico_frame *f)
+{
+ struct pico_trans *hdr = (struct pico_trans *) f->transport_hdr;
+ int ret = 0;
+
+ if (!hdr) {
+ pico_err = PICO_ERR_EFAULT;
+ return -1;
+ }
+
+ ret = pico_transport_crc_check(f);
+ if (ret < 1)
+ return ret;
+ else
+ ret = 0;
+
+ if ((hdr) && (pico_socket_deliver(self, f, hdr->dport) == 0))
+ return ret;
+
+ if (!IS_BCAST(f)) {
+ dbg("Socket not found... \n");
+ pico_notify_socket_unreachable(f);
+#ifdef PICO_SUPPORT_TCP
+ /* if tcp protocol send RST segment */
+ //if (self->proto_number == PICO_PROTO_TCP)
+ // pico_tcp_reply_rst(f);
+#endif
+ ret = -1;
+ pico_err = PICO_ERR_ENOENT;
+ }
+ pico_frame_discard(f);
+ return ret;
+}
+
+#define SL_LOOP_MIN 1
+
+
+int pico_sockets_loop(int loop_score)
+{
+ static struct pico_tree_node *index_udp, * index_tcp;
+
+ struct pico_sockport *start;
+ struct pico_socket *s;
+
+#ifdef PICO_SUPPORT_UDP
+ struct pico_frame *f;
+
+ if (sp_udp == NULL)
+ {
+ index_udp = pico_tree_firstNode(UDPTable.root);
+ sp_udp = index_udp->keyValue;
+ }
+
+ /* init start node */
+ start = sp_udp;
+
+ /* round-robin all transport protocols, break if traversed all protocols */
+ while (loop_score > SL_LOOP_MIN && sp_udp != NULL) {
+ struct pico_tree_node * index;
+
+ pico_tree_foreach(index,&sp_udp->socks){
+ s = index->keyValue;
+ f = pico_dequeue(&s->q_out);
+ while (f && (loop_score > 0)) {
+ pico_proto_udp.push(&pico_proto_udp, f);
+ loop_score -= 1;
+ f = pico_dequeue(&s->q_out);
+ }
+ }
+
+ index_udp = pico_tree_next(index_udp);
+ sp_udp = index_udp->keyValue;
+
+ if (sp_udp == NULL)
+ {
+ index_udp = pico_tree_firstNode(UDPTable.root);
+ sp_udp = index_udp->keyValue;
+ }
+ if (sp_udp == start)
+ break;
+ }
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+ if (sp_tcp == NULL)
+ {
+ index_tcp = pico_tree_firstNode(TCPTable.root);
+ sp_tcp = index_tcp->keyValue;
+ }
+
+ /* init start node */
+ start = sp_tcp;
+
+ while (loop_score > SL_LOOP_MIN && sp_tcp != NULL) {
+ struct pico_tree_node * index = NULL, * safe_index = NULL;
+ pico_tree_foreach_safe(index, &sp_tcp->socks,safe_index){
+ s = index->keyValue;
+ loop_score = pico_tcp_output(s, loop_score);
+ if ((s->ev_pending) && s->wakeup) {
+ s->wakeup(s->ev_pending, s);
+ }
+ if (loop_score <= 0) {
+ loop_score = 0;
+ break;
+ }
+
+ // checking socket activity
+ if((uint32_t)(PICO_TIME_MS() - s->timestamp) >= PICO_SOCKET_TIMEOUT) {
+ // checking for hanging sockets
+ if( (TCP_STATE(s) != PICO_SOCKET_STATE_TCP_LISTEN) && (TCP_STATE(s) != PICO_SOCKET_STATE_TCP_ESTABLISHED ) )
+ {
+ pico_socket_del(s);
+ index_tcp = pico_tree_firstNode(TCPTable.root);
+ }
+ // if no activity, force the socket into closing state
+ if( TCP_STATE(s) == PICO_SOCKET_STATE_TCP_ESTABLISHED )
+ {
+ pico_socket_close(s);
+ s->timestamp = PICO_TIME_MS();
+ }
+ }
+ }
+
+ /* check if RB_FOREACH ended, if not, break to keep the cur sp_tcp */
+ if ( (index && index->keyValue) || !index_tcp)
+ break;
+
+ index_tcp = pico_tree_next(index_tcp);
+ sp_tcp = index_tcp->keyValue;
+
+ if (sp_tcp == NULL)
+ {
+ index_tcp = pico_tree_firstNode(TCPTable.root);
+ sp_tcp = index_tcp->keyValue;
+ }
+ if (sp_tcp == start)
+ break;
+ }
+#endif
+
+ return loop_score;
+}
+
+
+struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, int len)
+{
+ struct pico_frame *f = NULL;
+
+#ifdef PICO_SUPPORT_IPV6
+ if (IS_SOCK_IPV6(s))
+ f = pico_proto_ipv6.alloc(&pico_proto_ipv6, len);
+#endif
+
+#ifdef PICO_SUPPORT_IPV4
+ if (IS_SOCK_IPV4(s))
+ f = pico_proto_ipv4.alloc(&pico_proto_ipv4, len);
+#endif
+ if (!f) {
+ pico_err = PICO_ERR_ENOMEM;
+ return f;
+ }
+ f->payload = f->transport_hdr;
+ f->payload_len = len;
+ f->sock = s;
+ return f;
+}
+
+int pico_transport_error(struct pico_frame *f, uint8_t proto, int code)
+{
+ int ret = -1;
+ struct pico_trans *trans = (struct pico_trans*) f->transport_hdr;
+ struct pico_sockport *port = NULL;
+ struct pico_socket *s = NULL;
+ switch (proto) {
+
+
+#ifdef PICO_SUPPORT_UDP
+ case PICO_PROTO_UDP:
+ port = pico_get_sockport(proto, trans->sport);
+ break;
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+ case PICO_PROTO_TCP:
+ port = pico_get_sockport(proto, trans->sport);
+ break;
+#endif
+
+ default:
+ /* Protocol not available */
+ ret = -1;
+ }
+ if (port) {
+ struct pico_tree_node * index;
+ ret = 0;
+
+ pico_tree_foreach(index,&port->socks) {
+ s = index->keyValue;
+ if (trans->dport == s->remote_port) {
+ if (s->wakeup) {
+ //dbg("SOCKET ERROR FROM ICMP NOTIFICATION. (icmp code= %d)\n\n", code);
+ switch(code) {
+ case PICO_ICMP_UNREACH_PROTOCOL:
+ pico_err = PICO_ERR_EPROTO;
+ break;
+
+ case PICO_ICMP_UNREACH_PORT:
+ pico_err = PICO_ERR_ECONNREFUSED;
+ break;
+
+ case PICO_ICMP_UNREACH_NET:
+ case PICO_ICMP_UNREACH_NET_PROHIB:
+ case PICO_ICMP_UNREACH_NET_UNKNOWN:
+ pico_err = PICO_ERR_ENETUNREACH;
+ break;
+
+ default:
+ pico_err = PICO_ERR_EHOSTUNREACH;
+ }
+ s->wakeup(PICO_SOCK_EV_ERR, s);
+ }
+ break;
+ }
+ }
+ }
+ pico_frame_discard(f);
+ return ret;
+}
+#endif
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_stack.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,734 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+
+#include "pico_config.h"
+#include "pico_frame.h"
+#include "pico_device.h"
+#include "pico_protocol.h"
+#include "pico_stack.h"
+#include "pico_addressing.h"
+#include "pico_dns_client.h"
+
+#include "pico_eth.h"
+#include "pico_arp.h"
+#include "pico_ipv4.h"
+#include "pico_ipv6.h"
+#include "pico_icmp4.h"
+#include "pico_igmp.h"
+#include "pico_udp.h"
+#include "pico_tcp.h"
+#include "pico_socket.h"
+#include "heap.h"
+
+#define IS_LIMITED_BCAST(f) ( ((struct pico_ipv4_hdr *) f->net_hdr)->dst.addr == PICO_IP4_BCAST )
+
+#ifdef PICO_SUPPORT_MCAST
+# define PICO_SIZE_MCAST 3
+ const uint8_t PICO_ETHADDR_MCAST[6] = {0x01, 0x00, 0x5e, 0x00, 0x00, 0x00};
+#endif
+
+volatile unsigned long long pico_tick;
+volatile pico_err_t pico_err;
+
+static uint32_t _rand_seed;
+
+void pico_rand_feed(uint32_t feed)
+{
+ if (!feed)
+ return;
+ _rand_seed *= 1664525;
+ _rand_seed += 1013904223;
+ _rand_seed ^= ~(feed);
+}
+
+uint32_t pico_rand(void)
+{
+ pico_rand_feed(pico_tick);
+ return _rand_seed;
+}
+
+/* NOTIFICATIONS: distributed notifications for stack internal errors.
+ */
+
+int pico_notify_socket_unreachable(struct pico_frame *f)
+{
+ if (0) {}
+#ifdef PICO_SUPPORT_ICMP4
+ else if (IS_IPV4(f)) {
+ pico_icmp4_port_unreachable(f);
+ }
+#endif
+#ifdef PICO_SUPPORT_ICMP6
+ else if (IS_IPV6(f)) {
+ pico_icmp6_port_unreachable(f);
+ }
+#endif
+
+ return 0;
+}
+
+int pico_notify_proto_unreachable(struct pico_frame *f)
+{
+ if (0) {}
+#ifdef PICO_SUPPORT_ICMP4
+ else if (IS_IPV4(f)) {
+ pico_icmp4_proto_unreachable(f);
+ }
+#endif
+#ifdef PICO_SUPPORT_ICMP6
+ else if (IS_IPV6(f)) {
+ pico_icmp6_proto_unreachable(f);
+ }
+#endif
+ return 0;
+}
+
+int pico_notify_dest_unreachable(struct pico_frame *f)
+{
+ if (0) {}
+#ifdef PICO_SUPPORT_ICMP4
+ else if (IS_IPV4(f)) {
+ pico_icmp4_dest_unreachable(f);
+ }
+#endif
+#ifdef PICO_SUPPORT_ICMP6
+ else if (IS_IPV6(f)) {
+ pico_icmp6_dest_unreachable(f);
+ }
+#endif
+ return 0;
+}
+
+int pico_notify_ttl_expired(struct pico_frame *f)
+{
+ if (0) {}
+#ifdef PICO_SUPPORT_ICMP4
+ else if (IS_IPV4(f)) {
+ pico_icmp4_ttl_expired(f);
+ }
+#endif
+#ifdef PICO_SUPPORT_ICMP6
+ else if (IS_IPV6(f)) {
+ pico_icmp6_ttl_expired(f);
+ }
+#endif
+ return 0;
+}
+
+
+/* Transport layer */
+int pico_transport_receive(struct pico_frame *f, uint8_t proto)
+{
+ int ret = -1;
+ switch (proto) {
+
+#ifdef PICO_SUPPORT_ICMP4
+ case PICO_PROTO_ICMP4:
+ ret = pico_enqueue(pico_proto_icmp4.q_in, f);
+ break;
+#endif
+
+#ifdef PICO_SUPPORT_IGMP
+ case PICO_PROTO_IGMP:
+ ret = pico_enqueue(pico_proto_igmp.q_in, f);
+ break;
+#endif
+
+#ifdef PICO_SUPPORT_UDP
+ case PICO_PROTO_UDP:
+ ret = pico_enqueue(pico_proto_udp.q_in, f);
+ break;
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+ case PICO_PROTO_TCP:
+ ret = pico_enqueue(pico_proto_tcp.q_in, f);
+ break;
+#endif
+
+ default:
+ /* Protocol not available */
+ dbg("pkt: no such protocol (%d)\n", proto);
+ pico_notify_proto_unreachable(f);
+ pico_frame_discard(f);
+ ret = -1;
+ }
+ return ret;
+}
+
+int pico_transport_send(struct pico_frame *f)
+{
+ if (!f || !f->sock || !f->sock->proto) {
+ pico_frame_discard(f);
+ return -1;
+ }
+ return f->sock->proto->push(f->sock->net, f);
+}
+
+int pico_network_receive(struct pico_frame *f)
+{
+ if (0) {}
+#ifdef PICO_SUPPORT_IPV4
+ else if (IS_IPV4(f)) {
+ pico_enqueue(pico_proto_ipv4.q_in, f);
+ }
+#endif
+#ifdef PICO_SUPPORT_IPV6
+ else if (IS_IPV6(f)) {
+ pico_enqueue(pico_proto_ipv6.q_in, f);
+ }
+#endif
+ else {
+ dbg("Network not found.\n");
+ pico_frame_discard(f);
+ return -1;
+ }
+ return f->buffer_len;
+}
+
+
+/* Network layer: interface towards socket for frame sending */
+int pico_network_send(struct pico_frame *f)
+{
+ if (!f || !f->sock || !f->sock->net) {
+ pico_frame_discard(f);
+ return -1;
+ }
+ return f->sock->net->push(f->sock->net, f);
+}
+
+int pico_destination_is_local(struct pico_frame *f)
+{
+ if (0) { }
+#ifdef PICO_SUPPORT_IPV4
+ else if (IS_IPV4(f)) {
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ if (pico_ipv4_link_find(&hdr->dst))
+ return 1;
+ }
+#endif
+#ifdef PICO_SUPPORT_IPV6
+ else if (IS_IPV6(f)) {
+ }
+#endif
+ return 0;
+}
+
+int pico_source_is_local(struct pico_frame *f)
+{
+ if (0) { }
+#ifdef PICO_SUPPORT_IPV4
+ else if (IS_IPV4(f)) {
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ if (hdr->src.addr == PICO_IPV4_INADDR_ANY)
+ return 1;
+ if (pico_ipv4_link_find(&hdr->src))
+ return 1;
+ }
+#endif
+#ifdef PICO_SUPPORT_IPV6
+ else if (IS_IPV6(f)) {
+ /* XXX */
+ }
+#endif
+ return 0;
+
+
+}
+
+
+/* DATALINK LEVEL: interface from network to the device
+ * and vice versa.
+ */
+
+/* The pico_ethernet_receive() function is used by
+ * those devices supporting ETH in order to push packets up
+ * into the stack.
+ */
+int pico_ethernet_receive(struct pico_frame *f)
+{
+ struct pico_eth_hdr *hdr;
+ if (!f || !f->dev || !f->datalink_hdr)
+ goto discard;
+ hdr = (struct pico_eth_hdr *) f->datalink_hdr;
+ if ( (memcmp(hdr->daddr, f->dev->eth->mac.addr, PICO_SIZE_ETH) != 0) &&
+#ifdef PICO_SUPPORT_MCAST
+ (memcmp(hdr->daddr, PICO_ETHADDR_MCAST, PICO_SIZE_MCAST) != 0) &&
+#endif
+ (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) != 0) )
+ goto discard;
+
+ f->net_hdr = f->datalink_hdr + sizeof(struct pico_eth_hdr);
+ if (hdr->proto == PICO_IDETH_ARP)
+ return pico_arp_receive(f);
+ if ((hdr->proto == PICO_IDETH_IPV4) || (hdr->proto == PICO_IDETH_IPV6))
+ return pico_network_receive(f);
+discard:
+ pico_frame_discard(f);
+ return -1;
+}
+
+static int destination_is_bcast(struct pico_frame *f)
+{
+ if (!f)
+ return 0;
+
+ if (IS_IPV6(f))
+ return 0;
+#ifdef PICO_SUPPORT_IPV4
+ else {
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ return pico_ipv4_is_broadcast(hdr->dst.addr);
+ }
+#endif
+ return 0;
+}
+
+#ifdef PICO_SUPPORT_MCAST
+static int destination_is_mcast(struct pico_frame *f)
+{
+ if (!f)
+ return 0;
+
+ if (IS_IPV6(f))
+ return 0;
+#ifdef PICO_SUPPORT_IPV4
+ else {
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ return pico_ipv4_is_multicast(hdr->dst.addr);
+ }
+#endif
+ return 0;
+}
+
+static struct pico_eth *pico_ethernet_mcast_translate(struct pico_frame *f, uint8_t *pico_mcast_mac)
+{
+ struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+
+ /* place 23 lower bits of IP in lower 23 bits of MAC */
+ pico_mcast_mac[5] = (long_be(hdr->dst.addr) & 0x000000FF);
+ pico_mcast_mac[4] = (long_be(hdr->dst.addr) & 0x0000FF00) >> 8;
+ pico_mcast_mac[3] = (long_be(hdr->dst.addr) & 0x007F0000) >> 16;
+
+ return (struct pico_eth *)pico_mcast_mac;
+}
+
+
+#endif /* PICO_SUPPORT_MCAST */
+
+/* This is called by dev loop in order to ensure correct ethernet addressing.
+ * Returns 0 if the destination is unknown, and -1 if the packet is not deliverable
+ * due to ethernet addressing (i.e., no arp association was possible.
+ *
+ * Only IP packets must pass by this. ARP will always use direct dev->send() function, so
+ * we assume IP is used.
+ */
+int pico_ethernet_send(struct pico_frame *f)
+{
+ const struct pico_eth *dstmac = NULL;
+ int ret = -1;
+
+ if (IS_IPV6(f)) {
+ /*TODO: Neighbor solicitation */
+ dstmac = NULL;
+ }
+
+ else if (IS_IPV4(f)) {
+ if (IS_BCAST(f) || destination_is_bcast(f)) {
+ dstmac = (const struct pico_eth *const) PICO_ETHADDR_ALL;
+ }
+#ifdef PICO_SUPPORT_MCAST
+ else if (destination_is_mcast(f)) {
+ uint8_t pico_mcast_mac[6] = {0x01, 0x00, 0x5e, 0x00, 0x00, 0x00};
+ dstmac = pico_ethernet_mcast_translate(f, pico_mcast_mac);
+ }
+#endif
+ else {
+ dstmac = pico_arp_get(f);
+ if (!dstmac)
+ return 0;
+ }
+ /* This sets destination and source address, then pushes the packet to the device. */
+ if (dstmac && (f->start > f->buffer) && ((f->start - f->buffer) >= PICO_SIZE_ETHHDR)) {
+ struct pico_eth_hdr *hdr;
+ f->start -= PICO_SIZE_ETHHDR;
+ f->len += PICO_SIZE_ETHHDR;
+ f->datalink_hdr = f->start;
+ hdr = (struct pico_eth_hdr *) f->datalink_hdr;
+ memcpy(hdr->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH);
+ memcpy(hdr->daddr, dstmac, PICO_SIZE_ETH);
+ hdr->proto = PICO_IDETH_IPV4;
+ if(!memcmp(hdr->daddr, hdr->saddr, PICO_SIZE_ETH)){
+ dbg("sending out packet destined for our own mac\n");
+ return pico_ethernet_receive(f);
+ }else if(IS_LIMITED_BCAST(f)){
+ ret = pico_device_broadcast(f);
+ }else {
+ ret = f->dev->send(f->dev, f->start, f->len);
+ /* Frame is discarded after this return by the caller */
+ }
+
+ if(!ret) pico_frame_discard(f);
+ return ret;
+ } else {
+ return -1;
+ }
+ } /* End IPV4 ethernet addressing */
+ return -1;
+
+}
+
+void pico_store_network_origin(void *src, struct pico_frame *f)
+{
+ #ifdef PICO_SUPPORT_IPV4
+ struct pico_ip4 *ip4;
+ #endif
+
+ #ifdef PICO_SUPPORT_IPV6
+ struct pico_ip6 *ip6;
+ #endif
+
+ #ifdef PICO_SUPPORT_IPV4
+ if (IS_IPV4(f)) {
+ struct pico_ipv4_hdr *hdr;
+ hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ ip4 = (struct pico_ip4 *) src;
+ ip4->addr = hdr->src.addr;
+ }
+ #endif
+ #ifdef PICO_SUPPORT_IPV6
+ if (IS_IPV6(f)) {
+ struct pico_ipv6_hdr *hdr;
+ hdr = (struct pico_ipv6_hdr *) f->net_hdr;
+ ip6 = (struct pico_ip6 *) src;
+ memcpy(ip6->addr, hdr->src.addr, PICO_SIZE_IP6);
+ }
+ #endif
+}
+
+
+/* LOWEST LEVEL: interface towards devices. */
+/* Device driver will call this function which returns immediately.
+ * Incoming packet will be processed later on in the dev loop.
+ */
+int pico_stack_recv(struct pico_device *dev, uint8_t *buffer, int len)
+{
+ struct pico_frame *f;
+ int ret;
+ if (len <= 0)
+ return -1;
+ f = pico_frame_alloc(len);
+ if (!f)
+ return -1;
+
+ /* Association to the device that just received the frame. */
+ f->dev = dev;
+
+ /* Setup the start pointer, lenght. */
+ f->start = f->buffer;
+ f->len = f->buffer_len;
+ if (f->len > 8) {
+ int mid_frame = (f->buffer_len >> 2)<<1;
+ mid_frame -= (mid_frame % 4);
+ pico_rand_feed(*(uint32_t*)(f->buffer + mid_frame));
+ }
+ memcpy(f->buffer, buffer, len);
+ ret = pico_enqueue(dev->q_in, f);
+ if (ret <= 0) {
+ pico_frame_discard(f);
+ }
+ return ret;
+}
+
+int pico_sendto_dev(struct pico_frame *f)
+{
+ if (!f->dev) {
+ pico_frame_discard(f);
+ return -1;
+ } else {
+ if (f->len > 8) {
+ int mid_frame = (f->buffer_len >> 2)<<1;
+ mid_frame -= (mid_frame % 4);
+ pico_rand_feed(*(uint32_t*)(f->buffer + mid_frame));
+ }
+ return pico_enqueue(f->dev->q_out, f);
+ }
+}
+
+struct pico_timer
+{
+ unsigned long expire;
+ void *arg;
+ void (*timer)(unsigned long timestamp, void *arg);
+};
+
+typedef struct pico_timer pico_timer;
+
+DECLARE_HEAP(pico_timer, expire);
+
+static heap_pico_timer *Timers;
+
+void pico_check_timers(void)
+{
+ struct pico_timer timer;
+ struct pico_timer *t = heap_first(Timers);
+ pico_tick = PICO_TIME_MS();
+ while((t) && (t->expire < pico_tick)) {
+ t->timer(pico_tick, t->arg);
+ heap_peek(Timers, &timer);
+ t = heap_first(Timers);
+ }
+}
+
+
+#define PROTO_DEF_NR 11
+#define PROTO_DEF_AVG_NR 4
+#define PROTO_DEF_SCORE 32
+#define PROTO_MIN_SCORE 32
+#define PROTO_MAX_SCORE 128
+#define PROTO_LAT_IND 3 /* latecy indication 0-3 (lower is better latency performance), x1, x2, x4, x8 */
+#define PROTO_MAX_LOOP (PROTO_MAX_SCORE<<PROTO_LAT_IND) /* max global loop score, so per tick */
+
+static int calc_score(int *score, int *index, int avg[][PROTO_DEF_AVG_NR], int *ret)
+{
+ int temp, i, j, sum;
+ int max_total = PROTO_MAX_LOOP, total = 0;
+
+ //dbg("USED SCORES> ");
+
+ for (i = 0; i < PROTO_DEF_NR; i++) {
+
+ /* if used looped score */
+ if (ret[i] < score[i]) {
+ temp = score[i] - ret[i]; /* remaining loop score */
+
+ //dbg("%3d - ",temp);
+
+ if (index[i] >= PROTO_DEF_AVG_NR)
+ index[i] = 0; /* reset index */
+ j = index[i];
+ avg[i][j] = temp;
+
+ index[i]++;
+
+ if (ret[i] == 0 && (score[i]<<1 <= PROTO_MAX_SCORE) && ((total+(score[i]<<1)) < max_total) ) { /* used all loop score -> increase next score directly */
+ score[i] <<= 1;
+ total += score[i];
+ continue;
+ }
+
+ sum = 0;
+ for (j = 0; j < PROTO_DEF_AVG_NR; j++)
+ sum += avg[i][j]; /* calculate sum */
+
+ sum >>= 2; /* divide by 4 to get average used score */
+
+ /* criterion to increase next loop score */
+ if (sum > (score[i] - (score[i]>>2)) && (score[i]<<1 <= PROTO_MAX_SCORE) && ((total+(score[i]<<1)) < max_total)) { /* > 3/4 */
+ score[i] <<= 1; /* double loop score */
+ total += score[i];
+ continue;
+ }
+
+ /* criterion to decrease next loop score */
+ if (sum < (score[i]>>2) && (score[i]>>1 >= PROTO_MIN_SCORE)) { /* < 1/4 */
+ score[i] >>= 1; /* half loop score */
+ total += score[i];
+ continue;
+ }
+
+ /* also add non-changed scores */
+ total += score[i];
+ }
+ else if (ret[i] == score[i]) {
+ /* no used loop score - gradually decrease */
+
+ // dbg("%3d - ",0);
+
+ if (index[i] >= PROTO_DEF_AVG_NR)
+ index[i] = 0; /* reset index */
+ j = index[i];
+ avg[i][j] = 0;
+
+ index[i]++;
+
+ sum = 0;
+ for (j = 0; j < PROTO_DEF_AVG_NR; j++)
+ sum += avg[i][j]; /* calculate sum */
+
+ sum >>= 2; /* divide by 4 to get average used score */
+
+ if ((sum == 0) && (score[i]>>1 >= PROTO_MIN_SCORE)) {
+ score[i] >>= 1; /* half loop score */
+ total += score[i];
+ for (j = 0; j < PROTO_DEF_AVG_NR; j++)
+ avg[i][j] = score[i];
+ }
+
+ }
+ }
+
+ //dbg("\n");
+
+ return 0;
+}
+
+
+
+/*
+
+ .
+ .vS.
+ <aoSo.
+ .XoS22.
+ .S2S22. ._... ...... ..._.
+ :=|2S2X2|=++; <vSX2XX2z+ |vSSSXSSs>. :iXXZUZXXe=
+ )2SS2SS2S2S2I =oS2S2S2S2X22;. _vuXS22S2S2S22i ._wZZXZZZXZZXZX=
+ )22S2S2S2S2Sl |S2S2S22S2SSSXc: .S2SS2S2S22S2SS= .]#XZZZXZXZZZZZZ:
+ )oSS2SS2S2Sol |2}!"""!32S22S(. uS2S2Se**12oS2e ]dXZZXX2?YYXXXZ*
+ .:2S2So:..-. . :]S2S2e;=X2SS2o .)oc ]XZZXZ( =nX:
+ .S2S22. ___s_i,.)oS2So(;2SS2So, ` 3XZZZZc, -
+ .S2SSo. =oXXXSSS2XoS2S2o( XS2S2XSos;. ]ZZZZXXXX|=
+ .S2S22. .)S2S2S22S2S2S2S2o( "X2SS2S2S2Sus,, +3XZZZZZXZZoos_
+ .S2S22. .]2S2SS22S222S2SS2o( ]S22S2S2S222So :3XXZZZZZZZZXXv
+ .S2S22. =u2SS2e"~---"{2S2So( -"12S2S2SSS2Su. "?SXXXZXZZZZXo
+ .S2SSo. )SS22z; :S2S2o( ={vS2S2S22v .<vXZZZZZZZ;
+ .S2S2S: ]oSS2c; =22S2o( -"S2SS2n ~4XXZXZ(
+ .2S2S2i )2S2S2[. .)XS2So( <;. .2S2S2o :<. ]XZZZX(
+ nX2S2S,,_s_=3oSS2SoaasuXXS2S2o( .oXoasi_aioSSS22l.]dZoaas_aadXZZXZ'
+ vS2SSSXXX2; )S2S2S2SoS2S2S2S2o( iS2S222XSoSS22So.)nXZZXXXZZXXZZXZo
+ +32S22S2Sn -+S2S2S2S2So22S2So( 12S2SS2S2SS22S}- )SXXZZZZZZZZZXX!-
+ .)S22222i .i2S2S2o>;:S2S2o( .<vSoSoSo2S(; :nXXXXXZXXX(
+ .-~~~~- --- . - - --~~~-- --^^~~-
+ .
+
+
+ ... curious about our source code? We are hiring! mailto:<recruiting@tass.be>
+
+
+*/
+
+void pico_stack_tick(void)
+{
+ static int score[PROTO_DEF_NR] = {PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE};
+ static int index[PROTO_DEF_NR] = {0,0,0,0,0,0};
+ static int avg[PROTO_DEF_NR][PROTO_DEF_AVG_NR];
+ static int ret[PROTO_DEF_NR] = {0};
+
+ pico_check_timers();
+
+ //dbg("LOOP_SCORES> %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d\n",score[0],score[1],score[2],score[3],score[4],score[5],score[6],score[7],score[8],score[9],score[10]);
+
+ //score = pico_protocols_loop(100);
+
+ ret[0] = pico_devices_loop(score[0],PICO_LOOP_DIR_IN);
+ pico_rand_feed(ret[0]);
+
+ ret[1] = pico_protocol_datalink_loop(score[1], PICO_LOOP_DIR_IN);
+ pico_rand_feed(ret[1]);
+
+ ret[2] = pico_protocol_network_loop(score[2], PICO_LOOP_DIR_IN);
+ pico_rand_feed(ret[2]);
+
+ ret[3] = pico_protocol_transport_loop(score[3], PICO_LOOP_DIR_IN);
+ pico_rand_feed(ret[3]);
+
+
+ ret[5] = score[5];
+#if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6)
+#if defined (PICO_SUPPORT_TCP) || defined (PICO_SUPPORT_UDP)
+ ret[5] = pico_sockets_loop(score[5]); // swapped
+ pico_rand_feed(ret[5]);
+#endif
+#endif
+
+ ret[4] = pico_protocol_socket_loop(score[4], PICO_LOOP_DIR_IN);
+ pico_rand_feed(ret[4]);
+
+
+ ret[6] = pico_protocol_socket_loop(score[6], PICO_LOOP_DIR_OUT);
+ pico_rand_feed(ret[6]);
+
+ ret[7] = pico_protocol_transport_loop(score[7], PICO_LOOP_DIR_OUT);
+ pico_rand_feed(ret[7]);
+
+ ret[8] = pico_protocol_network_loop(score[8], PICO_LOOP_DIR_OUT);
+ pico_rand_feed(ret[8]);
+
+ ret[9] = pico_protocol_datalink_loop(score[9], PICO_LOOP_DIR_OUT);
+ pico_rand_feed(ret[9]);
+
+ ret[10] = pico_devices_loop(score[10],PICO_LOOP_DIR_OUT);
+ pico_rand_feed(ret[10]);
+
+ /* calculate new loop scores for next iteration */
+ calc_score(score, index,(int (*)[]) avg, ret);
+}
+
+void pico_stack_loop(void)
+{
+ while(1) {
+ pico_stack_tick();
+ PICO_IDLE();
+ }
+}
+
+void pico_timer_add(unsigned long expire, void (*timer)(unsigned long, void *), void *arg)
+{
+ pico_timer t;
+ t.expire = PICO_TIME_MS() + expire;
+ t.arg = arg;
+ t.timer = timer;
+ heap_insert(Timers, &t);
+ if (Timers->n > PICO_MAX_TIMERS) {
+ dbg("Warning: I have %d timers\n", Timers->n);
+ }
+}
+
+void pico_stack_init(void)
+{
+
+#ifdef PICO_SUPPORT_IPV4
+ pico_protocol_init(&pico_proto_ipv4);
+#endif
+
+#ifdef PICO_SUPPORT_IPV6
+ pico_protocol_init(&pico_proto_ipv6);
+#endif
+
+#ifdef PICO_SUPPORT_ICMP4
+ pico_protocol_init(&pico_proto_icmp4);
+#endif
+
+#ifdef PICO_SUPPORT_IGMP
+ pico_protocol_init(&pico_proto_igmp);
+#endif
+
+#ifdef PICO_SUPPORT_UDP
+ pico_protocol_init(&pico_proto_udp);
+#endif
+
+#ifdef PICO_SUPPORT_TCP
+ pico_protocol_init(&pico_proto_tcp);
+#endif
+
+#ifdef PICO_SUPPORT_DNS_CLIENT
+ pico_dns_client_init();
+#endif
+
+ pico_rand_feed(123456);
+
+ /* Initialize timer heap */
+ Timers = heap_init();
+ pico_stack_tick();
+ pico_stack_tick();
+ pico_stack_tick();
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_tree.c Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,496 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Author: Andrei Carp <andrei.carp@tass.be>
+*********************************************************************/
+
+#include "pico_tree.h"
+#include "pico_config.h"
+#include "pico_protocol.h"
+
+#define RED 0
+#define BLACK 1
+
+// By default the null leafs are black
+struct pico_tree_node LEAF = {
+ NULL, // key
+ &LEAF, &LEAF, &LEAF, // parent, left,right
+ BLACK, // color
+};
+
+#define IS_LEAF(x) (x == &LEAF)
+#define IS_NOT_LEAF(x) (x != &LEAF)
+#define INIT_LEAF (&LEAF)
+
+#define AM_I_LEFT_CHILD(x) (x == x->parent->leftChild)
+#define AM_I_RIGHT_CHILD(x) (x == x->parent->rightChild)
+
+#define PARENT(x) (x->parent)
+#define GRANPA(x) (x->parent->parent)
+
+/*
+ * Local Functions
+ */
+static struct pico_tree_node * create_node(struct pico_tree * tree,void *key);
+static void rotateToLeft(struct pico_tree* tree, struct pico_tree_node* node);
+static void rotateToRight(struct pico_tree* root, struct pico_tree_node* node);
+static void fix_insert_collisions(struct pico_tree* tree, struct pico_tree_node* node);
+static void fix_delete_collisions(struct pico_tree* tree, struct pico_tree_node * node);
+static void switchNodes(struct pico_tree* tree, struct pico_tree_node* nodeA, struct pico_tree_node* nodeB);
+
+/*
+ * Exported functions
+ */
+
+struct pico_tree_node *pico_tree_firstNode(struct pico_tree_node * node)
+{
+ while(IS_NOT_LEAF(node->leftChild))
+ node = node->leftChild;
+ return node;
+}
+
+struct pico_tree_node * pico_tree_lastNode(struct pico_tree_node * node)
+{
+ while(IS_NOT_LEAF(node->rightChild))
+ node = node->rightChild;
+ return node;
+}
+
+struct pico_tree_node * pico_tree_next(struct pico_tree_node * node)
+{
+ if(IS_NOT_LEAF(node->rightChild))
+ {
+ node = node->rightChild;
+ while(IS_NOT_LEAF(node->leftChild))
+ node = node->leftChild;
+ }
+ else
+ {
+ if (IS_NOT_LEAF(node->parent) && AM_I_LEFT_CHILD(node))
+ node = node->parent;
+ else {
+ while (IS_NOT_LEAF(node->parent) && AM_I_RIGHT_CHILD(node))
+ node = node->parent;
+
+ node = node->parent;
+ }
+ }
+ return node;
+}
+
+struct pico_tree_node * pico_tree_prev(struct pico_tree_node * node)
+{
+ if (IS_NOT_LEAF(node->leftChild)) {
+ node = node->leftChild;
+ while (IS_NOT_LEAF(node->rightChild))
+ node = node->rightChild;
+ } else {
+ if (IS_NOT_LEAF(node->parent) && AM_I_RIGHT_CHILD(node))
+ node = node->parent;
+ else {
+ while (IS_NOT_LEAF(node) && AM_I_LEFT_CHILD(node))
+ node = node->parent;
+
+ node = node->parent;
+ }
+ }
+ return node;
+}
+
+void * pico_tree_insert(struct pico_tree* tree, void * key){
+
+ struct pico_tree_node * last_node = INIT_LEAF;
+ struct pico_tree_node * temp = tree->root;
+ struct pico_tree_node * insert;
+ void * LocalKey;
+ int result = 0;
+
+ LocalKey = (IS_NOT_LEAF(tree->root) ? pico_tree_findKey(tree,key) : NULL);
+ // if node already in, bail out
+ if(LocalKey)
+ return LocalKey;
+ else
+ {
+ insert = create_node(tree,key);
+ if(!insert)
+ {
+ pico_err = PICO_ERR_ENOMEM;
+ // to let the user know that it couldn't insert
+ return (void *)&LEAF;
+ }
+ }
+
+ // search for the place to insert the new node
+ while(IS_NOT_LEAF(temp))
+ {
+ last_node = temp;
+ result = tree->compare(insert->keyValue,temp->keyValue);
+
+ temp = (result < 0 ? temp->leftChild : temp->rightChild);
+ }
+
+ // make the needed connections
+ insert->parent = last_node;
+
+ if(IS_LEAF(last_node))
+ tree->root = insert;
+ else{
+ result = tree->compare(insert->keyValue,last_node->keyValue);
+ if(result < 0)
+ last_node->leftChild = insert;
+ else
+ last_node->rightChild = insert;
+ }
+
+ // fix colour issues
+ fix_insert_collisions(tree, insert);
+
+ return NULL;
+}
+
+struct pico_tree_node * pico_tree_findNode(struct pico_tree * tree, void * key)
+{
+ struct pico_tree_node *found;
+
+ found = tree->root;
+
+ while(IS_NOT_LEAF(found))
+ {
+ int result;
+ result = tree->compare(found->keyValue, key);
+ if(result == 0)
+ {
+ return found;
+ }
+ else if(result < 0)
+ found = found->rightChild;
+ else
+ found = found->leftChild;
+ }
+
+
+
+ return NULL;
+}
+
+void * pico_tree_findKey(struct pico_tree * tree, void * key)
+{
+ struct pico_tree_node *found;
+
+
+ found = tree->root;
+
+ while(IS_NOT_LEAF(found))
+ {
+ int result;
+
+ result = tree->compare(found->keyValue, key);
+ if(result == 0)
+ return found->keyValue;
+ else if(result < 0)
+ found = found->rightChild;
+ else
+ found = found->leftChild;
+
+ }
+
+ return NULL;
+}
+
+void * pico_tree_first(struct pico_tree * tree)
+{
+ return pico_tree_firstNode(tree->root)->keyValue;
+}
+
+void * pico_tree_last(struct pico_tree * tree)
+{
+ return pico_tree_lastNode(tree->root)->keyValue;
+}
+
+void * pico_tree_delete(struct pico_tree * tree, void * key){
+
+ uint8_t nodeColor; // keeps the color of the node to be deleted
+ void * lkey; // keeps a copy of the key which will be removed
+ struct pico_tree_node * delete; // keeps a copy of the node to be extracted
+ struct pico_tree_node * temp; // temporary
+ struct pico_tree_node * ltemp; // used for switching nodes, as a copy
+
+ delete = pico_tree_findNode(tree, key);
+ ltemp = delete;
+
+ // this key isn't in the tree, bail out
+ if(!delete)
+ return NULL;
+
+ lkey = delete->keyValue;
+ nodeColor = delete->color;
+
+ if(IS_LEAF(delete->leftChild))
+ {
+ temp = ltemp->rightChild;
+ switchNodes(tree, ltemp, ltemp->rightChild);
+ }
+ else
+ if(IS_LEAF(delete->rightChild))
+ {
+ struct pico_tree_node * _ltemp = delete;
+ temp = _ltemp->leftChild;
+ switchNodes(tree, _ltemp, _ltemp->leftChild);
+ }
+ else{
+ struct pico_tree_node * min;
+ min = pico_tree_firstNode(delete->rightChild);
+ nodeColor = min->color;
+
+ temp = min->rightChild;
+ if(min->parent == ltemp && IS_NOT_LEAF(temp))
+ temp->parent = min;
+ else{
+ switchNodes(tree, min, min->rightChild);
+ min->rightChild = ltemp->rightChild;
+ if(IS_NOT_LEAF(min->rightChild)) min->rightChild->parent = min;
+ }
+ switchNodes(tree, ltemp, min);
+ min->leftChild = ltemp->leftChild;
+ if(IS_NOT_LEAF(min->leftChild)) min->leftChild->parent = min;
+ min->color = ltemp->color;
+ }
+
+ // deleted node is black, this will mess up the black path property
+ if(nodeColor == BLACK)
+ fix_delete_collisions(tree, temp);
+
+ pico_free(delete);
+
+ return lkey;
+}
+
+int pico_tree_empty(struct pico_tree * tree)
+{
+ return (!tree->root || IS_LEAF(tree->root));
+}
+
+/*
+ * Private functions
+ */
+static void rotateToLeft(struct pico_tree* tree, struct pico_tree_node* node)
+{
+ struct pico_tree_node* temp;
+
+ temp = node->rightChild;
+
+ if(temp == &LEAF) return;
+
+ node->rightChild = temp->leftChild;
+
+ if(IS_NOT_LEAF(temp->leftChild))
+ temp->leftChild->parent = node;
+
+ temp->parent = node->parent;
+
+ if(IS_LEAF(node->parent))
+ tree->root = temp;
+ else
+ if(node == node->parent->leftChild)
+ node->parent->leftChild = temp;
+ else
+ node->parent->rightChild = temp;
+
+ temp->leftChild = node;
+ node->parent = temp;
+}
+
+
+static void rotateToRight(struct pico_tree * tree, struct pico_tree_node * node)
+{
+ struct pico_tree_node* temp;
+
+ temp = node->leftChild;
+ node->leftChild = temp->rightChild;
+
+ if(temp == &LEAF) return;
+
+ if(IS_NOT_LEAF(temp->rightChild))
+ temp->rightChild->parent = node;
+
+ temp->parent = node->parent;
+
+ if(IS_LEAF(node->parent))
+ tree->root = temp;
+ else
+ if(node == node->parent->rightChild)
+ node->parent->rightChild = temp;
+ else
+ node->parent->leftChild = temp;
+
+ temp->rightChild = node;
+ node->parent = temp;
+ return;
+}
+
+static struct pico_tree_node * create_node(struct pico_tree * tree, void* key)
+{
+ struct pico_tree_node *temp;
+ IGNORE_PARAMETER(tree);
+ temp = (struct pico_tree_node *)pico_zalloc(sizeof(struct pico_tree_node));
+
+ if(!temp)
+ return NULL;
+
+ temp->keyValue = key;
+ temp->parent = &LEAF;
+ temp->leftChild = &LEAF;
+ temp->rightChild = &LEAF;
+ // by default every new node is red
+ temp->color = RED;
+ return temp;
+}
+
+/*
+ * This function fixes the possible collisions in the tree.
+ * Eg. if a node is red his children must be black !
+ */
+static void fix_insert_collisions(struct pico_tree* tree, struct pico_tree_node* node)
+{
+ struct pico_tree_node* temp;
+
+ while(node->parent->color == RED && IS_NOT_LEAF(GRANPA(node)) )
+ {
+ if(AM_I_RIGHT_CHILD(node->parent))
+ {
+ temp = GRANPA(node)->leftChild;
+ if(temp->color == RED){
+ node->parent->color = BLACK;
+ temp->color = BLACK;
+ GRANPA(node)->color = RED;
+ node = GRANPA(node);
+ }
+ else if(temp->color == BLACK){
+ if(node == node->parent->leftChild){
+ node = node->parent;
+ rotateToRight(tree, node);
+ }
+ node->parent->color = BLACK;
+ GRANPA(node)->color = RED;
+ rotateToLeft(tree, GRANPA(node));
+ }
+ }
+ else if(AM_I_LEFT_CHILD(node->parent))
+ {
+ temp = GRANPA(node)->rightChild;
+ if(temp->color == RED){
+ node->parent->color = BLACK;
+ temp->color = BLACK;
+ GRANPA(node)->color = RED;
+ node = GRANPA(node);
+ }
+ else if(temp->color == BLACK){
+ if(AM_I_RIGHT_CHILD(node)){
+ node = node->parent;
+ rotateToLeft(tree, node);
+ }
+ node->parent->color = BLACK;
+ GRANPA(node)->color = RED;
+ rotateToRight(tree, GRANPA(node));
+ }
+ }
+ }
+
+ // make sure that the root of the tree stays black
+ tree->root->color = BLACK;
+}
+
+static void switchNodes(struct pico_tree* tree, struct pico_tree_node* nodeA, struct pico_tree_node* nodeB)
+{
+
+ if(IS_LEAF(nodeA->parent))
+ tree->root = nodeB;
+ else
+ if(IS_NOT_LEAF(nodeA))
+ {
+ if(AM_I_LEFT_CHILD(nodeA))
+ nodeA->parent->leftChild = nodeB;
+ else
+ nodeA->parent->rightChild = nodeB;
+ }
+
+ if(IS_NOT_LEAF(nodeB)) nodeB->parent = nodeA->parent;
+
+}
+
+/*
+ * This function fixes the possible collisions in the tree.
+ * Eg. if a node is red his children must be black !
+ * In this case the function fixes the constant black path property.
+ */
+static void fix_delete_collisions(struct pico_tree* tree, struct pico_tree_node * node)
+{
+ struct pico_tree_node* temp;
+
+ while( node != tree->root && node->color == BLACK && IS_NOT_LEAF(node))
+ {
+ if(AM_I_LEFT_CHILD(node)){
+
+ temp = node->parent->rightChild;
+ if(temp->color == RED)
+ {
+ temp->color = BLACK;
+ node->parent->color = RED;
+ rotateToLeft(tree, node->parent);
+ temp = node->parent->rightChild;
+ }
+ if(temp->leftChild->color == BLACK && temp->rightChild->color == BLACK)
+ {
+ temp->color = RED;
+ node = node->parent;
+ }
+ else
+ {
+ if(temp->rightChild->color == BLACK)
+ {
+ temp->leftChild->color = BLACK;
+ temp->color = RED;
+ rotateToRight(tree, temp);
+ temp = temp->parent->rightChild;
+ }
+ temp->color = node->parent->color;
+ node->parent->color = BLACK;
+ temp->rightChild->color = BLACK;
+ rotateToLeft(tree, node->parent);
+ node = tree->root;
+ }
+ }
+ else{
+ temp = node->parent->leftChild;
+ if(temp->color == RED)
+ {
+ temp->color = BLACK;
+ node->parent->color = RED;
+ rotateToRight(tree, node->parent);
+ temp = node->parent->leftChild;
+ }
+ if(temp->rightChild->color == BLACK && temp->leftChild->color == BLACK)
+ {
+ temp->color = RED;
+ node = node->parent;
+ }
+ else{
+ if(temp->leftChild->color == BLACK)
+ {
+ temp->rightChild->color = BLACK;
+ temp->color = RED;
+ rotateToLeft(tree, temp);
+ temp = temp->parent->leftChild;
+ }
+ temp->color = node->parent->color;
+ node->parent->color = BLACK;
+ temp->leftChild->color = BLACK;
+ rotateToRight(tree, node->parent);
+ node = tree->root;
+ }
+ }
+ }
+
+ node->color = BLACK;
+}
