Daniele Lacamera
CDC/ECM driver for mbed, based on USBDevice by mbed-official. Uses PicoTCP to access Ethernet USB device. License: GPLv2
Fork of
USB_Ethernet
by 
Daniele Lacamera
Revision 2:540f6e142d59, committed 2013-08-03
- Comitter:
- daniele
- Date:
- Sat Aug 03 13:16:14 2013 +0000
- Parent:
- 1:759afa79ebe8
- Commit message:
- Moved to single package
Changed in this revision
diff -r 759afa79ebe8 -r 540f6e142d59 USBCDC_ECM.cpp
--- a/USBCDC_ECM.cpp Sat Aug 03 08:51:00 2013 +0000
+++ b/USBCDC_ECM.cpp Sat Aug 03 13:16:14 2013 +0000
@@ -1,32 +1,14 @@
-/* Copyright (c) 2010-2011 mbed.org, MIT License
-*
-* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
-* and associated documentation files (the "Software"), to deal in the Software without
-* restriction, including without limitation the rights to use, copy, modify, merge, publish,
-* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
-* Software is furnished to do so, subject to the following conditions:
-*
-* The above copyright notice and this permission notice shall be included in all copies or
-* substantial portions of the Software.
-*
-* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
-* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
-* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
-* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
-*/
+/*********************************************************************
+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: Daniele Lacamera, Julien Duraj
+*********************************************************************/
#include "stdint.h"
#include "USBCDC_ECM.h"
-
-static uint8_t cdc_line_coding[7]= {0x80, 0x25, 0x00, 0x00, 0x00, 0x00, 0x08};
-
#define DEFAULT_CONFIGURATION (1)
-
-#define CDC_SET_LINE_CODING 0x20
-#define CDC_GET_LINE_CODING 0x21
-#define CDC_SET_CONTROL_LINE_STATE 0x22
-
#define MAX_CDC_REPORT_SIZE MAX_PACKET_SIZE_EPBULK
USBCDC_ECM::USBCDC_ECM(uint16_t vendor_id, uint16_t product_id, uint16_t product_release): USBDevice(vendor_id, product_id, product_release) {
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBDescriptor.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice/USBDescriptor.h Sat Aug 03 13:16:14 2013 +0000 @@ -0,0 +1,74 @@ +/* Copyright (c) 2010-2011 mbed.org, MIT License +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software +* and associated documentation files (the "Software"), to deal in the Software without +* restriction, including without limitation the rights to use, copy, modify, merge, publish, +* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the +* Software is furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all copies or +* substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING +* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +/* Standard descriptor types */ +#define DEVICE_DESCRIPTOR (1) +#define CONFIGURATION_DESCRIPTOR (2) +#define STRING_DESCRIPTOR (3) +#define INTERFACE_DESCRIPTOR (4) +#define ENDPOINT_DESCRIPTOR (5) +#define QUALIFIER_DESCRIPTOR (6) + +/* Standard descriptor lengths */ +#define DEVICE_DESCRIPTOR_LENGTH (0x12) +#define CONFIGURATION_DESCRIPTOR_LENGTH (0x09) +#define INTERFACE_DESCRIPTOR_LENGTH (0x09) +#define ENDPOINT_DESCRIPTOR_LENGTH (0x07) + + +/*string offset*/ +#define STRING_OFFSET_LANGID (0) +#define STRING_OFFSET_IMANUFACTURER (1) +#define STRING_OFFSET_IPRODUCT (2) +#define STRING_OFFSET_ISERIAL (3) +#define STRING_OFFSET_ICONFIGURATION (4) +#define STRING_OFFSET_IINTERFACE (5) + +/* USB Specification Release Number */ +#define USB_VERSION_2_0 (0x0200) + +/* Least/Most significant byte of short integer */ +#define LSB(n) ((n)&0xff) +#define MSB(n) (((n)&0xff00)>>8) + +/* Convert physical endpoint number to descriptor endpoint number */ +#define PHY_TO_DESC(endpoint) (((endpoint)>>1) | (((endpoint) & 1) ? 0x80:0)) + +/* bmAttributes in configuration descriptor */ +/* C_RESERVED must always be set */ +#define C_RESERVED (1U<<7) +#define C_SELF_POWERED (1U<<6) +#define C_REMOTE_WAKEUP (1U<<5) + +/* bMaxPower in configuration descriptor */ +#define C_POWER(mA) ((mA)/2) + +/* bmAttributes in endpoint descriptor */ +#define E_CONTROL (0x00) +#define E_ISOCHRONOUS (0x01) +#define E_BULK (0x02) +#define E_INTERRUPT (0x03) + +/* For isochronous endpoints only: */ +#define E_NO_SYNCHRONIZATION (0x00) +#define E_ASYNCHRONOUS (0x04) +#define E_ADAPTIVE (0x08) +#define E_SYNCHRONOUS (0x0C) +#define E_DATA (0x00) +#define E_FEEDBACK (0x10) +#define E_IMPLICIT_FEEDBACK (0x20)
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBDevice.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBDevice.cpp Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,977 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "stdint.h"
+
+#include "USBEndpoints.h"
+#include "USBDevice.h"
+#include "USBDescriptor.h"
+
+#define DEBUG
+
+/* Device status */
+#define DEVICE_STATUS_SELF_POWERED (1U<<0)
+#define DEVICE_STATUS_REMOTE_WAKEUP (1U<<1)
+
+/* Endpoint status */
+#define ENDPOINT_STATUS_HALT (1U<<0)
+
+/* Standard feature selectors */
+#define DEVICE_REMOTE_WAKEUP (1)
+#define ENDPOINT_HALT (0)
+
+/* Macro to convert wIndex endpoint number to physical endpoint number */
+#define WINDEX_TO_PHYSICAL(endpoint) (((endpoint & 0x0f) << 1) + \
+ ((endpoint & 0x80) ? 1 : 0))
+
+
+bool USBDevice::requestGetDescriptor(void)
+{
+ bool success = false;
+#ifdef DEBUG
+ printf("get descr: type: %d\r\n", DESCRIPTOR_TYPE(transfer.setup.wValue));
+#endif
+ switch (DESCRIPTOR_TYPE(transfer.setup.wValue))
+ {
+ case DEVICE_DESCRIPTOR:
+ if (deviceDesc() != NULL)
+ {
+ if ((deviceDesc()[0] == DEVICE_DESCRIPTOR_LENGTH) \
+ && (deviceDesc()[1] == DEVICE_DESCRIPTOR))
+ {
+#ifdef DEBUG
+ printf("device descr\r\n");
+#endif
+ transfer.remaining = DEVICE_DESCRIPTOR_LENGTH;
+ transfer.ptr = deviceDesc();
+ transfer.direction = DEVICE_TO_HOST;
+ success = true;
+ }
+ }
+ break;
+ case CONFIGURATION_DESCRIPTOR:
+ if (configurationDesc() != NULL)
+ {
+ if ((configurationDesc()[0] == CONFIGURATION_DESCRIPTOR_LENGTH) \
+ && (configurationDesc()[1] == CONFIGURATION_DESCRIPTOR))
+ {
+#ifdef DEBUG
+ printf("conf descr request\r\n");
+#endif
+ /* Get wTotalLength */
+ transfer.remaining = configurationDesc()[2] \
+ | (configurationDesc()[3] << 8);
+
+ transfer.ptr = configurationDesc();
+ transfer.direction = DEVICE_TO_HOST;
+ success = true;
+ }
+ }
+ break;
+ case STRING_DESCRIPTOR:
+#ifdef DEBUG
+ printf("str descriptor\r\n");
+#endif
+ switch (DESCRIPTOR_INDEX(transfer.setup.wValue))
+ {
+ case STRING_OFFSET_LANGID:
+#ifdef DEBUG
+ printf("1\r\n");
+#endif
+ transfer.remaining = stringLangidDesc()[0];
+ transfer.ptr = stringLangidDesc();
+ transfer.direction = DEVICE_TO_HOST;
+ success = true;
+ break;
+ case STRING_OFFSET_IMANUFACTURER:
+#ifdef DEBUG
+ printf("2\r\n");
+#endif
+ transfer.remaining = stringImanufacturerDesc()[0];
+ transfer.ptr = stringImanufacturerDesc();
+ transfer.direction = DEVICE_TO_HOST;
+ success = true;
+ break;
+ case STRING_OFFSET_IPRODUCT:
+#ifdef DEBUG
+ printf("3\r\n");
+#endif
+ transfer.remaining = stringIproductDesc()[0];
+ transfer.ptr = stringIproductDesc();
+ transfer.direction = DEVICE_TO_HOST;
+ success = true;
+ break;
+ case STRING_OFFSET_ISERIAL:
+#ifdef DEBUG
+ printf("4\r\n");
+#endif
+ transfer.remaining = stringIserialDesc()[0];
+ transfer.ptr = stringIserialDesc();
+ transfer.direction = DEVICE_TO_HOST;
+ success = true;
+ break;
+ case STRING_OFFSET_ICONFIGURATION:
+#ifdef DEBUG
+ printf("5\r\n");
+#endif
+ transfer.remaining = stringIConfigurationDesc()[0];
+ transfer.ptr = stringIConfigurationDesc();
+ transfer.direction = DEVICE_TO_HOST;
+ success = true;
+ break;
+ case STRING_OFFSET_IINTERFACE:
+#ifdef DEBUG
+ printf("6\r\n");
+#endif
+ transfer.remaining = stringIinterfaceDesc()[0];
+ transfer.ptr = stringIinterfaceDesc();
+ transfer.direction = DEVICE_TO_HOST;
+ success = true;
+ break;
+ }
+ break;
+ case INTERFACE_DESCRIPTOR:
+#ifdef DEBUG
+ printf("interface descr\r\n");
+#endif
+ case ENDPOINT_DESCRIPTOR:
+#ifdef DEBUG
+ printf("endpoint descr\r\n");
+#endif
+ /* TODO: Support is optional, not implemented here */
+ break;
+ default:
+#ifdef DEBUG
+ printf("ERROR\r\n");
+#endif
+ break;
+ }
+
+ return success;
+}
+
+void USBDevice::decodeSetupPacket(uint8_t *data, SETUP_PACKET *packet)
+{
+ /* Fill in the elements of a SETUP_PACKET structure from raw data */
+ packet->bmRequestType.dataTransferDirection = (data[0] & 0x80) >> 7;
+ packet->bmRequestType.Type = (data[0] & 0x60) >> 5;
+ packet->bmRequestType.Recipient = data[0] & 0x1f;
+ packet->bRequest = data[1];
+ packet->wValue = (data[2] | (uint16_t)data[3] << 8);
+ packet->wIndex = (data[4] | (uint16_t)data[5] << 8);
+ packet->wLength = (data[6] | (uint16_t)data[7] << 8);
+}
+
+
+bool USBDevice::controlOut(void)
+{
+ /* Control transfer data OUT stage */
+ uint8_t buffer[MAX_PACKET_SIZE_EP0];
+ uint32_t packetSize;
+
+ /* Check we should be transferring data OUT */
+ if (transfer.direction != HOST_TO_DEVICE)
+ {
+ return false;
+ }
+
+ /* Read from endpoint */
+ packetSize = EP0getReadResult(buffer);
+
+ /* Check if transfer size is valid */
+ if (packetSize > transfer.remaining)
+ {
+ /* Too big */
+ return false;
+ }
+
+ /* Update transfer */
+ transfer.ptr += packetSize;
+ transfer.remaining -= packetSize;
+
+ /* Check if transfer has completed */
+ if (transfer.remaining == 0)
+ {
+ /* Transfer completed */
+ if (transfer.notify)
+ {
+ /* Notify class layer. */
+ USBCallback_requestCompleted(buffer, packetSize);
+ transfer.notify = false;
+ }
+ /* Status stage */
+ EP0write(NULL, 0);
+ }
+ else
+ {
+ EP0read();
+ }
+
+ return true;
+}
+
+bool USBDevice::controlIn(void)
+{
+ /* Control transfer data IN stage */
+ uint32_t packetSize;
+
+ /* Check if transfer has completed (status stage transactions */
+ /* also have transfer.remaining == 0) */
+ if (transfer.remaining == 0)
+ {
+ if (transfer.zlp)
+ {
+ /* Send zero length packet */
+ EP0write(NULL, 0);
+ transfer.zlp = false;
+ }
+
+ /* Transfer completed */
+ if (transfer.notify)
+ {
+ /* Notify class layer. */
+ USBCallback_requestCompleted(NULL, 0);
+ transfer.notify = false;
+ }
+
+ EP0read();
+ EP0readStage();
+
+ /* Completed */
+ return true;
+ }
+
+ /* Check we should be transferring data IN */
+ if (transfer.direction != DEVICE_TO_HOST)
+ {
+ return false;
+ }
+
+ packetSize = transfer.remaining;
+
+ if (packetSize > MAX_PACKET_SIZE_EP0)
+ {
+ packetSize = MAX_PACKET_SIZE_EP0;
+ }
+
+ /* Write to endpoint */
+ EP0write(transfer.ptr, packetSize);
+
+ /* Update transfer */
+ transfer.ptr += packetSize;
+ transfer.remaining -= packetSize;
+
+ return true;
+}
+
+bool USBDevice::requestSetAddress(void)
+{
+ /* Set the device address */
+ setAddress(transfer.setup.wValue);
+
+ if (transfer.setup.wValue == 0)
+ {
+ device.state = DEFAULT;
+ }
+ else
+ {
+ device.state = ADDRESS;
+ }
+
+ return true;
+}
+
+bool USBDevice::requestSetConfiguration(void)
+{
+
+ device.configuration = transfer.setup.wValue;
+ /* Set the device configuration */
+ if (device.configuration == 0)
+ {
+ /* Not configured */
+ unconfigureDevice();
+ device.state = ADDRESS;
+ }
+ else
+ {
+ if (USBCallback_setConfiguration(device.configuration))
+ {
+ /* Valid configuration */
+ configureDevice();
+ device.state = CONFIGURED;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool USBDevice::requestGetConfiguration(void)
+{
+ /* Send the device configuration */
+ transfer.ptr = &device.configuration;
+ transfer.remaining = sizeof(device.configuration);
+ transfer.direction = DEVICE_TO_HOST;
+ return true;
+}
+
+bool USBDevice::requestGetInterface(void)
+{
+ /* Return the selected alternate setting for an interface */
+
+ if (device.state != CONFIGURED)
+ {
+ return false;
+ }
+
+ /* Send the alternate setting */
+ transfer.setup.wIndex = currentInterface;
+ transfer.ptr = ¤tAlternate;
+ transfer.remaining = sizeof(currentAlternate);
+ transfer.direction = DEVICE_TO_HOST;
+ return true;
+}
+
+bool USBDevice::requestSetInterface(void)
+{
+ bool success = false;
+ if(USBCallback_setInterface(transfer.setup.wIndex, transfer.setup.wValue))
+ {
+ success = true;
+ currentInterface = transfer.setup.wIndex;
+ currentAlternate = transfer.setup.wValue;
+ }
+ return success;
+}
+
+bool USBDevice::requestSetFeature()
+{
+ bool success = false;
+
+ if (device.state != CONFIGURED)
+ {
+ /* Endpoint or interface must be zero */
+ if (transfer.setup.wIndex != 0)
+ {
+ return false;
+ }
+ }
+
+ switch (transfer.setup.bmRequestType.Recipient)
+ {
+ case DEVICE_RECIPIENT:
+ /* TODO: Remote wakeup feature not supported */
+ break;
+ case ENDPOINT_RECIPIENT:
+ if (transfer.setup.wValue == ENDPOINT_HALT)
+ {
+ /* TODO: We should check that the endpoint number is valid */
+ stallEndpoint(
+ WINDEX_TO_PHYSICAL(transfer.setup.wIndex));
+ success = true;
+ }
+ break;
+ default:
+ break;
+ }
+
+ return success;
+}
+
+bool USBDevice::requestClearFeature()
+{
+ bool success = false;
+
+ if (device.state != CONFIGURED)
+ {
+ /* Endpoint or interface must be zero */
+ if (transfer.setup.wIndex != 0)
+ {
+ return false;
+ }
+ }
+
+ switch (transfer.setup.bmRequestType.Recipient)
+ {
+ case DEVICE_RECIPIENT:
+ /* TODO: Remote wakeup feature not supported */
+ break;
+ case ENDPOINT_RECIPIENT:
+ /* TODO: We should check that the endpoint number is valid */
+ if (transfer.setup.wValue == ENDPOINT_HALT)
+ {
+ unstallEndpoint( WINDEX_TO_PHYSICAL(transfer.setup.wIndex));
+ success = true;
+ }
+ break;
+ default:
+ break;
+ }
+
+ return success;
+}
+
+bool USBDevice::requestGetStatus(void)
+{
+ static uint16_t status;
+ bool success = false;
+
+ if (device.state != CONFIGURED)
+ {
+ /* Endpoint or interface must be zero */
+ if (transfer.setup.wIndex != 0)
+ {
+ return false;
+ }
+ }
+
+ switch (transfer.setup.bmRequestType.Recipient)
+ {
+ case DEVICE_RECIPIENT:
+ /* TODO: Currently only supports self powered devices */
+ status = DEVICE_STATUS_SELF_POWERED;
+ success = true;
+ break;
+ case INTERFACE_RECIPIENT:
+ status = 0;
+ success = true;
+ break;
+ case ENDPOINT_RECIPIENT:
+ /* TODO: We should check that the endpoint number is valid */
+ if (getEndpointStallState(
+ WINDEX_TO_PHYSICAL(transfer.setup.wIndex)))
+ {
+ status = ENDPOINT_STATUS_HALT;
+ }
+ else
+ {
+ status = 0;
+ }
+ success = true;
+ break;
+ default:
+ break;
+ }
+
+ if (success)
+ {
+ /* Send the status */
+ transfer.ptr = (uint8_t *)&status; /* Assumes little endian */
+ transfer.remaining = sizeof(status);
+ transfer.direction = DEVICE_TO_HOST;
+ }
+
+ return success;
+}
+
+bool USBDevice::requestSetup(void)
+{
+ bool success = false;
+
+ /* Process standard requests */
+ if ((transfer.setup.bmRequestType.Type == STANDARD_TYPE))
+ {
+ switch (transfer.setup.bRequest)
+ {
+ case GET_STATUS:
+ success = requestGetStatus();
+ break;
+ case CLEAR_FEATURE:
+ success = requestClearFeature();
+ break;
+ case SET_FEATURE:
+ success = requestSetFeature();
+ break;
+ case SET_ADDRESS:
+ success = requestSetAddress();
+ break;
+ case GET_DESCRIPTOR:
+ success = requestGetDescriptor();
+ break;
+ case SET_DESCRIPTOR:
+ /* TODO: Support is optional, not implemented here */
+ success = false;
+ break;
+ case GET_CONFIGURATION:
+ success = requestGetConfiguration();
+ break;
+ case SET_CONFIGURATION:
+ success = requestSetConfiguration();
+ break;
+ case GET_INTERFACE:
+ success = requestGetInterface();
+ break;
+ case SET_INTERFACE:
+ success = requestSetInterface();
+ break;
+ default:
+ break;
+ }
+ }
+
+ return success;
+}
+
+bool USBDevice::controlSetup(void)
+{
+ bool success = false;
+
+ /* Control transfer setup stage */
+ uint8_t buffer[MAX_PACKET_SIZE_EP0];
+
+ EP0setup(buffer);
+
+ /* Initialise control transfer state */
+ decodeSetupPacket(buffer, &transfer.setup);
+ transfer.ptr = NULL;
+ transfer.remaining = 0;
+ transfer.direction = 0;
+ transfer.zlp = false;
+ transfer.notify = false;
+
+#ifdef DEBUG
+ printf("dataTransferDirection: %d\r\nType: %d\r\nRecipient: %d\r\nbRequest: %d\r\nwValue: %d\r\nwIndex: %d\r\nwLength: %d\r\n",transfer.setup.bmRequestType.dataTransferDirection,
+ transfer.setup.bmRequestType.Type,
+ transfer.setup.bmRequestType.Recipient,
+ transfer.setup.bRequest,
+ transfer.setup.wValue,
+ transfer.setup.wIndex,
+ transfer.setup.wLength);
+#endif
+
+ /* Class / vendor specific */
+ success = USBCallback_request();
+
+ if (!success)
+ {
+ /* Standard requests */
+ if (!requestSetup())
+ {
+#ifdef DEBUG
+ printf("fail!!!!\r\n");
+#endif
+ return false;
+ }
+ }
+
+ /* Check transfer size and direction */
+ if (transfer.setup.wLength>0)
+ {
+ if (transfer.setup.bmRequestType.dataTransferDirection \
+ == DEVICE_TO_HOST)
+ {
+ /* IN data stage is required */
+ if (transfer.direction != DEVICE_TO_HOST)
+ {
+ return false;
+ }
+
+ /* Transfer must be less than or equal to the size */
+ /* requested by the host */
+ if (transfer.remaining > transfer.setup.wLength)
+ {
+ transfer.remaining = transfer.setup.wLength;
+ }
+ }
+ else
+ {
+
+ /* OUT data stage is required */
+ if (transfer.direction != HOST_TO_DEVICE)
+ {
+ return false;
+ }
+
+ /* Transfer must be equal to the size requested by the host */
+ if (transfer.remaining != transfer.setup.wLength)
+ {
+ return false;
+ }
+ }
+ }
+ else
+ {
+ /* No data stage; transfer size must be zero */
+ if (transfer.remaining != 0)
+ {
+ return false;
+ }
+ }
+
+ /* Data or status stage if applicable */
+ if (transfer.setup.wLength>0)
+ {
+ if (transfer.setup.bmRequestType.dataTransferDirection \
+ == DEVICE_TO_HOST)
+ {
+ /* Check if we'll need to send a zero length packet at */
+ /* the end of this transfer */
+ if (transfer.setup.wLength > transfer.remaining)
+ {
+ /* Device wishes to transfer less than host requested */
+ if ((transfer.remaining % MAX_PACKET_SIZE_EP0) == 0)
+ {
+ /* Transfer is a multiple of EP0 max packet size */
+ transfer.zlp = true;
+ }
+ }
+
+ /* IN stage */
+ controlIn();
+ }
+ else
+ {
+ /* OUT stage */
+ EP0read();
+ }
+ }
+ else
+ {
+ /* Status stage */
+ EP0write(NULL, 0);
+ }
+
+ return true;
+}
+
+void USBDevice::busReset(void)
+{
+ device.state = DEFAULT;
+ device.configuration = 0;
+ device.suspended = false;
+
+ /* Call class / vendor specific busReset function */
+ USBCallback_busReset();
+}
+
+void USBDevice::EP0setupCallback(void)
+{
+ /* Endpoint 0 setup event */
+ if (!controlSetup())
+ {
+ /* Protocol stall */
+ EP0stall();
+ }
+
+ /* Return true if an OUT data stage is expected */
+}
+
+void USBDevice::EP0out(void)
+{
+ /* Endpoint 0 OUT data event */
+ if (!controlOut())
+ {
+ /* Protocol stall; this will stall both endpoints */
+ EP0stall();
+ }
+}
+
+void USBDevice::EP0in(void)
+{
+#ifdef DEBUG
+ printf("EP0IN\r\n");
+#endif
+ /* Endpoint 0 IN data event */
+ if (!controlIn())
+ {
+ /* Protocol stall; this will stall both endpoints */
+ EP0stall();
+ }
+}
+
+bool USBDevice::configured(void)
+{
+ /* Returns true if device is in the CONFIGURED state */
+ return (device.state == CONFIGURED);
+}
+
+void USBDevice::connect(void)
+{
+ /* Connect device */
+ USBHAL::connect();
+ /* Block if not configured */
+ while (!configured());
+}
+
+void USBDevice::disconnect(void)
+{
+ /* Disconnect device */
+ USBHAL::disconnect();
+}
+
+CONTROL_TRANSFER * USBDevice::getTransferPtr(void)
+{
+ return &transfer;
+}
+
+bool USBDevice::addEndpoint(uint8_t endpoint, uint32_t maxPacket)
+{
+ return realiseEndpoint(endpoint, maxPacket, 0);
+}
+
+bool USBDevice::addRateFeedbackEndpoint(uint8_t endpoint, uint32_t maxPacket)
+{
+ /* For interrupt endpoints only */
+ return realiseEndpoint(endpoint, maxPacket, RATE_FEEDBACK_MODE);
+}
+
+uint8_t * USBDevice::findDescriptor(uint8_t descriptorType)
+{
+ /* Find a descriptor within the list of descriptors */
+ /* following a configuration descriptor. */
+ uint16_t wTotalLength;
+ uint8_t *ptr;
+
+ if (configurationDesc() == NULL)
+ {
+ return NULL;
+ }
+
+ /* Check this is a configuration descriptor */
+ if ((configurationDesc()[0] != CONFIGURATION_DESCRIPTOR_LENGTH) \
+ || (configurationDesc()[1] != CONFIGURATION_DESCRIPTOR))
+ {
+ return NULL;
+ }
+
+ wTotalLength = configurationDesc()[2] | (configurationDesc()[3] << 8);
+
+ /* Check there are some more descriptors to follow */
+ if (wTotalLength <= (CONFIGURATION_DESCRIPTOR_LENGTH+2))
+ /* +2 is for bLength and bDescriptorType of next descriptor */
+ {
+ return false;
+ }
+
+ /* Start at first descriptor after the configuration descriptor */
+ ptr = &(configurationDesc()[CONFIGURATION_DESCRIPTOR_LENGTH]);
+
+ do {
+ if (ptr[1] /* bDescriptorType */ == descriptorType)
+ {
+ /* Found */
+ return ptr;
+ }
+
+ /* Skip to next descriptor */
+ ptr += ptr[0]; /* bLength */
+ } while (ptr < (configurationDesc() + wTotalLength));
+
+ /* Reached end of the descriptors - not found */
+ return NULL;
+}
+
+
+void USBDevice::connectStateChanged(unsigned int connected)
+{
+}
+
+void USBDevice::suspendStateChanged(unsigned int suspended)
+{
+}
+
+
+USBDevice::USBDevice(uint16_t vendor_id, uint16_t product_id, uint16_t product_release){
+ VENDOR_ID = vendor_id;
+ PRODUCT_ID = product_id;
+ PRODUCT_RELEASE = product_release;
+
+ /* Set initial device state */
+ device.state = POWERED;
+ device.configuration = 0;
+ device.suspended = false;
+};
+
+
+bool USBDevice::readStart(uint8_t endpoint, uint32_t maxSize)
+{
+ return endpointRead(endpoint, maxSize) == EP_PENDING;
+}
+
+
+bool USBDevice::write(uint8_t endpoint, uint8_t * buffer, uint32_t size, uint32_t maxSize)
+{
+ EP_STATUS result;
+
+ if (size > maxSize)
+ {
+ return false;
+ }
+
+
+ if(!configured()) {
+ return false;
+ }
+
+ /* Send report */
+ result = endpointWrite(endpoint, buffer, size);
+
+ if (result != EP_PENDING)
+ {
+ return false;
+ }
+
+ /* Wait for completion */
+ do {
+ result = endpointWriteResult(endpoint);
+ } while ((result == EP_PENDING) && configured());
+
+ return (result == EP_COMPLETED);
+}
+
+
+bool USBDevice::writeNB(uint8_t endpoint, uint8_t * buffer, uint32_t size, uint32_t maxSize)
+{
+ EP_STATUS result;
+
+ if (size > maxSize)
+ {
+ return false;
+ }
+
+ if(!configured()) {
+ return false;
+ }
+
+ /* Send report */
+ result = endpointWrite(endpoint, buffer, size);
+
+ if (result != EP_PENDING)
+ {
+ return false;
+ }
+
+ result = endpointWriteResult(endpoint);
+
+ return (result == EP_COMPLETED);
+}
+
+
+
+bool USBDevice::readEP(uint8_t endpoint, uint8_t * buffer, uint32_t * size, uint32_t maxSize)
+{
+ EP_STATUS result;
+
+ if(!configured()) {
+ return false;
+ }
+
+ /* Wait for completion */
+ do {
+ result = endpointReadResult(endpoint, buffer, size);
+ } while ((result == EP_PENDING) && configured());
+
+ return (result == EP_COMPLETED);
+}
+
+
+bool USBDevice::readEP_NB(uint8_t endpoint, uint8_t * buffer, uint32_t * size, uint32_t maxSize)
+{
+ EP_STATUS result;
+
+ if(!configured()) {
+ return false;
+ }
+
+ result = endpointReadResult(endpoint, buffer, size);
+
+ return (result == EP_COMPLETED);
+}
+
+
+
+uint8_t * USBDevice::deviceDesc() {
+ static uint8_t deviceDescriptor[] = {
+ DEVICE_DESCRIPTOR_LENGTH, /* bLength */
+ DEVICE_DESCRIPTOR, /* bDescriptorType */
+ LSB(USB_VERSION_2_0), /* bcdUSB (LSB) */
+ MSB(USB_VERSION_2_0), /* bcdUSB (MSB) */
+ 0x00, /* bDeviceClass */
+ 0x00, /* bDeviceSubClass */
+ 0x00, /* bDeviceprotocol */
+ MAX_PACKET_SIZE_EP0, /* bMaxPacketSize0 */
+ LSB(VENDOR_ID), /* idVendor (LSB) */
+ MSB(VENDOR_ID), /* idVendor (MSB) */
+ LSB(PRODUCT_ID), /* idProduct (LSB) */
+ MSB(PRODUCT_ID), /* idProduct (MSB) */
+ LSB(PRODUCT_RELEASE), /* bcdDevice (LSB) */
+ MSB(PRODUCT_RELEASE), /* bcdDevice (MSB) */
+ STRING_OFFSET_IMANUFACTURER, /* iManufacturer */
+ STRING_OFFSET_IPRODUCT, /* iProduct */
+ STRING_OFFSET_ISERIAL, /* iSerialNumber */
+ 0x01 /* bNumConfigurations */
+ };
+ return deviceDescriptor;
+}
+
+uint8_t * USBDevice::stringLangidDesc() {
+ static uint8_t stringLangidDescriptor[] = {
+ 0x04, /*bLength*/
+ STRING_DESCRIPTOR, /*bDescriptorType 0x03*/
+ 0x09,0x00, /*bString Lang ID - 0x009 - English*/
+ };
+ return stringLangidDescriptor;
+}
+
+uint8_t * USBDevice::stringImanufacturerDesc() {
+ static uint8_t stringImanufacturerDescriptor[] = {
+ 0x12, /*bLength*/
+ STRING_DESCRIPTOR, /*bDescriptorType 0x03*/
+ 'm',0,'b',0,'e',0,'d',0,'.',0,'o',0,'r',0,'g',0, /*bString iManufacturer - mbed.org*/
+ };
+ return stringImanufacturerDescriptor;
+}
+
+uint8_t * USBDevice::stringIserialDesc() {
+ static uint8_t stringIserialDescriptor[] = {
+ 0x16, /*bLength*/
+ STRING_DESCRIPTOR, /*bDescriptorType 0x03*/
+ '0',0,'1',0,'2',0,'3',0,'4',0,'5',0,'6',0,'7',0,'8',0,'9',0, /*bString iSerial - 0123456789*/
+ };
+ return stringIserialDescriptor;
+}
+
+uint8_t * USBDevice::stringIConfigurationDesc() {
+ static uint8_t stringIconfigurationDescriptor[] = {
+ 0x06, /*bLength*/
+ STRING_DESCRIPTOR, /*bDescriptorType 0x03*/
+ '0',0,'1',0, /*bString iConfiguration - 01*/
+ };
+ return stringIconfigurationDescriptor;
+}
+
+uint8_t * USBDevice::stringIinterfaceDesc() {
+ static uint8_t stringIinterfaceDescriptor[] = {
+ 0x08, /*bLength*/
+ STRING_DESCRIPTOR, /*bDescriptorType 0x03*/
+ 'U',0,'S',0,'B',0, /*bString iInterface - USB*/
+ };
+ return stringIinterfaceDescriptor;
+}
+
+uint8_t * USBDevice::stringIproductDesc() {
+ static uint8_t stringIproductDescriptor[] = {
+ 0x16, /*bLength*/
+ STRING_DESCRIPTOR, /*bDescriptorType 0x03*/
+ 'U',0,'S',0,'B',0,' ',0,'D',0,'E',0,'V',0,'I',0,'C',0,'E',0 /*bString iProduct - USB DEVICE*/
+ };
+ return stringIproductDescriptor;
+}
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBDevice.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBDevice.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,269 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef USBDEVICE_H
+#define USBDEVICE_H
+
+#include "mbed.h"
+#include "USBDevice_Types.h"
+#include "USBHAL.h"
+
+class USBDevice: public USBHAL
+{
+public:
+ USBDevice(uint16_t vendor_id, uint16_t product_id, uint16_t product_release);
+
+ /*
+ * Check if the device is configured
+ *
+ * @returns true if configured, false otherwise
+ */
+ bool configured(void);
+
+ /*
+ * Connect a device
+ */
+ void connect(void);
+
+ /*
+ * Disconnect a device
+ */
+ void disconnect(void);
+
+ /*
+ * Add an endpoint
+ *
+ * @param endpoint endpoint which will be added
+ * @param maxPacket Maximum size of a packet which can be sent for this endpoint
+ * @returns true if successful, false otherwise
+ */
+ bool addEndpoint(uint8_t endpoint, uint32_t maxPacket);
+
+ /*
+ * Start a reading on a certain endpoint.
+ * You can access the result of the reading by USBDevice_read
+ *
+ * @param endpoint endpoint which will be read
+ * @param maxSize the maximum length that can be read
+ * @return true if successful
+ */
+ bool readStart(uint8_t endpoint, uint32_t maxSize);
+
+ /*
+ * Read a certain endpoint. Before calling this function, USBUSBDevice_readStart
+ * must be called.
+ *
+ * Warning: blocking
+ *
+ * @param endpoint endpoint which will be read
+ * @param buffer buffer will be filled with the data received
+ * @param size the number of bytes read will be stored in *size
+ * @param maxSize the maximum length that can be read
+ * @returns true if successful
+ */
+ bool readEP(uint8_t endpoint, uint8_t * buffer, uint32_t * size, uint32_t maxSize);
+
+ /*
+ * Read a certain endpoint.
+ *
+ * Warning: non blocking
+ *
+ * @param endpoint endpoint which will be read
+ * @param buffer buffer will be filled with the data received (if data are available)
+ * @param size the number of bytes read will be stored in *size
+ * @param maxSize the maximum length that can be read
+ * @returns true if successful
+ */
+ bool readEP_NB(uint8_t endpoint, uint8_t * buffer, uint32_t * size, uint32_t maxSize);
+
+ /*
+ * Write a certain endpoint.
+ *
+ * Warning: blocking
+ *
+ * @param endpoint endpoint to write
+ * @param buffer data contained in buffer will be write
+ * @param size the number of bytes to write
+ * @param maxSize the maximum length that can be written on this endpoint
+ */
+ bool write(uint8_t endpoint, uint8_t * buffer, uint32_t size, uint32_t maxSize);
+
+
+ /*
+ * Write a certain endpoint.
+ *
+ * Warning: non blocking
+ *
+ * @param endpoint endpoint to write
+ * @param buffer data contained in buffer will be write
+ * @param size the number of bytes to write
+ * @param maxSize the maximum length that can be written on this endpoint
+ */
+ bool writeNB(uint8_t endpoint, uint8_t * buffer, uint32_t size, uint32_t maxSize);
+
+
+ /*
+ * Called by USBDevice layer on bus reset. Warning: Called in ISR context
+ *
+ * May be used to reset state
+ */
+ virtual void USBCallback_busReset(void) {};
+
+ /*
+ * Called by USBDevice on Endpoint0 request. Warning: Called in ISR context
+ * This is used to handle extensions to standard requests
+ * and class specific requests
+ *
+ * @returns true if class handles this request
+ */
+ virtual bool USBCallback_request() { return false; };
+
+ /*
+ * Called by USBDevice on Endpoint0 request completion
+ * if the 'notify' flag has been set to true. Warning: Called in ISR context
+ *
+ * In this case it is used to indicate that a HID report has
+ * been received from the host on endpoint 0
+ *
+ * @param buf buffer received on endpoint 0
+ * @param length length of this buffer
+ */
+ virtual void USBCallback_requestCompleted(uint8_t * buf, uint32_t length) {};
+
+ /*
+ * Called by USBDevice layer. Set configuration of the device.
+ * For instance, you can add all endpoints that you need on this function.
+ *
+ * @param configuration Number of the configuration
+ */
+ virtual bool USBCallback_setConfiguration(uint8_t configuration) { return false; };
+
+ /*
+ * Called by USBDevice layer. Set interface/alternate of the device.
+ *
+ * @param interface Number of the interface to be configured
+ * @param alternate Number of the alternate to be configured
+ * @returns true if class handles this request
+ */
+ virtual bool USBCallback_setInterface(uint16_t interface, uint8_t alternate) { return false; };
+
+ /*
+ * Get device descriptor. Warning: this method has to store the length of the report descriptor in reportLength.
+ *
+ * @returns pointer to the device descriptor
+ */
+ virtual uint8_t * deviceDesc();
+
+ /*
+ * Get configuration descriptor
+ *
+ * @returns pointer to the configuration descriptor
+ */
+ virtual uint8_t * configurationDesc(){return NULL;};
+
+ /*
+ * Get string lang id descriptor
+ *
+ * @return pointer to the string lang id descriptor
+ */
+ virtual uint8_t * stringLangidDesc();
+
+ /*
+ * Get string manufacturer descriptor
+ *
+ * @returns pointer to the string manufacturer descriptor
+ */
+ virtual uint8_t * stringImanufacturerDesc();
+
+ /*
+ * Get string product descriptor
+ *
+ * @returns pointer to the string product descriptor
+ */
+ virtual uint8_t * stringIproductDesc();
+
+ /*
+ * Get string serial descriptor
+ *
+ * @returns pointer to the string serial descriptor
+ */
+ virtual uint8_t * stringIserialDesc();
+
+ /*
+ * Get string configuration descriptor
+ *
+ * @returns pointer to the string configuration descriptor
+ */
+ virtual uint8_t * stringIConfigurationDesc();
+
+ /*
+ * Get string interface descriptor
+ *
+ * @returns pointer to the string interface descriptor
+ */
+ virtual uint8_t * stringIinterfaceDesc();
+
+ /*
+ * Get the length of the report descriptor
+ *
+ * @returns length of the report descriptor
+ */
+ virtual uint16_t reportDescLength() { return 0; };
+
+
+
+protected:
+ virtual void busReset(void);
+ virtual void EP0setupCallback(void);
+ virtual void EP0out(void);
+ virtual void EP0in(void);
+ virtual void connectStateChanged(unsigned int connected);
+ virtual void suspendStateChanged(unsigned int suspended);
+ uint8_t * findDescriptor(uint8_t descriptorType);
+ CONTROL_TRANSFER * getTransferPtr(void);
+
+ uint16_t VENDOR_ID;
+ uint16_t PRODUCT_ID;
+ uint16_t PRODUCT_RELEASE;
+
+private:
+ bool addRateFeedbackEndpoint(uint8_t endpoint, uint32_t maxPacket);
+ bool requestGetDescriptor(void);
+ bool controlOut(void);
+ bool controlIn(void);
+ bool requestSetAddress(void);
+ bool requestSetConfiguration(void);
+ bool requestSetFeature(void);
+ bool requestClearFeature(void);
+ bool requestGetStatus(void);
+ bool requestSetup(void);
+ bool controlSetup(void);
+ void decodeSetupPacket(uint8_t *data, SETUP_PACKET *packet);
+ bool requestGetConfiguration(void);
+ bool requestGetInterface(void);
+ bool requestSetInterface(void);
+
+ CONTROL_TRANSFER transfer;
+ USB_DEVICE device;
+
+ uint16_t currentInterface;
+ uint8_t currentAlternate;
+};
+
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBDevice_Types.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBDevice_Types.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,83 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef USBDEVICE_TYPES_H
+#define USBDEVICE_TYPES_H
+
+/* Standard requests */
+#define GET_STATUS (0)
+#define CLEAR_FEATURE (1)
+#define SET_FEATURE (3)
+#define SET_ADDRESS (5)
+#define GET_DESCRIPTOR (6)
+#define SET_DESCRIPTOR (7)
+#define GET_CONFIGURATION (8)
+#define SET_CONFIGURATION (9)
+#define GET_INTERFACE (10)
+#define SET_INTERFACE (11)
+
+/* bmRequestType.dataTransferDirection */
+#define HOST_TO_DEVICE (0)
+#define DEVICE_TO_HOST (1)
+
+/* bmRequestType.Type*/
+#define STANDARD_TYPE (0)
+#define CLASS_TYPE (1)
+#define VENDOR_TYPE (2)
+#define RESERVED_TYPE (3)
+
+/* bmRequestType.Recipient */
+#define DEVICE_RECIPIENT (0)
+#define INTERFACE_RECIPIENT (1)
+#define ENDPOINT_RECIPIENT (2)
+#define OTHER_RECIPIENT (3)
+
+/* Descriptors */
+#define DESCRIPTOR_TYPE(wValue) (wValue >> 8)
+#define DESCRIPTOR_INDEX(wValue) (wValue & 0xf)
+
+typedef struct {
+ struct {
+ uint8_t dataTransferDirection;
+ uint8_t Type;
+ uint8_t Recipient;
+ } bmRequestType;
+ uint8_t bRequest;
+ uint16_t wValue;
+ uint16_t wIndex;
+ uint16_t wLength;
+} SETUP_PACKET;
+
+typedef struct {
+ SETUP_PACKET setup;
+ uint8_t *ptr;
+ uint32_t remaining;
+ uint8_t direction;
+ bool zlp;
+ bool notify;
+} CONTROL_TRANSFER;
+
+typedef enum {ATTACHED, POWERED, DEFAULT, ADDRESS, CONFIGURED} DEVICE_STATE;
+
+typedef struct {
+ volatile DEVICE_STATE state;
+ uint8_t configuration;
+ bool suspended;
+} USB_DEVICE;
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBEndpoints.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBEndpoints.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,50 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef USBENDPOINTS_H
+#define USBENDPOINTS_H
+
+/* SETUP packet size */
+#define SETUP_PACKET_SIZE (8)
+
+/* Options flags for configuring endpoints */
+#define DEFAULT_OPTIONS (0)
+#define SINGLE_BUFFERED (1U << 0)
+#define ISOCHRONOUS (1U << 1)
+#define RATE_FEEDBACK_MODE (1U << 2) /* Interrupt endpoints only */
+
+/* Endpoint transfer status, for endpoints > 0 */
+typedef enum {
+ EP_COMPLETED, /* Transfer completed */
+ EP_PENDING, /* Transfer in progress */
+ EP_INVALID, /* Invalid parameter */
+ EP_STALLED, /* Endpoint stalled */
+} EP_STATUS;
+
+/* Include configuration for specific target */
+#if defined(TARGET_LPC1768) || defined(TARGET_LPC2368) || defined(TARGET_LPC4088)
+#include "USBEndpoints_LPC17_LPC23.h"
+#elif defined(TARGET_LPC11U24)
+#include "USBEndpoints_LPC11U.h"
+#elif defined(TARGET_KL25Z)
+#include "USBEndpoints_KL25Z.h"
+#else
+#error "Unknown target type"
+#endif
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBEndpoints_KL25Z.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice/USBEndpoints_KL25Z.h Sat Aug 03 13:16:14 2013 +0000 @@ -0,0 +1,93 @@ +/* Copyright (c) 2010-2011 mbed.org, MIT License +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software +* and associated documentation files (the "Software"), to deal in the Software without +* restriction, including without limitation the rights to use, copy, modify, merge, publish, +* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the +* Software is furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all copies or +* substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING +* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#define NUMBER_OF_LOGICAL_ENDPOINTS (16) +#define NUMBER_OF_PHYSICAL_ENDPOINTS (NUMBER_OF_LOGICAL_ENDPOINTS * 2) + +/* Define physical endpoint numbers */ + +/* Endpoint No. */ +/* ---------------- */ +#define EP0OUT (0) +#define EP0IN (1) +#define EP1OUT (2) +#define EP1IN (3) +#define EP2OUT (4) +#define EP2IN (5) +#define EP3OUT (6) +#define EP3IN (7) +#define EP4OUT (8) +#define EP4IN (9) +#define EP5OUT (10) +#define EP5IN (11) +#define EP6OUT (12) +#define EP6IN (13) +#define EP7OUT (14) +#define EP7IN (15) +#define EP8OUT (16) +#define EP8IN (17) +#define EP9OUT (18) +#define EP9IN (19) +#define EP10OUT (20) +#define EP10IN (21) +#define EP11OUT (22) +#define EP11IN (23) +#define EP12OUT (24) +#define EP12IN (25) +#define EP13OUT (26) +#define EP13IN (27) +#define EP14OUT (28) +#define EP14IN (29) +#define EP15OUT (30) +#define EP15IN (31) + +/* Maximum Packet sizes */ + +#define MAX_PACKET_SIZE_EP0 (64) +#define MAX_PACKET_SIZE_EP1 (64) +#define MAX_PACKET_SIZE_EP2 (64) +#define MAX_PACKET_SIZE_EP3 (1023) +#define MAX_PACKET_SIZE_EP4 (64) +#define MAX_PACKET_SIZE_EP5 (64) +#define MAX_PACKET_SIZE_EP6 (64) +#define MAX_PACKET_SIZE_EP7 (64) +#define MAX_PACKET_SIZE_EP8 (64) +#define MAX_PACKET_SIZE_EP9 (64) +#define MAX_PACKET_SIZE_EP10 (64) +#define MAX_PACKET_SIZE_EP11 (64) +#define MAX_PACKET_SIZE_EP12 (64) +#define MAX_PACKET_SIZE_EP13 (64) +#define MAX_PACKET_SIZE_EP14 (64) +#define MAX_PACKET_SIZE_EP15 (64) + +/* Generic endpoints - intended to be portable accross devices */ +/* and be suitable for simple USB devices. */ + +/* Bulk endpoints */ +#define EPBULK_OUT (EP2OUT) +#define EPBULK_IN (EP2IN) +/* Interrupt endpoints */ +#define EPINT_OUT (EP1OUT) +#define EPINT_IN (EP1IN) +/* Isochronous endpoints */ +#define EPISO_OUT (EP3OUT) +#define EPISO_IN (EP3IN) + +#define MAX_PACKET_SIZE_EPBULK (MAX_PACKET_SIZE_EP2) +#define MAX_PACKET_SIZE_EPINT (MAX_PACKET_SIZE_EP1) +#define MAX_PACKET_SIZE_EPISO (MAX_PACKET_SIZE_EP3)
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBEndpoints_LPC11U.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice/USBEndpoints_LPC11U.h Sat Aug 03 13:16:14 2013 +0000 @@ -0,0 +1,65 @@ +/* Copyright (c) 2010-2011 mbed.org, MIT License +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software +* and associated documentation files (the "Software"), to deal in the Software without +* restriction, including without limitation the rights to use, copy, modify, merge, publish, +* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the +* Software is furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all copies or +* substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING +* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#define NUMBER_OF_LOGICAL_ENDPOINTS (5) +#define NUMBER_OF_PHYSICAL_ENDPOINTS (NUMBER_OF_LOGICAL_ENDPOINTS * 2) + +/* Define physical endpoint numbers */ + +/* Endpoint No. Type(s) MaxPacket DoubleBuffer */ +/* ---------------- ------------ ---------- --- */ +#define EP0OUT (0) /* Control 64 No */ +#define EP0IN (1) /* Control 64 No */ +#define EP1OUT (2) /* Int/Bulk/Iso 64/64/1023 Yes */ +#define EP1IN (3) /* Int/Bulk/Iso 64/64/1023 Yes */ +#define EP2OUT (4) /* Int/Bulk/Iso 64/64/1023 Yes */ +#define EP2IN (5) /* Int/Bulk/Iso 64/64/1023 Yes */ +#define EP3OUT (6) /* Int/Bulk/Iso 64/64/1023 Yes */ +#define EP3IN (7) /* Int/Bulk/Iso 64/64/1023 Yes */ +#define EP4OUT (8) /* Int/Bulk/Iso 64/64/1023 Yes */ +#define EP4IN (9) /* Int/Bulk/Iso 64/64/1023 Yes */ + +/* Maximum Packet sizes */ + +#define MAX_PACKET_SIZE_EP0 (64) +#define MAX_PACKET_SIZE_EP1 (64) /* Int/Bulk */ +#define MAX_PACKET_SIZE_EP2 (64) /* Int/Bulk */ +#define MAX_PACKET_SIZE_EP3 (64) /* Int/Bulk */ +#define MAX_PACKET_SIZE_EP4 (64) /* Int/Bulk */ + +#define MAX_PACKET_SIZE_EP1_ISO (1023) /* Isochronous */ +#define MAX_PACKET_SIZE_EP2_ISO (1023) /* Isochronous */ +#define MAX_PACKET_SIZE_EP3_ISO (1023) /* Isochronous */ +#define MAX_PACKET_SIZE_EP4_ISO (1023) /* Isochronous */ + +/* Generic endpoints - intended to be portable accross devices */ +/* and be suitable for simple USB devices. */ + +/* Bulk endpoint */ +#define EPBULK_OUT (EP2OUT) +#define EPBULK_IN (EP2IN) +/* Interrupt endpoint */ +#define EPINT_OUT (EP1OUT) +#define EPINT_IN (EP1IN) +/* Isochronous endpoint */ +#define EPISO_OUT (EP3OUT) +#define EPISO_IN (EP3IN) + +#define MAX_PACKET_SIZE_EPBULK (MAX_PACKET_SIZE_EP2) +#define MAX_PACKET_SIZE_EPINT (MAX_PACKET_SIZE_EP1) +#define MAX_PACKET_SIZE_EPISO (MAX_PACKET_SIZE_EP3_ISO)
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBEndpoints_LPC17_LPC23.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice/USBEndpoints_LPC17_LPC23.h Sat Aug 03 13:16:14 2013 +0000 @@ -0,0 +1,93 @@ +/* Copyright (c) 2010-2011 mbed.org, MIT License +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software +* and associated documentation files (the "Software"), to deal in the Software without +* restriction, including without limitation the rights to use, copy, modify, merge, publish, +* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the +* Software is furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all copies or +* substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING +* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#define NUMBER_OF_LOGICAL_ENDPOINTS (16) +#define NUMBER_OF_PHYSICAL_ENDPOINTS (NUMBER_OF_LOGICAL_ENDPOINTS * 2) + +/* Define physical endpoint numbers */ + +/* Endpoint No. Type(s) MaxPacket DoubleBuffer */ +/* ---------------- ------------ ---------- --- */ +#define EP0OUT (0) /* Control 64 No */ +#define EP0IN (1) /* Control 64 No */ +#define EP1OUT (2) /* Interrupt 64 No */ +#define EP1IN (3) /* Interrupt 64 No */ +#define EP2OUT (4) /* Bulk 64 Yes */ +#define EP2IN (5) /* Bulk 64 Yes */ +#define EP3OUT (6) /* Isochronous 1023 Yes */ +#define EP3IN (7) /* Isochronous 1023 Yes */ +#define EP4OUT (8) /* Interrupt 64 No */ +#define EP4IN (9) /* Interrupt 64 No */ +#define EP5OUT (10) /* Bulk 64 Yes */ +#define EP5IN (11) /* Bulk 64 Yes */ +#define EP6OUT (12) /* Isochronous 1023 Yes */ +#define EP6IN (13) /* Isochronous 1023 Yes */ +#define EP7OUT (14) /* Interrupt 64 No */ +#define EP7IN (15) /* Interrupt 64 No */ +#define EP8OUT (16) /* Bulk 64 Yes */ +#define EP8IN (17) /* Bulk 64 Yes */ +#define EP9OUT (18) /* Isochronous 1023 Yes */ +#define EP9IN (19) /* Isochronous 1023 Yes */ +#define EP10OUT (20) /* Interrupt 64 No */ +#define EP10IN (21) /* Interrupt 64 No */ +#define EP11OUT (22) /* Bulk 64 Yes */ +#define EP11IN (23) /* Bulk 64 Yes */ +#define EP12OUT (24) /* Isochronous 1023 Yes */ +#define EP12IN (25) /* Isochronous 1023 Yes */ +#define EP13OUT (26) /* Interrupt 64 No */ +#define EP13IN (27) /* Interrupt 64 No */ +#define EP14OUT (28) /* Bulk 64 Yes */ +#define EP14IN (29) /* Bulk 64 Yes */ +#define EP15OUT (30) /* Bulk 64 Yes */ +#define EP15IN (31) /* Bulk 64 Yes */ + +/* Maximum Packet sizes */ + +#define MAX_PACKET_SIZE_EP0 (64) +#define MAX_PACKET_SIZE_EP1 (64) +#define MAX_PACKET_SIZE_EP2 (64) +#define MAX_PACKET_SIZE_EP3 (1023) +#define MAX_PACKET_SIZE_EP4 (64) +#define MAX_PACKET_SIZE_EP5 (64) +#define MAX_PACKET_SIZE_EP6 (1023) +#define MAX_PACKET_SIZE_EP7 (64) +#define MAX_PACKET_SIZE_EP8 (64) +#define MAX_PACKET_SIZE_EP9 (1023) +#define MAX_PACKET_SIZE_EP10 (64) +#define MAX_PACKET_SIZE_EP11 (64) +#define MAX_PACKET_SIZE_EP12 (1023) +#define MAX_PACKET_SIZE_EP13 (64) +#define MAX_PACKET_SIZE_EP14 (64) +#define MAX_PACKET_SIZE_EP15 (64) + +/* Generic endpoints - intended to be portable accross devices */ +/* and be suitable for simple USB devices. */ + +/* Bulk endpoints */ +#define EPBULK_OUT (EP2OUT) +#define EPBULK_IN (EP2IN) +/* Interrupt endpoints */ +#define EPINT_OUT (EP1OUT) +#define EPINT_IN (EP1IN) +/* Isochronous endpoints */ +#define EPISO_OUT (EP3OUT) +#define EPISO_IN (EP3IN) + +#define MAX_PACKET_SIZE_EPBULK (MAX_PACKET_SIZE_EP2) +#define MAX_PACKET_SIZE_EPINT (MAX_PACKET_SIZE_EP1) +#define MAX_PACKET_SIZE_EPISO (MAX_PACKET_SIZE_EP3)
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBHAL.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBHAL.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,117 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef USBBUSINTERFACE_H
+#define USBBUSINTERFACE_H
+
+#include "mbed.h"
+#include "USBEndpoints.h"
+
+#ifdef __GNUC__
+#define __packed __attribute__ ((__packed__))
+#endif
+
+class USBHAL {
+public:
+ /* Configuration */
+ USBHAL();
+ ~USBHAL();
+ void connect(void);
+ void disconnect(void);
+ void configureDevice(void);
+ void unconfigureDevice(void);
+ void setAddress(uint8_t address);
+ void remoteWakeup(void);
+
+ /* Endpoint 0 */
+ void EP0setup(uint8_t *buffer);
+ void EP0read(void);
+ void EP0readStage(void);
+ uint32_t EP0getReadResult(uint8_t *buffer);
+ void EP0write(uint8_t *buffer, uint32_t size);
+ void EP0getWriteResult(void);
+ void EP0stall(void);
+
+ /* Other endpoints */
+ EP_STATUS endpointRead(uint8_t endpoint, uint32_t maximumSize);
+ EP_STATUS endpointReadResult(uint8_t endpoint, uint8_t *data, uint32_t *bytesRead);
+ EP_STATUS endpointWrite(uint8_t endpoint, uint8_t *data, uint32_t size);
+ EP_STATUS endpointWriteResult(uint8_t endpoint);
+ void stallEndpoint(uint8_t endpoint);
+ void unstallEndpoint(uint8_t endpoint);
+ bool realiseEndpoint(uint8_t endpoint, uint32_t maxPacket, uint32_t options);
+ bool getEndpointStallState(unsigned char endpoint);
+ uint32_t endpointReadcore(uint8_t endpoint, uint8_t *buffer);
+
+protected:
+ virtual void busReset(void){};
+ virtual void EP0setupCallback(void){};
+ virtual void EP0out(void){};
+ virtual void EP0in(void){};
+ virtual void connectStateChanged(unsigned int connected){};
+ virtual void suspendStateChanged(unsigned int suspended){};
+ virtual void SOF(int frameNumber){};
+
+ virtual bool EP1_OUT_callback(){return false;};
+ virtual bool EP1_IN_callback(){return false;};
+ virtual bool EP2_OUT_callback(){return false;};
+ virtual bool EP2_IN_callback(){return false;};
+ virtual bool EP3_OUT_callback(){return false;};
+ virtual bool EP3_IN_callback(){return false;};
+ virtual bool EP4_OUT_callback(){return false;};
+ virtual bool EP4_IN_callback(){return false;};
+
+#if !defined(TARGET_LPC11U24)
+ virtual bool EP5_OUT_callback(){return false;};
+ virtual bool EP5_IN_callback(){return false;};
+ virtual bool EP6_OUT_callback(){return false;};
+ virtual bool EP6_IN_callback(){return false;};
+ virtual bool EP7_OUT_callback(){return false;};
+ virtual bool EP7_IN_callback(){return false;};
+ virtual bool EP8_OUT_callback(){return false;};
+ virtual bool EP8_IN_callback(){return false;};
+ virtual bool EP9_OUT_callback(){return false;};
+ virtual bool EP9_IN_callback(){return false;};
+ virtual bool EP10_OUT_callback(){return false;};
+ virtual bool EP10_IN_callback(){return false;};
+ virtual bool EP11_OUT_callback(){return false;};
+ virtual bool EP11_IN_callback(){return false;};
+ virtual bool EP12_OUT_callback(){return false;};
+ virtual bool EP12_IN_callback(){return false;};
+ virtual bool EP13_OUT_callback(){return false;};
+ virtual bool EP13_IN_callback(){return false;};
+ virtual bool EP14_OUT_callback(){return false;};
+ virtual bool EP14_IN_callback(){return false;};
+ virtual bool EP15_OUT_callback(){return false;};
+ virtual bool EP15_IN_callback(){return false;};
+#endif
+
+private:
+ void usbisr(void);
+ static void _usbisr(void);
+ static USBHAL * instance;
+
+#if defined(TARGET_LPC11U24)
+ bool (USBHAL::*epCallback[10 - 2])(void);
+#else
+ bool (USBHAL::*epCallback[32 - 2])(void);
+#endif
+
+
+};
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBHAL_KL25Z.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBHAL_KL25Z.cpp Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,520 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#if defined(TARGET_KL25Z)
+
+#include "USBHAL.h"
+
+USBHAL * USBHAL::instance;
+
+static volatile int epComplete = 0;
+
+// Convert physical endpoint number to register bit
+#define EP(endpoint) (1<<(endpoint))
+
+// Convert physical to logical
+#define PHY_TO_LOG(endpoint) ((endpoint)>>1)
+
+// Get endpoint direction
+#define IN_EP(endpoint) ((endpoint) & 1U ? true : false)
+#define OUT_EP(endpoint) ((endpoint) & 1U ? false : true)
+
+#define BD_OWN_MASK (1<<7)
+#define BD_DATA01_MASK (1<<6)
+#define BD_KEEP_MASK (1<<5)
+#define BD_NINC_MASK (1<<4)
+#define BD_DTS_MASK (1<<3)
+#define BD_STALL_MASK (1<<2)
+
+#define TX 1
+#define RX 0
+#define ODD 0
+#define EVEN 1
+// this macro waits a physical endpoint number
+#define EP_BDT_IDX(ep, dir, odd) (((ep * 4) + (2 * dir) + (1 * odd)))
+
+#define SETUP_TOKEN 0x0D
+#define IN_TOKEN 0x09
+#define OUT_TOKEN 0x01
+#define TOK_PID(idx) ((bdt[idx].info >> 2) & 0x0F)
+
+// for each endpt: 8 bytes
+typedef struct BDT {
+ uint8_t info; // BD[0:7]
+ uint8_t dummy; // RSVD: BD[8:15]
+ uint16_t byte_count; // BD[16:32]
+ uint32_t address; // Addr
+} BDT;
+
+
+// there are:
+// * 16 bidirectionnal endpt -> 32 physical endpt
+// * as there are ODD and EVEN buffer -> 32*2 bdt
+__attribute__((__aligned__(512))) BDT bdt[NUMBER_OF_PHYSICAL_ENDPOINTS * 2];
+uint8_t * endpoint_buffer[(NUMBER_OF_PHYSICAL_ENDPOINTS - 2) * 2];
+uint8_t * endpoint_buffer_iso[2*2];
+
+static uint8_t set_addr = 0;
+static uint8_t addr = 0;
+
+static uint32_t Data1 = 0x55555555;
+
+static uint32_t frameNumber() {
+ return((USB0->FRMNUML | (USB0->FRMNUMH << 8) & 0x07FF));
+}
+
+uint32_t USBHAL::endpointReadcore(uint8_t endpoint, uint8_t *buffer) {
+ return 0;
+}
+
+USBHAL::USBHAL(void) {
+ // Disable IRQ
+ NVIC_DisableIRQ(USB0_IRQn);
+
+ // fill in callback array
+ epCallback[0] = &USBHAL::EP1_OUT_callback;
+ epCallback[1] = &USBHAL::EP1_IN_callback;
+ epCallback[2] = &USBHAL::EP2_OUT_callback;
+ epCallback[3] = &USBHAL::EP2_IN_callback;
+ epCallback[4] = &USBHAL::EP3_OUT_callback;
+ epCallback[5] = &USBHAL::EP3_IN_callback;
+ epCallback[6] = &USBHAL::EP4_OUT_callback;
+ epCallback[7] = &USBHAL::EP4_IN_callback;
+ epCallback[8] = &USBHAL::EP5_OUT_callback;
+ epCallback[9] = &USBHAL::EP5_IN_callback;
+ epCallback[10] = &USBHAL::EP6_OUT_callback;
+ epCallback[11] = &USBHAL::EP6_IN_callback;
+ epCallback[12] = &USBHAL::EP7_OUT_callback;
+ epCallback[13] = &USBHAL::EP7_IN_callback;
+ epCallback[14] = &USBHAL::EP8_OUT_callback;
+ epCallback[15] = &USBHAL::EP8_IN_callback;
+ epCallback[16] = &USBHAL::EP9_OUT_callback;
+ epCallback[17] = &USBHAL::EP9_IN_callback;
+ epCallback[18] = &USBHAL::EP10_OUT_callback;
+ epCallback[19] = &USBHAL::EP10_IN_callback;
+ epCallback[20] = &USBHAL::EP11_OUT_callback;
+ epCallback[21] = &USBHAL::EP11_IN_callback;
+ epCallback[22] = &USBHAL::EP12_OUT_callback;
+ epCallback[23] = &USBHAL::EP12_IN_callback;
+ epCallback[24] = &USBHAL::EP13_OUT_callback;
+ epCallback[25] = &USBHAL::EP13_IN_callback;
+ epCallback[26] = &USBHAL::EP14_OUT_callback;
+ epCallback[27] = &USBHAL::EP14_IN_callback;
+ epCallback[28] = &USBHAL::EP15_OUT_callback;
+ epCallback[29] = &USBHAL::EP15_IN_callback;
+
+
+ // choose usb src as PLL
+ SIM->SOPT2 |= (SIM_SOPT2_USBSRC_MASK | SIM_SOPT2_PLLFLLSEL_MASK);
+
+ // enable OTG clock
+ SIM->SCGC4 |= SIM_SCGC4_USBOTG_MASK;
+
+ // Attach IRQ
+ instance = this;
+ NVIC_SetVector(USB0_IRQn, (uint32_t)&_usbisr);
+ NVIC_EnableIRQ(USB0_IRQn);
+
+ // USB Module Configuration
+ // Reset USB Module
+ USB0->USBTRC0 |= USB_USBTRC0_USBRESET_MASK;
+ while(USB0->USBTRC0 & USB_USBTRC0_USBRESET_MASK);
+
+ // Set BDT Base Register
+ USB0->BDTPAGE1=(uint8_t)((uint32_t)bdt>>8);
+ USB0->BDTPAGE2=(uint8_t)((uint32_t)bdt>>16);
+ USB0->BDTPAGE3=(uint8_t)((uint32_t)bdt>>24);
+
+ // Clear interrupt flag
+ USB0->ISTAT = 0xff;
+
+ // USB Interrupt Enablers
+ USB0->INTEN |= USB_INTEN_TOKDNEEN_MASK |
+ USB_INTEN_SOFTOKEN_MASK |
+ USB_INTEN_ERROREN_MASK |
+ USB_INTEN_USBRSTEN_MASK;
+
+ // Disable weak pull downs
+ USB0->USBCTRL &= ~(USB_USBCTRL_PDE_MASK | USB_USBCTRL_SUSP_MASK);
+
+ USB0->USBTRC0 |= 0x40;
+}
+
+USBHAL::~USBHAL(void) { }
+
+void USBHAL::connect(void) {
+ // enable USB
+ USB0->CTL |= USB_CTL_USBENSOFEN_MASK;
+ // Pull up enable
+ USB0->CONTROL |= USB_CONTROL_DPPULLUPNONOTG_MASK;
+}
+
+void USBHAL::disconnect(void) {
+ // disable USB
+ USB0->CTL &= ~USB_CTL_USBENSOFEN_MASK;
+ // Pull up disable
+ USB0->CONTROL &= ~USB_CONTROL_DPPULLUPNONOTG_MASK;
+}
+
+void USBHAL::configureDevice(void) {
+ // not needed
+}
+
+void USBHAL::unconfigureDevice(void) {
+ // not needed
+}
+
+void USBHAL::setAddress(uint8_t address) {
+ // we don't set the address now otherwise the usb controller does not ack
+ // we set a flag instead
+ // see usbisr when an IN token is received
+ set_addr = 1;
+ addr = address;
+}
+
+bool USBHAL::realiseEndpoint(uint8_t endpoint, uint32_t maxPacket, uint32_t flags) {
+ uint32_t handshake_flag = 0;
+ uint8_t * buf;
+
+ if (endpoint > NUMBER_OF_PHYSICAL_ENDPOINTS - 1) {
+ return false;
+ }
+
+ uint32_t log_endpoint = PHY_TO_LOG(endpoint);
+
+ if ((flags & ISOCHRONOUS) == 0) {
+ handshake_flag = USB_ENDPT_EPHSHK_MASK;
+ if (IN_EP(endpoint)) {
+ endpoint_buffer[EP_BDT_IDX(log_endpoint, TX, ODD )] = (uint8_t *) malloc (64*2);
+ buf = &endpoint_buffer[EP_BDT_IDX(log_endpoint, TX, ODD )][0];
+ } else {
+ endpoint_buffer[EP_BDT_IDX(log_endpoint, RX, ODD )] = (uint8_t *) malloc (64*2);
+ buf = &endpoint_buffer[EP_BDT_IDX(log_endpoint, RX, ODD )][0];
+ }
+ } else {
+ if (IN_EP(endpoint)) {
+ endpoint_buffer_iso[2] = (uint8_t *) malloc (1023*2);
+ buf = &endpoint_buffer_iso[2][0];
+ } else {
+ endpoint_buffer_iso[0] = (uint8_t *) malloc (1023*2);
+ buf = &endpoint_buffer_iso[0][0];
+ }
+ }
+
+ // IN endpt -> device to host (TX)
+ if (IN_EP(endpoint)) {
+ USB0->ENDPOINT[log_endpoint].ENDPT |= handshake_flag | // ep handshaking (not if iso endpoint)
+ USB_ENDPT_EPTXEN_MASK; // en TX (IN) tran
+ bdt[EP_BDT_IDX(log_endpoint, TX, ODD )].address = (uint32_t) buf;
+ bdt[EP_BDT_IDX(log_endpoint, TX, EVEN)].address = 0;
+ }
+ // OUT endpt -> host to device (RX)
+ else {
+ USB0->ENDPOINT[log_endpoint].ENDPT |= handshake_flag | // ep handshaking (not if iso endpoint)
+ USB_ENDPT_EPRXEN_MASK; // en RX (OUT) tran.
+ bdt[EP_BDT_IDX(log_endpoint, RX, ODD )].byte_count = maxPacket;
+ bdt[EP_BDT_IDX(log_endpoint, RX, ODD )].address = (uint32_t) buf;
+ bdt[EP_BDT_IDX(log_endpoint, RX, ODD )].info = BD_OWN_MASK | BD_DTS_MASK;
+ bdt[EP_BDT_IDX(log_endpoint, RX, EVEN)].info = 0;
+ }
+
+ Data1 |= (1 << endpoint);
+
+ return true;
+}
+
+// read setup packet
+void USBHAL::EP0setup(uint8_t *buffer) {
+ uint32_t sz;
+ endpointReadResult(EP0OUT, buffer, &sz);
+}
+
+void USBHAL::EP0readStage(void) {
+ Data1 &= ~1UL; // set DATA0
+ bdt[0].info = (BD_DTS_MASK | BD_OWN_MASK);
+}
+
+void USBHAL::EP0read(void) {
+ uint32_t idx = EP_BDT_IDX(PHY_TO_LOG(EP0OUT), RX, 0);
+ bdt[idx].byte_count = MAX_PACKET_SIZE_EP0;
+}
+
+uint32_t USBHAL::EP0getReadResult(uint8_t *buffer) {
+ uint32_t sz;
+ endpointReadResult(EP0OUT, buffer, &sz);
+ return sz;
+}
+
+void USBHAL::EP0write(uint8_t *buffer, uint32_t size) {
+ endpointWrite(EP0IN, buffer, size);
+}
+
+void USBHAL::EP0getWriteResult(void) {
+}
+
+void USBHAL::EP0stall(void) {
+ stallEndpoint(EP0OUT);
+}
+
+EP_STATUS USBHAL::endpointRead(uint8_t endpoint, uint32_t maximumSize) {
+ endpoint = PHY_TO_LOG(endpoint);
+ uint32_t idx = EP_BDT_IDX(endpoint, RX, 0);
+ bdt[idx].byte_count = maximumSize;
+ return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointReadResult(uint8_t endpoint, uint8_t * buffer, uint32_t *bytesRead) {
+ uint32_t n, sz, idx, setup = 0;
+ uint8_t not_iso;
+ uint8_t * ep_buf;
+
+ uint32_t log_endpoint = PHY_TO_LOG(endpoint);
+
+ if (endpoint > NUMBER_OF_PHYSICAL_ENDPOINTS - 1) {
+ return EP_INVALID;
+ }
+
+ // if read on a IN endpoint -> error
+ if (IN_EP(endpoint)) {
+ return EP_INVALID;
+ }
+
+ idx = EP_BDT_IDX(log_endpoint, RX, 0);
+ sz = bdt[idx].byte_count;
+ not_iso = USB0->ENDPOINT[log_endpoint].ENDPT & USB_ENDPT_EPHSHK_MASK;
+
+ //for isochronous endpoint, we don't wait an interrupt
+ if ((log_endpoint != 0) && not_iso && !(epComplete & EP(endpoint))) {
+ return EP_PENDING;
+ }
+
+ if ((log_endpoint == 0) && (TOK_PID(idx) == SETUP_TOKEN)) {
+ setup = 1;
+ }
+
+ // non iso endpoint
+ if (not_iso) {
+ ep_buf = endpoint_buffer[idx];
+ } else {
+ ep_buf = endpoint_buffer_iso[0];
+ }
+
+ for (n = 0; n < sz; n++) {
+ buffer[n] = ep_buf[n];
+ }
+
+ if (((Data1 >> endpoint) & 1) == ((bdt[idx].info >> 6) & 1)) {
+ if (setup && (buffer[6] == 0)) // if no setup data stage,
+ Data1 &= ~1UL; // set DATA0
+ else
+ Data1 ^= (1 << endpoint);
+ }
+
+ if (((Data1 >> endpoint) & 1)) {
+ bdt[idx].info = BD_DTS_MASK | BD_DATA01_MASK | BD_OWN_MASK;
+ }
+ else {
+ bdt[idx].info = BD_DTS_MASK | BD_OWN_MASK;
+ }
+
+ USB0->CTL &= ~USB_CTL_TXSUSPENDTOKENBUSY_MASK;
+ *bytesRead = sz;
+
+ epComplete &= ~EP(endpoint);
+ return EP_COMPLETED;
+}
+
+EP_STATUS USBHAL::endpointWrite(uint8_t endpoint, uint8_t *data, uint32_t size) {
+ uint32_t idx, n;
+ uint8_t * ep_buf;
+
+ if (endpoint > NUMBER_OF_PHYSICAL_ENDPOINTS - 1) {
+ return EP_INVALID;
+ }
+
+ // if write on a OUT endpoint -> error
+ if (OUT_EP(endpoint)) {
+ return EP_INVALID;
+ }
+
+ idx = EP_BDT_IDX(PHY_TO_LOG(endpoint), TX, 0);
+ bdt[idx].byte_count = size;
+
+
+ // non iso endpoint
+ if (USB0->ENDPOINT[PHY_TO_LOG(endpoint)].ENDPT & USB_ENDPT_EPHSHK_MASK) {
+ ep_buf = endpoint_buffer[idx];
+ } else {
+ ep_buf = endpoint_buffer_iso[2];
+ }
+
+ for (n = 0; n < size; n++) {
+ ep_buf[n] = data[n];
+ }
+
+ if ((Data1 >> endpoint) & 1) {
+ bdt[idx].info = BD_OWN_MASK | BD_DTS_MASK;
+ } else {
+ bdt[idx].info = BD_OWN_MASK | BD_DTS_MASK | BD_DATA01_MASK;
+ }
+
+ Data1 ^= (1 << endpoint);
+
+ return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointWriteResult(uint8_t endpoint) {
+ if (epComplete & EP(endpoint)) {
+ epComplete &= ~EP(endpoint);
+ return EP_COMPLETED;
+ }
+
+ return EP_PENDING;
+}
+
+void USBHAL::stallEndpoint(uint8_t endpoint) {
+ USB0->ENDPOINT[PHY_TO_LOG(endpoint)].ENDPT |= USB_ENDPT_EPSTALL_MASK;
+}
+
+void USBHAL::unstallEndpoint(uint8_t endpoint) {
+ USB0->ENDPOINT[PHY_TO_LOG(endpoint)].ENDPT &= ~USB_ENDPT_EPSTALL_MASK;
+}
+
+bool USBHAL::getEndpointStallState(uint8_t endpoint) {
+ uint8_t stall = (USB0->ENDPOINT[PHY_TO_LOG(endpoint)].ENDPT & USB_ENDPT_EPSTALL_MASK);
+ return (stall) ? true : false;
+}
+
+void USBHAL::remoteWakeup(void) {
+ // [TODO]
+}
+
+
+void USBHAL::_usbisr(void) {
+ instance->usbisr();
+}
+
+
+void USBHAL::usbisr(void) {
+ uint8_t i;
+ uint8_t istat = USB0->ISTAT;
+
+ // reset interrupt
+ if (istat & USB_ISTAT_USBRST_MASK) {
+ // disable all endpt
+ for(i = 0; i < 16; i++) {
+ USB0->ENDPOINT[i].ENDPT = 0x00;
+ }
+
+ // enable control endpoint
+ realiseEndpoint(EP0OUT, MAX_PACKET_SIZE_EP0, 0);
+ realiseEndpoint(EP0IN, MAX_PACKET_SIZE_EP0, 0);
+
+ Data1 = 0x55555555;
+ USB0->CTL |= USB_CTL_ODDRST_MASK;
+
+ USB0->ISTAT = 0xFF; // clear all interrupt status flags
+ USB0->ERRSTAT = 0xFF; // clear all error flags
+ USB0->ERREN = 0xFF; // enable error interrupt sources
+ USB0->ADDR = 0x00; // set default address
+
+ return;
+ }
+
+ // resume interrupt
+ if (istat & USB_ISTAT_RESUME_MASK) {
+ USB0->ISTAT = USB_ISTAT_RESUME_MASK;
+ }
+
+ // SOF interrupt
+ if (istat & USB_ISTAT_SOFTOK_MASK) {
+ USB0->ISTAT = USB_ISTAT_SOFTOK_MASK;
+ // SOF event, read frame number
+ SOF(frameNumber());
+ }
+
+ // stall interrupt
+ if (istat & 1<<7) {
+ if (USB0->ENDPOINT[0].ENDPT & USB_ENDPT_EPSTALL_MASK)
+ USB0->ENDPOINT[0].ENDPT &= ~USB_ENDPT_EPSTALL_MASK;
+ USB0->ISTAT |= USB_ISTAT_STALL_MASK;
+ }
+
+ // token interrupt
+ if (istat & 1<<3) {
+ uint32_t num = (USB0->STAT >> 4) & 0x0F;
+ uint32_t dir = (USB0->STAT >> 3) & 0x01;
+ uint32_t ev_odd = (USB0->STAT >> 2) & 0x01;
+
+ // setup packet
+ if ((num == 0) && (TOK_PID((EP_BDT_IDX(num, dir, ev_odd))) == SETUP_TOKEN)) {
+ Data1 &= ~0x02;
+ bdt[EP_BDT_IDX(0, TX, EVEN)].info &= ~BD_OWN_MASK;
+ bdt[EP_BDT_IDX(0, TX, ODD)].info &= ~BD_OWN_MASK;
+
+ // EP0 SETUP event (SETUP data received)
+ EP0setupCallback();
+
+ } else {
+ // OUT packet
+ if (TOK_PID((EP_BDT_IDX(num, dir, ev_odd))) == OUT_TOKEN) {
+ if (num == 0)
+ EP0out();
+ else {
+ epComplete |= (1 << EP(num));
+ if ((instance->*(epCallback[EP(num) - 2]))()) {
+ epComplete &= ~(1 << EP(num));
+ }
+ }
+ }
+
+ // IN packet
+ if (TOK_PID((EP_BDT_IDX(num, dir, ev_odd))) == IN_TOKEN) {
+ if (num == 0) {
+ EP0in();
+ if (set_addr == 1) {
+ USB0->ADDR = addr & 0x7F;
+ set_addr = 0;
+ }
+ }
+ else {
+ epComplete |= (1 << (EP(num) + 1));
+ if ((instance->*(epCallback[EP(num) + 1 - 2]))()) {
+ epComplete &= ~(1 << (EP(num) + 1));
+ }
+ }
+ }
+ }
+
+ USB0->ISTAT = USB_ISTAT_TOKDNE_MASK;
+ }
+
+ // sleep interrupt
+ if (istat & 1<<4) {
+ USB0->ISTAT |= USB_ISTAT_SLEEP_MASK;
+ }
+
+ // error interrupt
+ if (istat & USB_ISTAT_ERROR_MASK) {
+ USB0->ERRSTAT = 0xFF;
+ USB0->ISTAT |= USB_ISTAT_ERROR_MASK;
+ }
+}
+
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBHAL_LPC11U.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBHAL_LPC11U.cpp Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,684 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifdef TARGET_LPC11U24
+
+#include "USBHAL.h"
+
+USBHAL * USBHAL::instance;
+
+// Valid physical endpoint numbers are 0 to (NUMBER_OF_PHYSICAL_ENDPOINTS-1)
+#define LAST_PHYSICAL_ENDPOINT (NUMBER_OF_PHYSICAL_ENDPOINTS-1)
+
+// Convert physical endpoint number to register bit
+#define EP(endpoint) (1UL<<endpoint)
+
+// Convert physical to logical
+#define PHY_TO_LOG(endpoint) ((endpoint)>>1)
+
+// Get endpoint direction
+#define IN_EP(endpoint) ((endpoint) & 1U ? true : false)
+#define OUT_EP(endpoint) ((endpoint) & 1U ? false : true)
+
+// USB RAM
+#define USB_RAM_START (0x20004000)
+#define USB_RAM_SIZE (0x00000800)
+
+// SYSAHBCLKCTRL
+#define CLK_USB (1UL<<14)
+#define CLK_USBRAM (1UL<<27)
+
+// USB Information register
+#define FRAME_NR(a) ((a) & 0x7ff) // Frame number
+
+// USB Device Command/Status register
+#define DEV_ADDR_MASK (0x7f) // Device address
+#define DEV_ADDR(a) ((a) & DEV_ADDR_MASK)
+#define DEV_EN (1UL<<7) // Device enable
+#define SETUP (1UL<<8) // SETUP token received
+#define PLL_ON (1UL<<9) // PLL enabled in suspend
+#define DCON (1UL<<16) // Device status - connect
+#define DSUS (1UL<<17) // Device status - suspend
+#define DCON_C (1UL<<24) // Connect change
+#define DSUS_C (1UL<<25) // Suspend change
+#define DRES_C (1UL<<26) // Reset change
+#define VBUSDEBOUNCED (1UL<<28) // Vbus detected
+
+// Endpoint Command/Status list
+#define CMDSTS_A (1UL<<31) // Active
+#define CMDSTS_D (1UL<<30) // Disable
+#define CMDSTS_S (1UL<<29) // Stall
+#define CMDSTS_TR (1UL<<28) // Toggle Reset
+#define CMDSTS_RF (1UL<<27) // Rate Feedback mode
+#define CMDSTS_TV (1UL<<27) // Toggle Value
+#define CMDSTS_T (1UL<<26) // Endpoint Type
+#define CMDSTS_NBYTES(n) (((n)&0x3ff)<<16) // Number of bytes
+#define CMDSTS_ADDRESS_OFFSET(a) (((a)>>6)&0xffff) // Buffer start address
+
+#define BYTES_REMAINING(s) (((s)>>16)&0x3ff) // Bytes remaining after transfer
+
+// USB Non-endpoint interrupt sources
+#define FRAME_INT (1UL<<30)
+#define DEV_INT (1UL<<31)
+
+static volatile int epComplete = 0;
+
+// One entry for a double-buffered logical endpoint in the endpoint
+// command/status list. Endpoint 0 is single buffered, out[1] is used
+// for the SETUP packet and in[1] is not used
+typedef __packed struct {
+ uint32_t out[2];
+ uint32_t in[2];
+} EP_COMMAND_STATUS;
+
+typedef __packed struct {
+ uint8_t out[MAX_PACKET_SIZE_EP0];
+ uint8_t in[MAX_PACKET_SIZE_EP0];
+ uint8_t setup[SETUP_PACKET_SIZE];
+} CONTROL_TRANSFER;
+
+typedef __packed struct {
+ uint32_t maxPacket;
+ uint32_t buffer[2];
+ uint32_t options;
+} EP_STATE;
+
+static volatile EP_STATE endpointState[NUMBER_OF_PHYSICAL_ENDPOINTS];
+
+// Pointer to the endpoint command/status list
+static EP_COMMAND_STATUS *ep = NULL;
+
+// Pointer to endpoint 0 data (IN/OUT and SETUP)
+static CONTROL_TRANSFER *ct = NULL;
+
+// Shadow DEVCMDSTAT register to avoid accidentally clearing flags or
+// initiating a remote wakeup event.
+static volatile uint32_t devCmdStat;
+
+// Pointers used to allocate USB RAM
+static uint32_t usbRamPtr = USB_RAM_START;
+static uint32_t epRamPtr = 0; // Buffers for endpoints > 0 start here
+
+#define ROUND_UP_TO_MULTIPLE(x, m) ((((x)+((m)-1))/(m))*(m))
+
+void USBMemCopy(uint8_t *dst, uint8_t *src, uint32_t size);
+void USBMemCopy(uint8_t *dst, uint8_t *src, uint32_t size) {
+ if (size > 0) {
+ do {
+ *dst++ = *src++;
+ } while (--size > 0);
+ }
+}
+
+
+USBHAL::USBHAL(void) {
+ NVIC_DisableIRQ(USB_IRQn);
+
+ // fill in callback array
+ epCallback[0] = &USBHAL::EP1_OUT_callback;
+ epCallback[1] = &USBHAL::EP1_IN_callback;
+ epCallback[2] = &USBHAL::EP2_OUT_callback;
+ epCallback[3] = &USBHAL::EP2_IN_callback;
+ epCallback[4] = &USBHAL::EP3_OUT_callback;
+ epCallback[5] = &USBHAL::EP3_IN_callback;
+ epCallback[6] = &USBHAL::EP4_OUT_callback;
+ epCallback[7] = &USBHAL::EP4_IN_callback;
+
+ // nUSB_CONNECT output
+ LPC_IOCON->PIO0_6 = 0x00000001;
+
+ // Enable clocks (USB registers, USB RAM)
+ LPC_SYSCON->SYSAHBCLKCTRL |= CLK_USB | CLK_USBRAM;
+
+ // Ensure device disconnected (DCON not set)
+ LPC_USB->DEVCMDSTAT = 0;
+
+ // to ensure that the USB host sees the device as
+ // disconnected if the target CPU is reset.
+ wait(0.3);
+
+ // Reserve space in USB RAM for endpoint command/status list
+ // Must be 256 byte aligned
+ usbRamPtr = ROUND_UP_TO_MULTIPLE(usbRamPtr, 256);
+ ep = (EP_COMMAND_STATUS *)usbRamPtr;
+ usbRamPtr += (sizeof(EP_COMMAND_STATUS) * NUMBER_OF_LOGICAL_ENDPOINTS);
+ LPC_USB->EPLISTSTART = (uint32_t)(ep) & 0xffffff00;
+
+ // Reserve space in USB RAM for Endpoint 0
+ // Must be 64 byte aligned
+ usbRamPtr = ROUND_UP_TO_MULTIPLE(usbRamPtr, 64);
+ ct = (CONTROL_TRANSFER *)usbRamPtr;
+ usbRamPtr += sizeof(CONTROL_TRANSFER);
+ LPC_USB->DATABUFSTART =(uint32_t)(ct) & 0xffc00000;
+
+ // Setup command/status list for EP0
+ ep[0].out[0] = 0;
+ ep[0].in[0] = 0;
+ ep[0].out[1] = CMDSTS_ADDRESS_OFFSET((uint32_t)ct->setup);
+
+ // Route all interrupts to IRQ, some can be routed to
+ // USB_FIQ if you wish.
+ LPC_USB->INTROUTING = 0;
+
+ // Set device address 0, enable USB device, no remote wakeup
+ devCmdStat = DEV_ADDR(0) | DEV_EN | DSUS;
+ LPC_USB->DEVCMDSTAT = devCmdStat;
+
+ // Enable interrupts for device events and EP0
+ LPC_USB->INTEN = DEV_INT | EP(EP0IN) | EP(EP0OUT) | FRAME_INT;
+ instance = this;
+
+ //attach IRQ handler and enable interrupts
+ NVIC_SetVector(USB_IRQn, (uint32_t)&_usbisr);
+}
+
+USBHAL::~USBHAL(void) {
+ // Ensure device disconnected (DCON not set)
+ LPC_USB->DEVCMDSTAT = 0;
+ // Disable USB interrupts
+ NVIC_DisableIRQ(USB_IRQn);
+}
+
+void USBHAL::connect(void) {
+ NVIC_EnableIRQ(USB_IRQn);
+ devCmdStat |= DCON;
+ LPC_USB->DEVCMDSTAT = devCmdStat;
+}
+
+void USBHAL::disconnect(void) {
+ NVIC_DisableIRQ(USB_IRQn);
+ devCmdStat &= ~DCON;
+ LPC_USB->DEVCMDSTAT = devCmdStat;
+}
+
+void USBHAL::configureDevice(void) {
+ // Not required
+}
+
+void USBHAL::unconfigureDevice(void) {
+ // Not required
+}
+
+void USBHAL::EP0setup(uint8_t *buffer) {
+ // Copy setup packet data
+ USBMemCopy(buffer, ct->setup, SETUP_PACKET_SIZE);
+}
+
+void USBHAL::EP0read(void) {
+ // Start an endpoint 0 read
+
+ // The USB ISR will call USBDevice_EP0out() when a packet has been read,
+ // the USBDevice layer then calls USBBusInterface_EP0getReadResult() to
+ // read the data.
+
+ ep[0].out[0] = CMDSTS_A |CMDSTS_NBYTES(MAX_PACKET_SIZE_EP0) \
+ | CMDSTS_ADDRESS_OFFSET((uint32_t)ct->out);
+}
+
+uint32_t USBHAL::EP0getReadResult(uint8_t *buffer) {
+ // Complete an endpoint 0 read
+ uint32_t bytesRead;
+
+ // Find how many bytes were read
+ bytesRead = MAX_PACKET_SIZE_EP0 - BYTES_REMAINING(ep[0].out[0]);
+
+ // Copy data
+ USBMemCopy(buffer, ct->out, bytesRead);
+ return bytesRead;
+}
+
+
+void USBHAL::EP0readStage(void) {
+ // Not required
+}
+
+void USBHAL::EP0write(uint8_t *buffer, uint32_t size) {
+ // Start and endpoint 0 write
+
+ // The USB ISR will call USBDevice_EP0in() when the data has
+ // been written, the USBDevice layer then calls
+ // USBBusInterface_EP0getWriteResult() to complete the transaction.
+
+ // Copy data
+ USBMemCopy(ct->in, buffer, size);
+
+ // Start transfer
+ ep[0].in[0] = CMDSTS_A | CMDSTS_NBYTES(size) \
+ | CMDSTS_ADDRESS_OFFSET((uint32_t)ct->in);
+}
+
+
+EP_STATUS USBHAL::endpointRead(uint8_t endpoint, uint32_t maximumSize) {
+ uint8_t bf = 0;
+ uint32_t flags = 0;
+
+ //check which buffer must be filled
+ if (LPC_USB->EPBUFCFG & EP(endpoint)) {
+ // Double buffered
+ if (LPC_USB->EPINUSE & EP(endpoint)) {
+ bf = 1;
+ } else {
+ bf = 0;
+ }
+ }
+
+ // if isochronous endpoint, T = 1
+ if(endpointState[endpoint].options & ISOCHRONOUS)
+ {
+ flags |= CMDSTS_T;
+ }
+
+ //Active the endpoint for reading
+ ep[PHY_TO_LOG(endpoint)].out[bf] = CMDSTS_A | CMDSTS_NBYTES(maximumSize) \
+ | CMDSTS_ADDRESS_OFFSET((uint32_t)ct->out) | flags;
+ return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointReadResult(uint8_t endpoint, uint8_t *data, uint32_t *bytesRead) {
+
+ uint8_t bf = 0;
+
+ if (!(epComplete & EP(endpoint)))
+ return EP_PENDING;
+ else {
+ epComplete &= ~EP(endpoint);
+
+ //check which buffer has been filled
+ if (LPC_USB->EPBUFCFG & EP(endpoint)) {
+ // Double buffered (here we read the previous buffer which was used)
+ if (LPC_USB->EPINUSE & EP(endpoint)) {
+ bf = 0;
+ } else {
+ bf = 1;
+ }
+ }
+
+ // Find how many bytes were read
+ *bytesRead = (uint32_t) (endpointState[endpoint].maxPacket - BYTES_REMAINING(ep[PHY_TO_LOG(endpoint)].out[bf]));
+
+ // Copy data
+ USBMemCopy(data, ct->out, *bytesRead);
+ return EP_COMPLETED;
+ }
+}
+
+void USBHAL::EP0getWriteResult(void) {
+ // Not required
+}
+
+void USBHAL::EP0stall(void) {
+ ep[0].in[0] = CMDSTS_S;
+ ep[0].out[0] = CMDSTS_S;
+}
+
+void USBHAL::setAddress(uint8_t address) {
+ devCmdStat &= ~DEV_ADDR_MASK;
+ devCmdStat |= DEV_ADDR(address);
+ LPC_USB->DEVCMDSTAT = devCmdStat;
+}
+
+EP_STATUS USBHAL::endpointWrite(uint8_t endpoint, uint8_t *data, uint32_t size) {
+ uint32_t flags = 0;
+ uint32_t bf;
+
+ // Validate parameters
+ if (data == NULL) {
+ return EP_INVALID;
+ }
+
+ if (endpoint > LAST_PHYSICAL_ENDPOINT) {
+ return EP_INVALID;
+ }
+
+ if ((endpoint==EP0IN) || (endpoint==EP0OUT)) {
+ return EP_INVALID;
+ }
+
+ if (size > endpointState[endpoint].maxPacket) {
+ return EP_INVALID;
+ }
+
+ if (LPC_USB->EPBUFCFG & EP(endpoint)) {
+ // Double buffered
+ if (LPC_USB->EPINUSE & EP(endpoint)) {
+ bf = 1;
+ } else {
+ bf = 0;
+ }
+ } else {
+ // Single buffered
+ bf = 0;
+ }
+
+ // Check if already active
+ if (ep[PHY_TO_LOG(endpoint)].in[bf] & CMDSTS_A) {
+ return EP_INVALID;
+ }
+
+ // Check if stalled
+ if (ep[PHY_TO_LOG(endpoint)].in[bf] & CMDSTS_S) {
+ return EP_STALLED;
+ }
+
+ // Copy data to USB RAM
+ USBMemCopy((uint8_t *)endpointState[endpoint].buffer[bf], data, size);
+
+ // Add options
+ if (endpointState[endpoint].options & RATE_FEEDBACK_MODE) {
+ flags |= CMDSTS_RF;
+ }
+
+ if (endpointState[endpoint].options & ISOCHRONOUS) {
+ flags |= CMDSTS_T;
+ }
+
+ // Add transfer
+ ep[PHY_TO_LOG(endpoint)].in[bf] = CMDSTS_ADDRESS_OFFSET( \
+ endpointState[endpoint].buffer[bf]) \
+ | CMDSTS_NBYTES(size) | CMDSTS_A | flags;
+
+ return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointWriteResult(uint8_t endpoint) {
+ uint32_t bf;
+
+ // Validate parameters
+ if (endpoint > LAST_PHYSICAL_ENDPOINT) {
+ return EP_INVALID;
+ }
+
+ if (OUT_EP(endpoint)) {
+ return EP_INVALID;
+ }
+
+ if (LPC_USB->EPBUFCFG & EP(endpoint)) {
+ // Double buffered // TODO: FIX THIS
+ if (LPC_USB->EPINUSE & EP(endpoint)) {
+ bf = 1;
+ } else {
+ bf = 0;
+ }
+ } else {
+ // Single buffered
+ bf = 0;
+ }
+
+ // Check if endpoint still active
+ if (ep[PHY_TO_LOG(endpoint)].in[bf] & CMDSTS_A) {
+ return EP_PENDING;
+ }
+
+ // Check if stalled
+ if (ep[PHY_TO_LOG(endpoint)].in[bf] & CMDSTS_S) {
+ return EP_STALLED;
+ }
+
+ return EP_COMPLETED;
+}
+
+void USBHAL::stallEndpoint(uint8_t endpoint) {
+
+ // FIX: should this clear active bit?
+ if (IN_EP(endpoint)) {
+ ep[PHY_TO_LOG(endpoint)].in[0] |= CMDSTS_S;
+ ep[PHY_TO_LOG(endpoint)].in[1] |= CMDSTS_S;
+ } else {
+ ep[PHY_TO_LOG(endpoint)].out[0] |= CMDSTS_S;
+ ep[PHY_TO_LOG(endpoint)].out[1] |= CMDSTS_S;
+ }
+}
+
+void USBHAL::unstallEndpoint(uint8_t endpoint) {
+ if (LPC_USB->EPBUFCFG & EP(endpoint)) {
+ // Double buffered
+ if (IN_EP(endpoint)) {
+ ep[PHY_TO_LOG(endpoint)].in[0] = 0; // S = 0
+ ep[PHY_TO_LOG(endpoint)].in[1] = 0; // S = 0
+
+ if (LPC_USB->EPINUSE & EP(endpoint)) {
+ ep[PHY_TO_LOG(endpoint)].in[1] = CMDSTS_TR; // S = 0, TR = 1, TV = 0
+ } else {
+ ep[PHY_TO_LOG(endpoint)].in[0] = CMDSTS_TR; // S = 0, TR = 1, TV = 0
+ }
+ } else {
+ ep[PHY_TO_LOG(endpoint)].out[0] = 0; // S = 0
+ ep[PHY_TO_LOG(endpoint)].out[1] = 0; // S = 0
+
+ if (LPC_USB->EPINUSE & EP(endpoint)) {
+ ep[PHY_TO_LOG(endpoint)].out[1] = CMDSTS_TR; // S = 0, TR = 1, TV = 0
+ } else {
+ ep[PHY_TO_LOG(endpoint)].out[0] = CMDSTS_TR; // S = 0, TR = 1, TV = 0
+ }
+ }
+ } else {
+ // Single buffered
+ if (IN_EP(endpoint)) {
+ ep[PHY_TO_LOG(endpoint)].in[0] = CMDSTS_TR; // S = 0, TR = 1, TV = 0
+ } else {
+ ep[PHY_TO_LOG(endpoint)].out[0] = CMDSTS_TR; // S = 0, TR = 1, TV = 0
+ }
+ }
+}
+
+bool USBHAL::getEndpointStallState(unsigned char endpoint) {
+ if (IN_EP(endpoint)) {
+ if (LPC_USB->EPINUSE & EP(endpoint)) {
+ if (ep[PHY_TO_LOG(endpoint)].in[1] & CMDSTS_S) {
+ return true;
+ }
+ } else {
+ if (ep[PHY_TO_LOG(endpoint)].in[0] & CMDSTS_S) {
+ return true;
+ }
+ }
+ } else {
+ if (LPC_USB->EPINUSE & EP(endpoint)) {
+ if (ep[PHY_TO_LOG(endpoint)].out[1] & CMDSTS_S) {
+ return true;
+ }
+ } else {
+ if (ep[PHY_TO_LOG(endpoint)].out[0] & CMDSTS_S) {
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+bool USBHAL::realiseEndpoint(uint8_t endpoint, uint32_t maxPacket, uint32_t options) {
+ uint32_t tmpEpRamPtr;
+
+ if (endpoint > LAST_PHYSICAL_ENDPOINT) {
+ return false;
+ }
+
+ // Not applicable to the control endpoints
+ if ((endpoint==EP0IN) || (endpoint==EP0OUT)) {
+ return false;
+ }
+
+ // Allocate buffers in USB RAM
+ tmpEpRamPtr = epRamPtr;
+
+ // Must be 64 byte aligned
+ tmpEpRamPtr = ROUND_UP_TO_MULTIPLE(tmpEpRamPtr, 64);
+
+ if ((tmpEpRamPtr + maxPacket) > (USB_RAM_START + USB_RAM_SIZE)) {
+ // Out of memory
+ return false;
+ }
+
+ // Allocate first buffer
+ endpointState[endpoint].buffer[0] = tmpEpRamPtr;
+ tmpEpRamPtr += maxPacket;
+
+ if (!(options & SINGLE_BUFFERED)) {
+ // Must be 64 byte aligned
+ tmpEpRamPtr = ROUND_UP_TO_MULTIPLE(tmpEpRamPtr, 64);
+
+ if ((tmpEpRamPtr + maxPacket) > (USB_RAM_START + USB_RAM_SIZE)) {
+ // Out of memory
+ return false;
+ }
+
+ // Allocate second buffer
+ endpointState[endpoint].buffer[1] = tmpEpRamPtr;
+ tmpEpRamPtr += maxPacket;
+ }
+
+ // Commit to this USB RAM allocation
+ epRamPtr = tmpEpRamPtr;
+
+ // Remaining endpoint state values
+ endpointState[endpoint].maxPacket = maxPacket;
+ endpointState[endpoint].options = options;
+
+ // Enable double buffering if required
+ if (options & SINGLE_BUFFERED) {
+ LPC_USB->EPBUFCFG &= ~EP(endpoint);
+ } else {
+ // Double buffered
+ LPC_USB->EPBUFCFG |= EP(endpoint);
+ }
+
+ // Enable interrupt
+ LPC_USB->INTEN |= EP(endpoint);
+
+ // Enable endpoint
+ unstallEndpoint(endpoint);
+ return true;
+}
+
+void USBHAL::remoteWakeup(void) {
+ // Clearing DSUS bit initiates a remote wakeup if the
+ // device is currently enabled and suspended - otherwise
+ // it has no effect.
+ LPC_USB->DEVCMDSTAT = devCmdStat & ~DSUS;
+}
+
+
+static void disableEndpoints(void) {
+ uint32_t logEp;
+
+ // Ref. Table 158 "When a bus reset is received, software
+ // must set the disable bit of all endpoints to 1".
+
+ for (logEp = 1; logEp < NUMBER_OF_LOGICAL_ENDPOINTS; logEp++) {
+ ep[logEp].out[0] = CMDSTS_D;
+ ep[logEp].out[1] = CMDSTS_D;
+ ep[logEp].in[0] = CMDSTS_D;
+ ep[logEp].in[1] = CMDSTS_D;
+ }
+
+ // Start of USB RAM for endpoints > 0
+ epRamPtr = usbRamPtr;
+}
+
+
+
+void USBHAL::_usbisr(void) {
+ instance->usbisr();
+}
+
+void USBHAL::usbisr(void) {
+ // Start of frame
+ if (LPC_USB->INTSTAT & FRAME_INT) {
+ // Clear SOF interrupt
+ LPC_USB->INTSTAT = FRAME_INT;
+
+ // SOF event, read frame number
+ SOF(FRAME_NR(LPC_USB->INFO));
+ }
+
+ // Device state
+ if (LPC_USB->INTSTAT & DEV_INT) {
+ LPC_USB->INTSTAT = DEV_INT;
+
+ if (LPC_USB->DEVCMDSTAT & DSUS_C) {
+ // Suspend status changed
+ LPC_USB->DEVCMDSTAT = devCmdStat | DSUS_C;
+ if((LPC_USB->DEVCMDSTAT & DSUS) != 0) {
+ suspendStateChanged(1);
+ }
+ }
+
+ if (LPC_USB->DEVCMDSTAT & DRES_C) {
+ // Bus reset
+ LPC_USB->DEVCMDSTAT = devCmdStat | DRES_C;
+
+ suspendStateChanged(0);
+
+ // Disable endpoints > 0
+ disableEndpoints();
+
+ // Bus reset event
+ busReset();
+ }
+ }
+
+ // Endpoint 0
+ if (LPC_USB->INTSTAT & EP(EP0OUT)) {
+ // Clear EP0OUT/SETUP interrupt
+ LPC_USB->INTSTAT = EP(EP0OUT);
+
+ // Check if SETUP
+ if (LPC_USB->DEVCMDSTAT & SETUP) {
+ // Clear Active and Stall bits for EP0
+ // Documentation does not make it clear if we must use the
+ // EPSKIP register to achieve this, Fig. 16 and NXP reference
+ // code suggests we can just clear the Active bits - check with
+ // NXP to be sure.
+ ep[0].in[0] = 0;
+ ep[0].out[0] = 0;
+
+ // Clear EP0IN interrupt
+ LPC_USB->INTSTAT = EP(EP0IN);
+
+ // Clear SETUP (and INTONNAK_CI/O) in device status register
+ LPC_USB->DEVCMDSTAT = devCmdStat | SETUP;
+
+ // EP0 SETUP event (SETUP data received)
+ EP0setupCallback();
+ } else {
+ // EP0OUT ACK event (OUT data received)
+ EP0out();
+ }
+ }
+
+ if (LPC_USB->INTSTAT & EP(EP0IN)) {
+ // Clear EP0IN interrupt
+ LPC_USB->INTSTAT = EP(EP0IN);
+
+ // EP0IN ACK event (IN data sent)
+ EP0in();
+ }
+
+ for (uint8_t num = 2; num < 5*2; num++) {
+ if (LPC_USB->INTSTAT & EP(num)) {
+ LPC_USB->INTSTAT = EP(num);
+ epComplete |= EP(num);
+ if ((instance->*(epCallback[num - 2]))()) {
+ epComplete &= ~EP(num);
+ }
+ }
+ }
+}
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBHAL_LPC17.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBHAL_LPC17.cpp Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,623 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#if defined(TARGET_LPC1768) || defined(TARGET_LPC2368)
+
+#include "USBHAL.h"
+
+
+// Get endpoint direction
+#define IN_EP(endpoint) ((endpoint) & 1U ? true : false)
+#define OUT_EP(endpoint) ((endpoint) & 1U ? false : true)
+
+// Convert physical endpoint number to register bit
+#define EP(endpoint) (1UL<<endpoint)
+
+// Power Control for Peripherals register
+#define PCUSB (1UL<<31)
+
+// USB Clock Control register
+#define DEV_CLK_EN (1UL<<1)
+#define AHB_CLK_EN (1UL<<4)
+
+// USB Clock Status register
+#define DEV_CLK_ON (1UL<<1)
+#define AHB_CLK_ON (1UL<<4)
+
+// USB Device Interupt registers
+#define FRAME (1UL<<0)
+#define EP_FAST (1UL<<1)
+#define EP_SLOW (1UL<<2)
+#define DEV_STAT (1UL<<3)
+#define CCEMPTY (1UL<<4)
+#define CDFULL (1UL<<5)
+#define RxENDPKT (1UL<<6)
+#define TxENDPKT (1UL<<7)
+#define EP_RLZED (1UL<<8)
+#define ERR_INT (1UL<<9)
+
+// USB Control register
+#define RD_EN (1<<0)
+#define WR_EN (1<<1)
+#define LOG_ENDPOINT(endpoint) ((endpoint>>1)<<2)
+
+// USB Receive Packet Length register
+#define DV (1UL<<10)
+#define PKT_RDY (1UL<<11)
+#define PKT_LNGTH_MASK (0x3ff)
+
+// Serial Interface Engine (SIE)
+#define SIE_WRITE (0x01)
+#define SIE_READ (0x02)
+#define SIE_COMMAND (0x05)
+#define SIE_CMD_CODE(phase, data) ((phase<<8)|(data<<16))
+
+// SIE Command codes
+#define SIE_CMD_SET_ADDRESS (0xD0)
+#define SIE_CMD_CONFIGURE_DEVICE (0xD8)
+#define SIE_CMD_SET_MODE (0xF3)
+#define SIE_CMD_READ_FRAME_NUMBER (0xF5)
+#define SIE_CMD_READ_TEST_REGISTER (0xFD)
+#define SIE_CMD_SET_DEVICE_STATUS (0xFE)
+#define SIE_CMD_GET_DEVICE_STATUS (0xFE)
+#define SIE_CMD_GET_ERROR_CODE (0xFF)
+#define SIE_CMD_READ_ERROR_STATUS (0xFB)
+
+#define SIE_CMD_SELECT_ENDPOINT(endpoint) (0x00+endpoint)
+#define SIE_CMD_SELECT_ENDPOINT_CLEAR_INTERRUPT(endpoint) (0x40+endpoint)
+#define SIE_CMD_SET_ENDPOINT_STATUS(endpoint) (0x40+endpoint)
+
+#define SIE_CMD_CLEAR_BUFFER (0xF2)
+#define SIE_CMD_VALIDATE_BUFFER (0xFA)
+
+// SIE Device Status register
+#define SIE_DS_CON (1<<0)
+#define SIE_DS_CON_CH (1<<1)
+#define SIE_DS_SUS (1<<2)
+#define SIE_DS_SUS_CH (1<<3)
+#define SIE_DS_RST (1<<4)
+
+// SIE Device Set Address register
+#define SIE_DSA_DEV_EN (1<<7)
+
+// SIE Configue Device register
+#define SIE_CONF_DEVICE (1<<0)
+
+// Select Endpoint register
+#define SIE_SE_FE (1<<0)
+#define SIE_SE_ST (1<<1)
+#define SIE_SE_STP (1<<2)
+#define SIE_SE_PO (1<<3)
+#define SIE_SE_EPN (1<<4)
+#define SIE_SE_B_1_FULL (1<<5)
+#define SIE_SE_B_2_FULL (1<<6)
+
+// Set Endpoint Status command
+#define SIE_SES_ST (1<<0)
+#define SIE_SES_DA (1<<5)
+#define SIE_SES_RF_MO (1<<6)
+#define SIE_SES_CND_ST (1<<7)
+
+
+USBHAL * USBHAL::instance;
+
+static volatile int epComplete;
+static uint32_t endpointStallState;
+
+static void SIECommand(uint32_t command) {
+ // The command phase of a SIE transaction
+ LPC_USB->USBDevIntClr = CCEMPTY;
+ LPC_USB->USBCmdCode = SIE_CMD_CODE(SIE_COMMAND, command);
+ while (!(LPC_USB->USBDevIntSt & CCEMPTY));
+}
+
+static void SIEWriteData(uint8_t data) {
+ // The data write phase of a SIE transaction
+ LPC_USB->USBDevIntClr = CCEMPTY;
+ LPC_USB->USBCmdCode = SIE_CMD_CODE(SIE_WRITE, data);
+ while (!(LPC_USB->USBDevIntSt & CCEMPTY));
+}
+
+static uint8_t SIEReadData(uint32_t command) {
+ // The data read phase of a SIE transaction
+ LPC_USB->USBDevIntClr = CDFULL;
+ LPC_USB->USBCmdCode = SIE_CMD_CODE(SIE_READ, command);
+ while (!(LPC_USB->USBDevIntSt & CDFULL));
+ return (uint8_t)LPC_USB->USBCmdData;
+}
+
+static void SIEsetDeviceStatus(uint8_t status) {
+ // Write SIE device status register
+ SIECommand(SIE_CMD_SET_DEVICE_STATUS);
+ SIEWriteData(status);
+}
+
+static uint8_t SIEgetDeviceStatus(void) {
+ // Read SIE device status register
+ SIECommand(SIE_CMD_GET_DEVICE_STATUS);
+ return SIEReadData(SIE_CMD_GET_DEVICE_STATUS);
+}
+
+void SIEsetAddress(uint8_t address) {
+ // Write SIE device address register
+ SIECommand(SIE_CMD_SET_ADDRESS);
+ SIEWriteData((address & 0x7f) | SIE_DSA_DEV_EN);
+}
+
+static uint8_t SIEselectEndpoint(uint8_t endpoint) {
+ // SIE select endpoint command
+ SIECommand(SIE_CMD_SELECT_ENDPOINT(endpoint));
+ return SIEReadData(SIE_CMD_SELECT_ENDPOINT(endpoint));
+}
+
+static uint8_t SIEclearBuffer(void) {
+ // SIE clear buffer command
+ SIECommand(SIE_CMD_CLEAR_BUFFER);
+ return SIEReadData(SIE_CMD_CLEAR_BUFFER);
+}
+
+static void SIEvalidateBuffer(void) {
+ // SIE validate buffer command
+ SIECommand(SIE_CMD_VALIDATE_BUFFER);
+}
+
+static void SIEsetEndpointStatus(uint8_t endpoint, uint8_t status) {
+ // SIE set endpoint status command
+ SIECommand(SIE_CMD_SET_ENDPOINT_STATUS(endpoint));
+ SIEWriteData(status);
+}
+
+static uint16_t SIEgetFrameNumber(void) __attribute__ ((unused));
+static uint16_t SIEgetFrameNumber(void) {
+ // Read current frame number
+ uint16_t lowByte;
+ uint16_t highByte;
+
+ SIECommand(SIE_CMD_READ_FRAME_NUMBER);
+ lowByte = SIEReadData(SIE_CMD_READ_FRAME_NUMBER);
+ highByte = SIEReadData(SIE_CMD_READ_FRAME_NUMBER);
+
+ return (highByte << 8) | lowByte;
+}
+
+static void SIEconfigureDevice(void) {
+ // SIE Configure device command
+ SIECommand(SIE_CMD_CONFIGURE_DEVICE);
+ SIEWriteData(SIE_CONF_DEVICE);
+}
+
+static void SIEunconfigureDevice(void) {
+ // SIE Configure device command
+ SIECommand(SIE_CMD_CONFIGURE_DEVICE);
+ SIEWriteData(0);
+}
+
+static void SIEconnect(void) {
+ // Connect USB device
+ uint8_t status = SIEgetDeviceStatus();
+ SIEsetDeviceStatus(status | SIE_DS_CON);
+}
+
+
+static void SIEdisconnect(void) {
+ // Disconnect USB device
+ uint8_t status = SIEgetDeviceStatus();
+ SIEsetDeviceStatus(status & ~SIE_DS_CON);
+}
+
+
+static uint8_t selectEndpointClearInterrupt(uint8_t endpoint) {
+ // Implemented using using EP_INT_CLR.
+ LPC_USB->USBEpIntClr = EP(endpoint);
+ while (!(LPC_USB->USBDevIntSt & CDFULL));
+ return (uint8_t)LPC_USB->USBCmdData;
+}
+
+
+static void enableEndpointEvent(uint8_t endpoint) {
+ // Enable an endpoint interrupt
+ LPC_USB->USBEpIntEn |= EP(endpoint);
+}
+
+static void disableEndpointEvent(uint8_t endpoint) __attribute__ ((unused));
+static void disableEndpointEvent(uint8_t endpoint) {
+ // Disable an endpoint interrupt
+ LPC_USB->USBEpIntEn &= ~EP(endpoint);
+}
+
+static volatile uint32_t __attribute__((used)) dummyRead;
+uint32_t USBHAL::endpointReadcore(uint8_t endpoint, uint8_t *buffer) {
+ // Read from an OUT endpoint
+ uint32_t size;
+ uint32_t i;
+ uint32_t data = 0;
+ uint8_t offset;
+
+ LPC_USB->USBCtrl = LOG_ENDPOINT(endpoint) | RD_EN;
+ while (!(LPC_USB->USBRxPLen & PKT_RDY));
+
+ size = LPC_USB->USBRxPLen & PKT_LNGTH_MASK;
+
+ offset = 0;
+
+ if (size > 0) {
+ for (i=0; i<size; i++) {
+ if (offset==0) {
+ // Fetch up to four bytes of data as a word
+ data = LPC_USB->USBRxData;
+ }
+
+ // extract a byte
+ *buffer = (data>>offset) & 0xff;
+ buffer++;
+
+ // move on to the next byte
+ offset = (offset + 8) % 32;
+ }
+ } else {
+ dummyRead = LPC_USB->USBRxData;
+ }
+
+ LPC_USB->USBCtrl = 0;
+
+ if ((endpoint >> 1) % 3 || (endpoint >> 1) == 0) {
+ SIEselectEndpoint(endpoint);
+ SIEclearBuffer();
+ }
+
+ return size;
+}
+
+static void endpointWritecore(uint8_t endpoint, uint8_t *buffer, uint32_t size) {
+ // Write to an IN endpoint
+ uint32_t temp, data;
+ uint8_t offset;
+
+ LPC_USB->USBCtrl = LOG_ENDPOINT(endpoint) | WR_EN;
+
+ LPC_USB->USBTxPLen = size;
+ offset = 0;
+ data = 0;
+
+ if (size>0) {
+ do {
+ // Fetch next data byte into a word-sized temporary variable
+ temp = *buffer++;
+
+ // Add to current data word
+ temp = temp << offset;
+ data = data | temp;
+
+ // move on to the next byte
+ offset = (offset + 8) % 32;
+ size--;
+
+ if ((offset==0) || (size==0)) {
+ // Write the word to the endpoint
+ LPC_USB->USBTxData = data;
+ data = 0;
+ }
+ } while (size>0);
+ } else {
+ LPC_USB->USBTxData = 0;
+ }
+
+ // Clear WR_EN to cover zero length packet case
+ LPC_USB->USBCtrl=0;
+
+ SIEselectEndpoint(endpoint);
+ SIEvalidateBuffer();
+}
+
+USBHAL::USBHAL(void) {
+ // Disable IRQ
+ NVIC_DisableIRQ(USB_IRQn);
+
+ // fill in callback array
+ epCallback[0] = &USBHAL::EP1_OUT_callback;
+ epCallback[1] = &USBHAL::EP1_IN_callback;
+ epCallback[2] = &USBHAL::EP2_OUT_callback;
+ epCallback[3] = &USBHAL::EP2_IN_callback;
+ epCallback[4] = &USBHAL::EP3_OUT_callback;
+ epCallback[5] = &USBHAL::EP3_IN_callback;
+ epCallback[6] = &USBHAL::EP4_OUT_callback;
+ epCallback[7] = &USBHAL::EP4_IN_callback;
+ epCallback[8] = &USBHAL::EP5_OUT_callback;
+ epCallback[9] = &USBHAL::EP5_IN_callback;
+ epCallback[10] = &USBHAL::EP6_OUT_callback;
+ epCallback[11] = &USBHAL::EP6_IN_callback;
+ epCallback[12] = &USBHAL::EP7_OUT_callback;
+ epCallback[13] = &USBHAL::EP7_IN_callback;
+ epCallback[14] = &USBHAL::EP8_OUT_callback;
+ epCallback[15] = &USBHAL::EP8_IN_callback;
+ epCallback[16] = &USBHAL::EP9_OUT_callback;
+ epCallback[17] = &USBHAL::EP9_IN_callback;
+ epCallback[18] = &USBHAL::EP10_OUT_callback;
+ epCallback[19] = &USBHAL::EP10_IN_callback;
+ epCallback[20] = &USBHAL::EP11_OUT_callback;
+ epCallback[21] = &USBHAL::EP11_IN_callback;
+ epCallback[22] = &USBHAL::EP12_OUT_callback;
+ epCallback[23] = &USBHAL::EP12_IN_callback;
+ epCallback[24] = &USBHAL::EP13_OUT_callback;
+ epCallback[25] = &USBHAL::EP13_IN_callback;
+ epCallback[26] = &USBHAL::EP14_OUT_callback;
+ epCallback[27] = &USBHAL::EP14_IN_callback;
+ epCallback[28] = &USBHAL::EP15_OUT_callback;
+ epCallback[29] = &USBHAL::EP15_IN_callback;
+
+ // Enable power to USB device controller
+ LPC_SC->PCONP |= PCUSB;
+
+ // Enable USB clocks
+ LPC_USB->USBClkCtrl |= DEV_CLK_EN | AHB_CLK_EN;
+ while (LPC_USB->USBClkSt != (DEV_CLK_ON | AHB_CLK_ON));
+
+ // Configure pins P0.29 and P0.30 to be USB D+ and USB D-
+ LPC_PINCON->PINSEL1 &= 0xc3ffffff;
+ LPC_PINCON->PINSEL1 |= 0x14000000;
+
+ // Disconnect USB device
+ SIEdisconnect();
+
+ // Configure pin P2.9 to be Connect
+ LPC_PINCON->PINSEL4 &= 0xfffcffff;
+ LPC_PINCON->PINSEL4 |= 0x00040000;
+
+ // Connect must be low for at least 2.5uS
+ wait(0.3);
+
+ // Set the maximum packet size for the control endpoints
+ realiseEndpoint(EP0IN, MAX_PACKET_SIZE_EP0, 0);
+ realiseEndpoint(EP0OUT, MAX_PACKET_SIZE_EP0, 0);
+
+ // Attach IRQ
+ instance = this;
+ NVIC_SetVector(USB_IRQn, (uint32_t)&_usbisr);
+
+ // Enable interrupts for device events and EP0
+ LPC_USB->USBDevIntEn = EP_SLOW | DEV_STAT | FRAME;
+ enableEndpointEvent(EP0IN);
+ enableEndpointEvent(EP0OUT);
+}
+
+USBHAL::~USBHAL(void) {
+ // Ensure device disconnected
+ SIEdisconnect();
+ // Disable USB interrupts
+ NVIC_DisableIRQ(USB_IRQn);
+}
+
+void USBHAL::connect(void) {
+ NVIC_EnableIRQ(USB_IRQn);
+ // Connect USB device
+ SIEconnect();
+}
+
+void USBHAL::disconnect(void) {
+ NVIC_DisableIRQ(USB_IRQn);
+ // Disconnect USB device
+ SIEdisconnect();
+}
+
+void USBHAL::configureDevice(void) {
+ SIEconfigureDevice();
+}
+
+void USBHAL::unconfigureDevice(void) {
+ SIEunconfigureDevice();
+}
+
+void USBHAL::setAddress(uint8_t address) {
+ SIEsetAddress(address);
+}
+
+void USBHAL::EP0setup(uint8_t *buffer) {
+ endpointReadcore(EP0OUT, buffer);
+}
+
+void USBHAL::EP0read(void) {
+ // Not required
+}
+
+void USBHAL::EP0readStage(void) {
+ // Not required
+}
+
+uint32_t USBHAL::EP0getReadResult(uint8_t *buffer) {
+ return endpointReadcore(EP0OUT, buffer);
+}
+
+void USBHAL::EP0write(uint8_t *buffer, uint32_t size) {
+ endpointWritecore(EP0IN, buffer, size);
+}
+
+void USBHAL::EP0getWriteResult(void) {
+ // Not required
+}
+
+void USBHAL::EP0stall(void) {
+ // This will stall both control endpoints
+ stallEndpoint(EP0OUT);
+}
+
+EP_STATUS USBHAL::endpointRead(uint8_t endpoint, uint32_t maximumSize) {
+ return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointReadResult(uint8_t endpoint, uint8_t * buffer, uint32_t *bytesRead) {
+
+ //for isochronous endpoint, we don't wait an interrupt
+ if ((endpoint >> 1) % 3 || (endpoint >> 1) == 0) {
+ if (!(epComplete & EP(endpoint)))
+ return EP_PENDING;
+ }
+
+ *bytesRead = endpointReadcore(endpoint, buffer);
+ epComplete &= ~EP(endpoint);
+ return EP_COMPLETED;
+}
+
+EP_STATUS USBHAL::endpointWrite(uint8_t endpoint, uint8_t *data, uint32_t size) {
+ if (getEndpointStallState(endpoint)) {
+ return EP_STALLED;
+ }
+
+ epComplete &= ~EP(endpoint);
+
+ endpointWritecore(endpoint, data, size);
+ return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointWriteResult(uint8_t endpoint) {
+ if (epComplete & EP(endpoint)) {
+ epComplete &= ~EP(endpoint);
+ return EP_COMPLETED;
+ }
+
+ return EP_PENDING;
+}
+
+bool USBHAL::realiseEndpoint(uint8_t endpoint, uint32_t maxPacket, uint32_t flags) {
+ // Realise an endpoint
+ LPC_USB->USBDevIntClr = EP_RLZED;
+ LPC_USB->USBReEp |= EP(endpoint);
+ LPC_USB->USBEpInd = endpoint;
+ LPC_USB->USBMaxPSize = maxPacket;
+
+ while (!(LPC_USB->USBDevIntSt & EP_RLZED));
+ LPC_USB->USBDevIntClr = EP_RLZED;
+
+ // Clear stall state
+ endpointStallState &= ~EP(endpoint);
+
+ enableEndpointEvent(endpoint);
+ return true;
+}
+
+void USBHAL::stallEndpoint(uint8_t endpoint) {
+ // Stall an endpoint
+ if ( (endpoint==EP0IN) || (endpoint==EP0OUT) ) {
+ // Conditionally stall both control endpoints
+ SIEsetEndpointStatus(EP0OUT, SIE_SES_CND_ST);
+ } else {
+ SIEsetEndpointStatus(endpoint, SIE_SES_ST);
+
+ // Update stall state
+ endpointStallState |= EP(endpoint);
+ }
+}
+
+void USBHAL::unstallEndpoint(uint8_t endpoint) {
+ // Unstall an endpoint. The endpoint will also be reinitialised
+ SIEsetEndpointStatus(endpoint, 0);
+
+ // Update stall state
+ endpointStallState &= ~EP(endpoint);
+}
+
+bool USBHAL::getEndpointStallState(uint8_t endpoint) {
+ // Returns true if endpoint stalled
+ return endpointStallState & EP(endpoint);
+}
+
+void USBHAL::remoteWakeup(void) {
+ // Remote wakeup
+ uint8_t status;
+
+ // Enable USB clocks
+ LPC_USB->USBClkCtrl |= DEV_CLK_EN | AHB_CLK_EN;
+ while (LPC_USB->USBClkSt != (DEV_CLK_ON | AHB_CLK_ON));
+
+ status = SIEgetDeviceStatus();
+ SIEsetDeviceStatus(status & ~SIE_DS_SUS);
+}
+
+void USBHAL::_usbisr(void) {
+ instance->usbisr();
+}
+
+
+void USBHAL::usbisr(void) {
+ uint8_t devStat;
+
+ if (LPC_USB->USBDevIntSt & FRAME) {
+ // Start of frame event
+ SOF(SIEgetFrameNumber());
+ // Clear interrupt status flag
+ LPC_USB->USBDevIntClr = FRAME;
+ }
+
+ if (LPC_USB->USBDevIntSt & DEV_STAT) {
+ // Device Status interrupt
+ // Must clear the interrupt status flag before reading the device status from the SIE
+ LPC_USB->USBDevIntClr = DEV_STAT;
+
+ // Read device status from SIE
+ devStat = SIEgetDeviceStatus();
+ //printf("devStat: %d\r\n", devStat);
+
+ if (devStat & SIE_DS_SUS_CH) {
+ // Suspend status changed
+ if((devStat & SIE_DS_SUS) != 0) {
+ suspendStateChanged(0);
+ }
+ }
+
+ if (devStat & SIE_DS_RST) {
+ // Bus reset
+ if((devStat & SIE_DS_SUS) == 0) {
+ suspendStateChanged(1);
+ }
+ busReset();
+ }
+ }
+
+ if (LPC_USB->USBDevIntSt & EP_SLOW) {
+ // (Slow) Endpoint Interrupt
+
+ // Process each endpoint interrupt
+ if (LPC_USB->USBEpIntSt & EP(EP0OUT)) {
+ if (selectEndpointClearInterrupt(EP0OUT) & SIE_SE_STP) {
+ // this is a setup packet
+ EP0setupCallback();
+ } else {
+ EP0out();
+ }
+ LPC_USB->USBDevIntClr = EP_SLOW;
+ }
+
+ if (LPC_USB->USBEpIntSt & EP(EP0IN)) {
+ selectEndpointClearInterrupt(EP0IN);
+ LPC_USB->USBDevIntClr = EP_SLOW;
+ EP0in();
+ }
+
+ for (uint8_t num = 2; num < 16*2; num++) {
+ if (LPC_USB->USBEpIntSt & EP(num)) {
+ selectEndpointClearInterrupt(num);
+ epComplete |= EP(num);
+ LPC_USB->USBDevIntClr = EP_SLOW;
+ if ((instance->*(epCallback[num - 2]))()) {
+ epComplete &= ~EP(num);
+ }
+ }
+ }
+ }
+}
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 USBDevice/USBHAL_LPC40.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice/USBHAL_LPC40.cpp Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,625 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#if defined(TARGET_LPC4088)
+
+#include "USBHAL.h"
+
+
+// Get endpoint direction
+#define IN_EP(endpoint) ((endpoint) & 1U ? true : false)
+#define OUT_EP(endpoint) ((endpoint) & 1U ? false : true)
+
+// Convert physical endpoint number to register bit
+#define EP(endpoint) (1UL<<endpoint)
+
+// Power Control for Peripherals register
+#define PCUSB (1UL<<31)
+
+// USB Clock Control register
+#define DEV_CLK_EN (1UL<<1)
+#define AHB_CLK_EN (1UL<<4)
+
+// USB Clock Status register
+#define DEV_CLK_ON (1UL<<1)
+#define AHB_CLK_ON (1UL<<4)
+
+// USB Device Interupt registers
+#define FRAME (1UL<<0)
+#define EP_FAST (1UL<<1)
+#define EP_SLOW (1UL<<2)
+#define DEV_STAT (1UL<<3)
+#define CCEMPTY (1UL<<4)
+#define CDFULL (1UL<<5)
+#define RxENDPKT (1UL<<6)
+#define TxENDPKT (1UL<<7)
+#define EP_RLZED (1UL<<8)
+#define ERR_INT (1UL<<9)
+
+// USB Control register
+#define RD_EN (1<<0)
+#define WR_EN (1<<1)
+#define LOG_ENDPOINT(endpoint) ((endpoint>>1)<<2)
+
+// USB Receive Packet Length register
+#define DV (1UL<<10)
+#define PKT_RDY (1UL<<11)
+#define PKT_LNGTH_MASK (0x3ff)
+
+// Serial Interface Engine (SIE)
+#define SIE_WRITE (0x01)
+#define SIE_READ (0x02)
+#define SIE_COMMAND (0x05)
+#define SIE_CMD_CODE(phase, data) ((phase<<8)|(data<<16))
+
+// SIE Command codes
+#define SIE_CMD_SET_ADDRESS (0xD0)
+#define SIE_CMD_CONFIGURE_DEVICE (0xD8)
+#define SIE_CMD_SET_MODE (0xF3)
+#define SIE_CMD_READ_FRAME_NUMBER (0xF5)
+#define SIE_CMD_READ_TEST_REGISTER (0xFD)
+#define SIE_CMD_SET_DEVICE_STATUS (0xFE)
+#define SIE_CMD_GET_DEVICE_STATUS (0xFE)
+#define SIE_CMD_GET_ERROR_CODE (0xFF)
+#define SIE_CMD_READ_ERROR_STATUS (0xFB)
+
+#define SIE_CMD_SELECT_ENDPOINT(endpoint) (0x00+endpoint)
+#define SIE_CMD_SELECT_ENDPOINT_CLEAR_INTERRUPT(endpoint) (0x40+endpoint)
+#define SIE_CMD_SET_ENDPOINT_STATUS(endpoint) (0x40+endpoint)
+
+#define SIE_CMD_CLEAR_BUFFER (0xF2)
+#define SIE_CMD_VALIDATE_BUFFER (0xFA)
+
+// SIE Device Status register
+#define SIE_DS_CON (1<<0)
+#define SIE_DS_CON_CH (1<<1)
+#define SIE_DS_SUS (1<<2)
+#define SIE_DS_SUS_CH (1<<3)
+#define SIE_DS_RST (1<<4)
+
+// SIE Device Set Address register
+#define SIE_DSA_DEV_EN (1<<7)
+
+// SIE Configue Device register
+#define SIE_CONF_DEVICE (1<<0)
+
+// Select Endpoint register
+#define SIE_SE_FE (1<<0)
+#define SIE_SE_ST (1<<1)
+#define SIE_SE_STP (1<<2)
+#define SIE_SE_PO (1<<3)
+#define SIE_SE_EPN (1<<4)
+#define SIE_SE_B_1_FULL (1<<5)
+#define SIE_SE_B_2_FULL (1<<6)
+
+// Set Endpoint Status command
+#define SIE_SES_ST (1<<0)
+#define SIE_SES_DA (1<<5)
+#define SIE_SES_RF_MO (1<<6)
+#define SIE_SES_CND_ST (1<<7)
+
+
+USBHAL * USBHAL::instance;
+
+static volatile int epComplete;
+static uint32_t endpointStallState;
+
+static void SIECommand(uint32_t command) {
+ // The command phase of a SIE transaction
+ LPC_USB->DevIntClr = CCEMPTY;
+ LPC_USB->CmdCode = SIE_CMD_CODE(SIE_COMMAND, command);
+ while (!(LPC_USB->DevIntSt & CCEMPTY));
+}
+
+static void SIEWriteData(uint8_t data) {
+ // The data write phase of a SIE transaction
+ LPC_USB->DevIntClr = CCEMPTY;
+ LPC_USB->CmdCode = SIE_CMD_CODE(SIE_WRITE, data);
+ while (!(LPC_USB->DevIntSt & CCEMPTY));
+}
+
+static uint8_t SIEReadData(uint32_t command) {
+ // The data read phase of a SIE transaction
+ LPC_USB->DevIntClr = CDFULL;
+ LPC_USB->CmdCode = SIE_CMD_CODE(SIE_READ, command);
+ while (!(LPC_USB->DevIntSt & CDFULL));
+ return (uint8_t)LPC_USB->CmdData;
+}
+
+static void SIEsetDeviceStatus(uint8_t status) {
+ // Write SIE device status register
+ SIECommand(SIE_CMD_SET_DEVICE_STATUS);
+ SIEWriteData(status);
+}
+
+static uint8_t SIEgetDeviceStatus(void) {
+ // Read SIE device status register
+ SIECommand(SIE_CMD_GET_DEVICE_STATUS);
+ return SIEReadData(SIE_CMD_GET_DEVICE_STATUS);
+}
+
+void SIEsetAddress(uint8_t address) {
+ // Write SIE device address register
+ SIECommand(SIE_CMD_SET_ADDRESS);
+ SIEWriteData((address & 0x7f) | SIE_DSA_DEV_EN);
+}
+
+static uint8_t SIEselectEndpoint(uint8_t endpoint) {
+ // SIE select endpoint command
+ SIECommand(SIE_CMD_SELECT_ENDPOINT(endpoint));
+ return SIEReadData(SIE_CMD_SELECT_ENDPOINT(endpoint));
+}
+
+static uint8_t SIEclearBuffer(void) {
+ // SIE clear buffer command
+ SIECommand(SIE_CMD_CLEAR_BUFFER);
+ return SIEReadData(SIE_CMD_CLEAR_BUFFER);
+}
+
+static void SIEvalidateBuffer(void) {
+ // SIE validate buffer command
+ SIECommand(SIE_CMD_VALIDATE_BUFFER);
+}
+
+static void SIEsetEndpointStatus(uint8_t endpoint, uint8_t status) {
+ // SIE set endpoint status command
+ SIECommand(SIE_CMD_SET_ENDPOINT_STATUS(endpoint));
+ SIEWriteData(status);
+}
+
+static uint16_t SIEgetFrameNumber(void) __attribute__ ((unused));
+static uint16_t SIEgetFrameNumber(void) {
+ // Read current frame number
+ uint16_t lowByte;
+ uint16_t highByte;
+
+ SIECommand(SIE_CMD_READ_FRAME_NUMBER);
+ lowByte = SIEReadData(SIE_CMD_READ_FRAME_NUMBER);
+ highByte = SIEReadData(SIE_CMD_READ_FRAME_NUMBER);
+
+ return (highByte << 8) | lowByte;
+}
+
+static void SIEconfigureDevice(void) {
+ // SIE Configure device command
+ SIECommand(SIE_CMD_CONFIGURE_DEVICE);
+ SIEWriteData(SIE_CONF_DEVICE);
+}
+
+static void SIEunconfigureDevice(void) {
+ // SIE Configure device command
+ SIECommand(SIE_CMD_CONFIGURE_DEVICE);
+ SIEWriteData(0);
+}
+
+static void SIEconnect(void) {
+ // Connect USB device
+ uint8_t status = SIEgetDeviceStatus();
+ SIEsetDeviceStatus(status | SIE_DS_CON);
+}
+
+
+static void SIEdisconnect(void) {
+ // Disconnect USB device
+ uint8_t status = SIEgetDeviceStatus();
+ SIEsetDeviceStatus(status & ~SIE_DS_CON);
+}
+
+
+static uint8_t selectEndpointClearInterrupt(uint8_t endpoint) {
+ // Implemented using using EP_INT_CLR.
+ LPC_USB->EpIntClr = EP(endpoint);
+ while (!(LPC_USB->DevIntSt & CDFULL));
+ return (uint8_t)LPC_USB->CmdData;
+}
+
+
+static void enableEndpointEvent(uint8_t endpoint) {
+ // Enable an endpoint interrupt
+ LPC_USB->EpIntEn |= EP(endpoint);
+}
+
+static void disableEndpointEvent(uint8_t endpoint) __attribute__ ((unused));
+static void disableEndpointEvent(uint8_t endpoint) {
+ // Disable an endpoint interrupt
+ LPC_USB->EpIntEn &= ~EP(endpoint);
+}
+
+static volatile uint32_t __attribute__((used)) dummyRead;
+uint32_t USBHAL::endpointReadcore(uint8_t endpoint, uint8_t *buffer) {
+ // Read from an OUT endpoint
+ uint32_t size;
+ uint32_t i;
+ uint32_t data = 0;
+ uint8_t offset;
+
+ LPC_USB->Ctrl = LOG_ENDPOINT(endpoint) | RD_EN;
+ while (!(LPC_USB->RxPLen & PKT_RDY));
+
+ size = LPC_USB->RxPLen & PKT_LNGTH_MASK;
+
+ offset = 0;
+
+ if (size > 0) {
+ for (i=0; i<size; i++) {
+ if (offset==0) {
+ // Fetch up to four bytes of data as a word
+ data = LPC_USB->RxData;
+ }
+
+ // extract a byte
+ *buffer = (data>>offset) & 0xff;
+ buffer++;
+
+ // move on to the next byte
+ offset = (offset + 8) % 32;
+ }
+ } else {
+ dummyRead = LPC_USB->RxData;
+ }
+
+ LPC_USB->Ctrl = 0;
+
+ if ((endpoint >> 1) % 3 || (endpoint >> 1) == 0) {
+ SIEselectEndpoint(endpoint);
+ SIEclearBuffer();
+ }
+
+ return size;
+}
+
+static void endpointWritecore(uint8_t endpoint, uint8_t *buffer, uint32_t size) {
+ // Write to an IN endpoint
+ uint32_t temp, data;
+ uint8_t offset;
+
+ LPC_USB->Ctrl = LOG_ENDPOINT(endpoint) | WR_EN;
+
+ LPC_USB->TxPLen = size;
+ offset = 0;
+ data = 0;
+
+ if (size>0) {
+ do {
+ // Fetch next data byte into a word-sized temporary variable
+ temp = *buffer++;
+
+ // Add to current data word
+ temp = temp << offset;
+ data = data | temp;
+
+ // move on to the next byte
+ offset = (offset + 8) % 32;
+ size--;
+
+ if ((offset==0) || (size==0)) {
+ // Write the word to the endpoint
+ LPC_USB->TxData = data;
+ data = 0;
+ }
+ } while (size>0);
+ } else {
+ LPC_USB->TxData = 0;
+ }
+
+ // Clear WR_EN to cover zero length packet case
+ LPC_USB->Ctrl=0;
+
+ SIEselectEndpoint(endpoint);
+ SIEvalidateBuffer();
+}
+
+USBHAL::USBHAL(void) {
+ // Disable IRQ
+ NVIC_DisableIRQ(USB_IRQn);
+
+ // fill in callback array
+ epCallback[0] = &USBHAL::EP1_OUT_callback;
+ epCallback[1] = &USBHAL::EP1_IN_callback;
+ epCallback[2] = &USBHAL::EP2_OUT_callback;
+ epCallback[3] = &USBHAL::EP2_IN_callback;
+ epCallback[4] = &USBHAL::EP3_OUT_callback;
+ epCallback[5] = &USBHAL::EP3_IN_callback;
+ epCallback[6] = &USBHAL::EP4_OUT_callback;
+ epCallback[7] = &USBHAL::EP4_IN_callback;
+ epCallback[8] = &USBHAL::EP5_OUT_callback;
+ epCallback[9] = &USBHAL::EP5_IN_callback;
+ epCallback[10] = &USBHAL::EP6_OUT_callback;
+ epCallback[11] = &USBHAL::EP6_IN_callback;
+ epCallback[12] = &USBHAL::EP7_OUT_callback;
+ epCallback[13] = &USBHAL::EP7_IN_callback;
+ epCallback[14] = &USBHAL::EP8_OUT_callback;
+ epCallback[15] = &USBHAL::EP8_IN_callback;
+ epCallback[16] = &USBHAL::EP9_OUT_callback;
+ epCallback[17] = &USBHAL::EP9_IN_callback;
+ epCallback[18] = &USBHAL::EP10_OUT_callback;
+ epCallback[19] = &USBHAL::EP10_IN_callback;
+ epCallback[20] = &USBHAL::EP11_OUT_callback;
+ epCallback[21] = &USBHAL::EP11_IN_callback;
+ epCallback[22] = &USBHAL::EP12_OUT_callback;
+ epCallback[23] = &USBHAL::EP12_IN_callback;
+ epCallback[24] = &USBHAL::EP13_OUT_callback;
+ epCallback[25] = &USBHAL::EP13_IN_callback;
+ epCallback[26] = &USBHAL::EP14_OUT_callback;
+ epCallback[27] = &USBHAL::EP14_IN_callback;
+ epCallback[28] = &USBHAL::EP15_OUT_callback;
+ epCallback[29] = &USBHAL::EP15_IN_callback;
+
+ // Enable power to USB device controller
+ LPC_SC->PCONP |= PCUSB;
+
+ // Enable USB clocks
+ LPC_USB->USBClkCtrl |= DEV_CLK_EN | AHB_CLK_EN;
+ while ((LPC_USB->USBClkSt & (DEV_CLK_EN | AHB_CLK_EN)) != (DEV_CLK_ON | AHB_CLK_ON));
+
+ // Configure pins P0.29 and P0.30 to be USB D+ and USB D-
+ LPC_IOCON->P0_29 &= ~0x07;
+ LPC_IOCON->P0_29 |= 0x01;
+ LPC_IOCON->P0_30 &= ~0x07;
+ LPC_IOCON->P0_30 |= 0x01;
+
+ // Disconnect USB device
+ SIEdisconnect();
+
+ // Configure pin P2.9 to be Connect
+ LPC_IOCON->P2_9 &= ~0x07;
+ LPC_IOCON->P2_9 |= 0x01;
+
+ // Connect must be low for at least 2.5uS
+ wait(0.3);
+
+ // Set the maximum packet size for the control endpoints
+ realiseEndpoint(EP0IN, MAX_PACKET_SIZE_EP0, 0);
+ realiseEndpoint(EP0OUT, MAX_PACKET_SIZE_EP0, 0);
+
+ // Attach IRQ
+ instance = this;
+ NVIC_SetVector(USB_IRQn, (uint32_t)&_usbisr);
+
+ // Enable interrupts for device events and EP0
+ LPC_USB->DevIntEn = EP_SLOW | DEV_STAT | FRAME;
+ enableEndpointEvent(EP0IN);
+ enableEndpointEvent(EP0OUT);
+}
+
+USBHAL::~USBHAL(void) {
+ // Ensure device disconnected
+ SIEdisconnect();
+ // Disable USB interrupts
+ NVIC_DisableIRQ(USB_IRQn);
+}
+
+void USBHAL::connect(void) {
+ NVIC_EnableIRQ(USB_IRQn);
+ // Connect USB device
+ SIEconnect();
+}
+
+void USBHAL::disconnect(void) {
+ NVIC_DisableIRQ(USB_IRQn);
+ // Disconnect USB device
+ SIEdisconnect();
+}
+
+void USBHAL::configureDevice(void) {
+ SIEconfigureDevice();
+}
+
+void USBHAL::unconfigureDevice(void) {
+ SIEunconfigureDevice();
+}
+
+void USBHAL::setAddress(uint8_t address) {
+ SIEsetAddress(address);
+}
+
+void USBHAL::EP0setup(uint8_t *buffer) {
+ endpointReadcore(EP0OUT, buffer);
+}
+
+void USBHAL::EP0read(void) {
+ // Not required
+}
+
+void USBHAL::EP0readStage(void) {
+ // Not required
+}
+
+uint32_t USBHAL::EP0getReadResult(uint8_t *buffer) {
+ return endpointReadcore(EP0OUT, buffer);
+}
+
+void USBHAL::EP0write(uint8_t *buffer, uint32_t size) {
+ endpointWritecore(EP0IN, buffer, size);
+}
+
+void USBHAL::EP0getWriteResult(void) {
+ // Not required
+}
+
+void USBHAL::EP0stall(void) {
+ // This will stall both control endpoints
+ stallEndpoint(EP0OUT);
+}
+
+EP_STATUS USBHAL::endpointRead(uint8_t endpoint, uint32_t maximumSize) {
+ return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointReadResult(uint8_t endpoint, uint8_t * buffer, uint32_t *bytesRead) {
+
+ //for isochronous endpoint, we don't wait an interrupt
+ if ((endpoint >> 1) % 3 || (endpoint >> 1) == 0) {
+ if (!(epComplete & EP(endpoint)))
+ return EP_PENDING;
+ }
+
+ *bytesRead = endpointReadcore(endpoint, buffer);
+ epComplete &= ~EP(endpoint);
+ return EP_COMPLETED;
+}
+
+EP_STATUS USBHAL::endpointWrite(uint8_t endpoint, uint8_t *data, uint32_t size) {
+ if (getEndpointStallState(endpoint)) {
+ return EP_STALLED;
+ }
+
+ epComplete &= ~EP(endpoint);
+
+ endpointWritecore(endpoint, data, size);
+ return EP_PENDING;
+}
+
+EP_STATUS USBHAL::endpointWriteResult(uint8_t endpoint) {
+ if (epComplete & EP(endpoint)) {
+ epComplete &= ~EP(endpoint);
+ return EP_COMPLETED;
+ }
+
+ return EP_PENDING;
+}
+
+bool USBHAL::realiseEndpoint(uint8_t endpoint, uint32_t maxPacket, uint32_t flags) {
+ // Realise an endpoint
+ LPC_USB->DevIntClr = EP_RLZED;
+ LPC_USB->ReEp |= EP(endpoint);
+ LPC_USB->EpInd = endpoint;
+ LPC_USB->MaxPSize = maxPacket;
+
+ while (!(LPC_USB->DevIntSt & EP_RLZED));
+ LPC_USB->DevIntClr = EP_RLZED;
+
+ // Clear stall state
+ endpointStallState &= ~EP(endpoint);
+
+ enableEndpointEvent(endpoint);
+ return true;
+}
+
+void USBHAL::stallEndpoint(uint8_t endpoint) {
+ // Stall an endpoint
+ if ( (endpoint==EP0IN) || (endpoint==EP0OUT) ) {
+ // Conditionally stall both control endpoints
+ SIEsetEndpointStatus(EP0OUT, SIE_SES_CND_ST);
+ } else {
+ SIEsetEndpointStatus(endpoint, SIE_SES_ST);
+
+ // Update stall state
+ endpointStallState |= EP(endpoint);
+ }
+}
+
+void USBHAL::unstallEndpoint(uint8_t endpoint) {
+ // Unstall an endpoint. The endpoint will also be reinitialised
+ SIEsetEndpointStatus(endpoint, 0);
+
+ // Update stall state
+ endpointStallState &= ~EP(endpoint);
+}
+
+bool USBHAL::getEndpointStallState(uint8_t endpoint) {
+ // Returns true if endpoint stalled
+ return endpointStallState & EP(endpoint);
+}
+
+void USBHAL::remoteWakeup(void) {
+ // Remote wakeup
+ uint8_t status;
+
+ // Enable USB clocks
+ LPC_USB->USBClkCtrl |= DEV_CLK_EN | AHB_CLK_EN;
+ while (LPC_USB->USBClkSt != (DEV_CLK_ON | AHB_CLK_ON));
+
+ status = SIEgetDeviceStatus();
+ SIEsetDeviceStatus(status & ~SIE_DS_SUS);
+}
+
+void USBHAL::_usbisr(void) {
+ instance->usbisr();
+}
+
+
+void USBHAL::usbisr(void) {
+ uint8_t devStat;
+
+ if (LPC_USB->DevIntSt & FRAME) {
+ // Start of frame event
+ SOF(SIEgetFrameNumber());
+ // Clear interrupt status flag
+ LPC_USB->DevIntClr = FRAME;
+ }
+
+ if (LPC_USB->DevIntSt & DEV_STAT) {
+ // Device Status interrupt
+ // Must clear the interrupt status flag before reading the device status from the SIE
+ LPC_USB->DevIntClr = DEV_STAT;
+
+ // Read device status from SIE
+ devStat = SIEgetDeviceStatus();
+ //printf("devStat: %d\r\n", devStat);
+
+ if (devStat & SIE_DS_SUS_CH) {
+ // Suspend status changed
+ if((devStat & SIE_DS_SUS) != 0) {
+ suspendStateChanged(0);
+ }
+ }
+
+ if (devStat & SIE_DS_RST) {
+ // Bus reset
+ if((devStat & SIE_DS_SUS) == 0) {
+ suspendStateChanged(1);
+ }
+ busReset();
+ }
+ }
+
+ if (LPC_USB->DevIntSt & EP_SLOW) {
+ // (Slow) Endpoint Interrupt
+
+ // Process each endpoint interrupt
+ if (LPC_USB->EpIntSt & EP(EP0OUT)) {
+ if (selectEndpointClearInterrupt(EP0OUT) & SIE_SE_STP) {
+ // this is a setup packet
+ EP0setupCallback();
+ } else {
+ EP0out();
+ }
+ LPC_USB->DevIntClr = EP_SLOW;
+ }
+
+ if (LPC_USB->EpIntSt & EP(EP0IN)) {
+ selectEndpointClearInterrupt(EP0IN);
+ LPC_USB->DevIntClr = EP_SLOW;
+ EP0in();
+ }
+
+ for (uint8_t num = 2; num < 16*2; num++) {
+ if (LPC_USB->EpIntSt & EP(num)) {
+ selectEndpointClearInterrupt(num);
+ epComplete |= EP(num);
+ LPC_USB->DevIntClr = EP_SLOW;
+ if ((instance->*(epCallback[num - 2]))()) {
+ epComplete &= ~EP(num);
+ }
+ }
+ }
+ }
+}
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 include/PicoCondition.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/PicoCondition.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,45 @@
+#ifndef __PICOMUTEX__
+#define __PICOMUTEX__
+/*
+* Cross-Threading Mutex Class
+*/
+
+#include "mbed.h"
+#include "rtos.h"
+#include "Queue.h"
+
+class PicoCondition
+{
+ private:
+ Queue <int,1> * queue;
+ public:
+ PicoCondition()
+ {
+ queue = new Queue<int,1>();
+ }
+
+ ~PicoCondition()
+ {
+ if(queue)
+ {
+ delete queue;
+ queue = NULL;
+ }
+ }
+
+ bool unlock(uint32_t millisec=0,int * ptr=NULL)
+ {
+ osStatus status;
+ status = queue->put(ptr, millisec);
+ return (status == osEventMessage || status == osOK);
+ }
+
+ bool lock(uint32_t millisec=osWaitForever)
+ {
+ osEvent event = queue->get(millisec);
+ return (event.status == osEventMessage || event.status == osOK);
+ }
+};
+
+
+#endif
\ No newline at end of file
diff -r 759afa79ebe8 -r 540f6e142d59 include/arch/pico_mbed.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/arch/pico_mbed.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 include/heap.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/heap.h Sat Aug 03 13:16: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; \
+ int 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); \
+} \
+
+
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_addressing.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_addressing.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_arp.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/pico_arp.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_config.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/pico_config.h Sat Aug 03 13:16:14 2013 +0000 @@ -0,0 +1,34 @@ +/********************************************************************* +PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved. +See LICENSE and COPYING for usage. + +*********************************************************************/ +#ifndef _INCLUDE_PICO_CONFIG +#define _INCLUDE_PICO_CONFIG +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include "pico_constants.h" + + +#define MBED +//#define PICO_SUPPORT_CRC +#define PICO_SUPPORT_DEVLOOP +#define PICO_SUPPORT_DHCPC +#define PICO_SUPPORT_DHCPD +#define PICO_SUPPORT_DNS_CLIENT +#define PICO_SUPPORT_HTTP_CLIENT +#define PICO_SUPPORT_HTTP +#define PICO_SUPPORT_HTTP_SERVER +#define PICO_SUPPORT_ICMP4 +#define PICO_SUPPORT_PING +#define PICO_SUPPORT_IGMP2 +//#define PICO_SUPPORT_IPFILTER +//#define PICO_SUPPORT_IPFRAG +#define PICO_SUPPORT_IPV4 +#define PICO_SUPPORT_MCAST +#define PICO_SUPPORT_NAT +#define PICO_SUPPORT_TCP +#define PICO_SUPPORT_UDP +# include "arch/pico_mbed.h" +#endif
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_constants.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_constants.h Sat Aug 03 13:16: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 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 << 8) + 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 & 0xFF) << 8) | ((le >> 8) & 0xFF);
+}
+
+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(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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_device.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_device.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,39 @@
+/*********************************************************************
+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;
+};
+
+int pico_device_init(struct pico_device *dev, 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(char* name);
+int pico_device_broadcast(struct pico_frame * f);
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_eth.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_eth.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_frame.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_frame.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_module_eth.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_module_eth.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_protocol.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_protocol.h Sat Aug 03 13:16: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 {
+ char name[MAX_PROTOCOL_NAME];
+ uint32_t hash;
+ enum pico_layer layer;
+ int proto_number;
+ struct pico_queue *q_in;
+ struct pico_queue *q_out;
+ struct pico_frame *(*alloc)(struct pico_protocol *self, int size); /* Frame allocation. */
+ int (*push) (struct pico_protocol *self, struct pico_frame *p); /* Push function, for active outgoing pkts from above */
+ int (*process_out)(struct pico_protocol *self, struct pico_frame *p); /* Send loop. */
+ int (*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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_queue.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_queue.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_socket.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_socket.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,205 @@
+/*********************************************************************
+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;
+ 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_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, void *buf, int len);
+
+int pico_socket_sendto(struct pico_socket *s, 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, 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, void *srv_addr, uint16_t remote_port);
+int pico_socket_listen(struct pico_socket *s, 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);
+
+/* Port check */
+int pico_is_port_free(uint16_t proto, uint16_t port, void *addr, void *net);
+
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_stack.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/pico_stack.h Sat Aug 03 13:16:14 2013 +0000 @@ -0,0 +1,65 @@ +/********************************************************************* +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 + +/* ===== 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
diff -r 759afa79ebe8 -r 540f6e142d59 include/pico_tree.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/pico_tree.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dev_loop.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dev_loop.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,68 @@
+/*********************************************************************
+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)
+{
+ 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)
+{
+}
+
+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;
+}
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dev_loop.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_dev_loop.h Sat Aug 03 13:16: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 +
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dev_mbed.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dev_mbed.cpp Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dev_mbed.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_dev_mbed.h Sat Aug 03 13:16: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 */ +
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dev_mbed_usb.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dev_mbed_usb.cpp Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,124 @@
+/*********************************************************************
+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: Daniele Lacamera, Julien Duraj
+*********************************************************************/
+#include "mbed.h"
+#include "USBCDC_ECM.h"
+#include "ethernet_api.h"
+#include "pico_dev_mbed_usb.h"
+
+extern "C" {
+#include "pico_device.h"
+#include "pico_stack.h"
+}
+
+struct pico_device_mbed_usb {
+ struct pico_device dev;
+ int bytes_left_in_frame;
+ USBCDC_ECM *ecm;
+};
+
+#define ETH_MTU 1514
+static uint8_t buf[ETH_MTU];
+static int rx_i = 0;
+
+Serial __pc(p9, p10, "Serial port"); // tx, rx
+
+extern "C" {
+
+static int pico_mbed_usb_send(struct pico_device *dev, void *buffer, int len)
+{
+ int ret, sent = 0;
+ struct pico_device_mbed_usb *mb = (struct pico_device_mbed_usb *) dev;
+
+ //printf("USBETH> sending %d bytes\n", len);
+ if (len > ETH_MTU)
+ return -1;
+ while (sent < len) {
+ int to_send = 64;
+ if ((len - sent) < to_send)
+ to_send = len -sent;
+ ret = mb->ecm->send(((uint8_t *)buffer) + sent, to_send);
+ if (!ret)
+ return -1;
+ sent += to_send;
+ }
+ if ((len % 64) == 0) {
+ mb->ecm->send(((uint8_t *)buffer), 0);
+ }
+ //printf("USBETH> Sent %d bytes.\n", sent);
+ return sent;
+}
+
+static int pico_mbed_usb_poll(struct pico_device *dev, int loop_score)
+{
+ int size;
+ struct pico_device_mbed_usb *mb = (struct pico_device_mbed_usb *) dev;
+
+ while(loop_score > 0)
+ {
+ bool ret;
+ /* check for new frame(s) */
+ ret = mb->ecm->readEP_NB(buf + rx_i, (uint32_t *)&size);
+
+ /* return if no frame has arrived */
+ if (!ret)
+ return loop_score;
+
+ rx_i += size;
+ if (size < 64) {
+ /* read and process frame */
+ pico_stack_recv(dev, buf, rx_i);
+ loop_score--;
+ rx_i = 0;
+ }
+ }
+ return loop_score;
+}
+
+/* Public interface: create/destroy. */
+void pico_mbed_usb_destroy(struct pico_device *dev)
+{
+ pico_device_destroy(dev);
+}
+
+struct pico_device *pico_mbed_usb_create(char *name, USBCDC_ECM *ecm)
+{
+ std::uint8_t mac[PICO_SIZE_ETH] = {0x00, 0x01, 0xaa, 0x00, 0x02, 0xbb};
+ struct pico_device_mbed_usb *mb = (struct pico_device_mbed_usb*) pico_zalloc(sizeof(struct pico_device_mbed_usb));
+
+ if (!mb)
+ return NULL;
+
+ 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->ecm = ecm;
+ mb->dev.send = pico_mbed_usb_send;
+ mb->dev.poll = pico_mbed_usb_poll;
+ mb->dev.destroy = pico_mbed_usb_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;
+ }
+
+ __pc.printf("ETH> Device %s created.\r\n", mb->dev.name);
+
+ return (struct pico_device *)mb;
+}
+
+void pico_mbed_usb_get_address(char *mac)
+{
+ /* TODO */
+}
+
+}
\ No newline at end of file
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dev_mbed_usb.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dev_mbed_usb.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,29 @@
+/*********************************************************************
+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_USB_H
+#define PICO_DEV_MBED_USB_H
+#ifdef __cplusplus
+#include "USBCDC_ECM.h"
+extern "C" {
+#endif
+
+#include "pico_config.h"
+#include "pico_device.h"
+
+void pico_mbed_usb_destroy(struct pico_device *vde);
+struct pico_device *pico_mbed_usb_create(char *name, USBCDC_ECM *usb);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PICO_DEV_MBED_H */
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dhcp_client.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_client.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,731 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Frederik Van Slycken, Kristof Roelants
+*********************************************************************/
+
+#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"
+
+#ifdef PICO_SUPPORT_DHCPC
+
+/***********
+ * structs *
+ ***********/
+
+static uint8_t dhcp_client_mutex = 1; /* to serialize client negotations if multiple devices */
+
+struct dhcp_timer_param{
+ uint16_t type;
+ struct pico_dhcp_client_cookie* cli;
+ int valid;
+};
+
+struct pico_dhcp_client_cookie
+{
+ uint32_t xid;
+ uint32_t *xid_user;
+ struct pico_ip4 address;
+ struct pico_ip4 netmask;
+ struct pico_ip4 gateway;
+ struct pico_ip4 nameserver;
+ struct pico_ip4 server_id;
+ uint32_t lease_time;
+ uint32_t T1;
+ uint32_t T2;
+ struct pico_socket* socket;
+ int connected;
+ struct pico_device* device;
+ unsigned long start_time;
+ int attempt;
+ enum dhcp_negotiation_state state;
+ void (*cb)(void* cli, int code);
+ struct dhcp_timer_param* timer_param_1;
+ struct dhcp_timer_param* timer_param_2;
+ struct dhcp_timer_param* timer_param_lease;
+ struct dhcp_timer_param* timer_param_retransmit;
+ int link_added;
+};
+
+static int dhcp_cookies_cmp(void *ka, void *kb)
+{
+ struct pico_dhcp_client_cookie *a = ka, *b = kb;
+ if (a->xid < b->xid)
+ return -1;
+ else if (a->xid > b->xid)
+ return 1;
+ else
+ return 0;
+}
+PICO_TREE_DECLARE(DHCPCookies, dhcp_cookies_cmp);
+
+/*************************
+ * function declarations *
+ *************************/
+static void pico_dhcp_state_machine(int type, struct pico_dhcp_client_cookie* cli, uint8_t* data, int len);
+static void pico_dhcp_reinitiate_negotiation(unsigned long now, void *arg);
+
+//cb
+static void pico_dhcp_wakeup(uint16_t ev, struct pico_socket *s);
+static void dhcp_timer_cb(unsigned long tick, void* param);
+
+//util
+static void pico_dhcp_retry(struct pico_dhcp_client_cookie *client);
+static int dhclient_send(struct pico_dhcp_client_cookie *cli, uint8_t msg_type);
+static int pico_dhcp_verify_and_identify_type(uint8_t* data, int len, struct pico_dhcp_client_cookie *cli);
+static int init_cookie(struct pico_dhcp_client_cookie* cli);
+static struct pico_dhcp_client_cookie* get_cookie_by_xid(uint32_t xid);
+static uint32_t get_xid(uint8_t* data);
+
+//fsm functions
+static int recv_offer(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+static int recv_ack(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+static int renew(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+static int reset(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+static int retransmit(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+
+//fsm implementation
+static void pico_dhcp_state_machine(int type, struct pico_dhcp_client_cookie* cli, uint8_t* data, int len);
+
+/***************
+ * entry point *
+ ***************/
+
+static uint32_t pico_dhcp_execute_init(struct pico_dhcp_client_cookie *cli)
+{
+ if (!dhcp_client_mutex) {
+ pico_timer_add(3000, pico_dhcp_reinitiate_negotiation, cli);
+ return 0;
+ }
+ dhcp_client_mutex--;
+
+ if (init_cookie(cli) < 0)
+ return -1;
+
+ dbg("DHCPC: cookie with xid %u\n", cli->xid);
+
+ if (pico_tree_insert(&DHCPCookies, cli)) {
+ pico_err = PICO_ERR_EAGAIN;
+ if(cli->cb != NULL) {
+ cli->cb(cli, PICO_DHCP_ERROR);
+ }
+ pico_free(cli);
+ return -1; /* Element key already exists */
+ }
+
+ if (dhclient_send(cli, PICO_DHCP_MSG_DISCOVER) < 0)
+ return -1;
+
+ return 0;
+}
+
+/* returns a pointer to the client cookie. The user should pass this pointer every time he calls a dhcp-function. This is so that we can (one day) support dhcp on multiple interfaces */
+int pico_dhcp_initiate_negotiation(struct pico_device *device, void (*callback)(void *cli, int code), uint32_t *xid)
+{
+ struct pico_dhcp_client_cookie *cli;
+
+ if(!device || !callback || !xid){
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ cli = pico_zalloc(sizeof(struct pico_dhcp_client_cookie));
+ if(!cli){
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+
+ cli->device = device;
+ cli->cb = callback;
+ cli->xid_user = xid;
+ *(cli->xid_user) = 0;
+
+ return pico_dhcp_execute_init(cli);
+}
+
+static void pico_dhcp_reinitiate_negotiation(unsigned long now, void *arg)
+{
+ struct pico_dhcp_client_cookie *cli = (struct pico_dhcp_client_cookie *) arg;
+
+ pico_dhcp_execute_init(cli);
+
+ return;
+}
+
+/********************
+ * access functions *
+ ********************/
+
+struct pico_ip4 pico_dhcp_get_address(void* cli)
+{
+ return ((struct pico_dhcp_client_cookie*)cli)->address;
+}
+
+struct pico_ip4 pico_dhcp_get_gateway(void* cli)
+{
+ return ((struct pico_dhcp_client_cookie*)cli)->gateway;
+}
+
+struct pico_ip4 pico_dhcp_get_nameserver(void* cli)
+{
+ return ((struct pico_dhcp_client_cookie*)cli)->nameserver;
+}
+
+/*************
+ * callbacks *
+ *************/
+
+static void pico_dhcp_wakeup(uint16_t ev, struct pico_socket *s)
+{
+ uint8_t buf[DHCPC_DATAGRAM_SIZE];
+ int r=0;
+ uint32_t peer;
+ uint16_t port;
+ int type;
+
+ struct pico_dhcp_client_cookie *cli;
+ dbg("DHCPC: called dhcp_wakeup\n");
+ if (ev == PICO_SOCK_EV_RD) {
+ do {
+ r = pico_socket_recvfrom(s, buf, DHCPC_DATAGRAM_SIZE, &peer, &port);
+ cli = get_cookie_by_xid(get_xid(buf));
+ if(cli == NULL)
+ return;
+ if (r > 0 && port == PICO_DHCPD_PORT) {
+ type = pico_dhcp_verify_and_identify_type(buf, r, cli);
+ pico_dhcp_state_machine(type, cli, buf, r);
+ }
+ } while(r>0);
+ }
+}
+
+static void dhcp_timer_cb(unsigned long tick, void* param)
+{
+ struct dhcp_timer_param* param2 = (struct dhcp_timer_param*) param;
+ if(param2->valid == 1){
+ //dbg("called timer cb on active timer type %d\n",param2->type);
+ pico_dhcp_state_machine(param2->type, param2->cli, NULL, 0);
+ }
+ if(param2->cli->timer_param_1 == param){
+ param2->cli->timer_param_1 = NULL;
+ }
+ if(param2->cli->timer_param_2 == param){
+ param2->cli->timer_param_2 = NULL;
+ }
+ if(param2->cli->timer_param_lease == param){
+ param2->cli->timer_param_lease = NULL;
+ }
+ if(param2->cli->timer_param_retransmit == param){
+ param2->cli->timer_param_retransmit = NULL;
+ }
+
+ pico_free(param);
+
+}
+/*****************
+ * fsm functions *
+ *****************/
+
+static int recv_offer(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len)
+{
+ struct pico_dhcphdr *dhdr = (struct pico_dhcphdr *) data;
+ uint8_t *nextopt, opt_data[20], opt_type;
+ int opt_len = 20;
+ uint8_t msg_type = 0xFF;
+ int T1_set = 0;
+ int T2_set = 0;
+
+ cli->address.addr = dhdr->yiaddr;
+
+ opt_type = dhcp_get_next_option(dhdr->options, opt_data, &opt_len, &nextopt);
+ while (opt_type != PICO_DHCPOPT_END) {
+ if (opt_type == PICO_DHCPOPT_MSGTYPE)
+ msg_type = opt_data[0];
+ if ((opt_type == PICO_DHCPOPT_LEASETIME) && (opt_len == 4)){
+ memcpy(&cli->lease_time, opt_data, 4);
+ cli->lease_time = long_be(cli->lease_time);
+ }
+ if ((opt_type == PICO_DHCPOPT_RENEWALTIME) && (opt_len == 4)){
+ memcpy(&cli->T1, opt_data, 4);
+ cli->T1 = long_be(cli->T1);
+ T1_set =1;
+ }
+ if ((opt_type == PICO_DHCPOPT_REBINDINGTIME) && (opt_len == 4)){
+ memcpy(&cli->T2, opt_data, 4);
+ cli->T2 = long_be(cli->T2);
+ T2_set =1;
+ }
+ if ((opt_type == PICO_DHCPOPT_ROUTER) && (opt_len == 4)) //XXX assuming only one router will be advertised...
+ memcpy(&cli->gateway.addr, opt_data, 4);
+ if ((opt_type == PICO_DHCPOPT_DNS) && (opt_len == 4))
+ memcpy(&cli->nameserver.addr, opt_data, 4);
+ if ((opt_type == PICO_DHCPOPT_NETMASK) && (opt_len == 4))
+ memcpy(&cli->netmask.addr, opt_data, 4);
+ if ((opt_type == PICO_DHCPOPT_SERVERID) && (opt_len == 4))
+ memcpy(&cli->server_id.addr, opt_data, 4);
+ if (opt_type == PICO_DHCPOPT_OPTIONOVERLOAD)
+ dbg("DHCPC: WARNING: option overload present (not processed)");
+
+ opt_len = 20;
+ opt_type = dhcp_get_next_option(NULL, opt_data, &opt_len, &nextopt);
+ }
+
+ /* default values for T1 and T2 if necessary */
+ if(T1_set != 1)
+ cli->T1 = cli->lease_time >> 1;
+ if(T2_set != 1)
+ cli->T2 = (cli->lease_time * 875) / 1000;
+
+
+
+ if ((msg_type != PICO_DHCP_MSG_OFFER) || !cli->lease_time || !cli->netmask.addr || !cli->server_id.addr )
+ return 0;
+
+
+ dhclient_send(cli, PICO_DHCP_MSG_REQUEST);
+ cli->state = DHCPSTATE_REQUEST;
+ return 1;
+}
+
+static int recv_ack(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len)
+{
+ struct pico_ip4 address = {0};
+
+ if(cli->link_added == 0){
+ /* close the socket bound on address 0.0.0.0 */
+ pico_socket_close(cli->socket);
+ cli->socket = NULL;
+ pico_ipv4_link_del(cli->device, address);
+ pico_ipv4_link_add(cli->device, cli->address, cli->netmask);
+ cli->link_added = 1;
+ }
+ cli->state = DHCPSTATE_BOUND;
+
+ dbg("DHCPC: T1: %d\n",cli->T1);
+ dbg("DHCPC: T2: %d\n",cli->T2);
+ dbg("DHCPC: lease time: %d\n",cli->lease_time);
+
+ if(cli->timer_param_1)
+ cli->timer_param_1->valid = 0;
+ if(cli->timer_param_2)
+ cli->timer_param_2->valid = 0;
+ if(cli->timer_param_lease)
+ cli->timer_param_lease->valid = 0;
+ if(cli->timer_param_retransmit)
+ cli->timer_param_retransmit->valid = 0;
+
+
+ cli->timer_param_1 = pico_zalloc(sizeof(struct dhcp_timer_param));
+ if(!cli->timer_param_1){
+ if(cli->cb != NULL){
+ pico_err = PICO_ERR_ENOMEM;
+ cli->cb(cli, PICO_DHCP_ERROR);
+ }
+ return 0;
+ }
+ cli->timer_param_2 = pico_zalloc(sizeof(struct dhcp_timer_param));
+ if(!cli->timer_param_2){
+ if(cli->cb != NULL){
+ pico_err = PICO_ERR_ENOMEM;
+ cli->cb(cli, PICO_DHCP_ERROR);
+ }
+ return 0;
+ }
+ cli->timer_param_lease = pico_zalloc(sizeof(struct dhcp_timer_param));
+ if(!cli->timer_param_lease){
+ if(cli->cb != NULL){
+ pico_err = PICO_ERR_ENOMEM;
+ cli->cb(cli, PICO_DHCP_ERROR);
+ }
+ return 0;
+ }
+ cli->timer_param_1->valid = 1;
+ cli->timer_param_2->valid = 1;
+ cli->timer_param_lease->valid = 1;
+
+ cli->timer_param_1->cli = cli;
+ cli->timer_param_2->cli = cli;
+ cli->timer_param_lease->cli = cli;
+
+ cli->timer_param_1->type = PICO_DHCP_EVENT_T1;
+ cli->timer_param_2->type = PICO_DHCP_EVENT_T2;
+ cli->timer_param_lease->type = PICO_DHCP_EVENT_LEASE;
+ //add timer
+ pico_timer_add(cli->T1*1000, dhcp_timer_cb, cli->timer_param_1);
+ pico_timer_add(cli->T2*1000, dhcp_timer_cb, cli->timer_param_2);
+ pico_timer_add(cli->lease_time*1000, dhcp_timer_cb, cli->timer_param_lease);
+
+ *(cli->xid_user) = cli->xid;
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_SUCCESS);
+ else
+ dbg("DHCPC: no callback\n");
+
+ dhcp_client_mutex++;
+ cli->state = DHCPSTATE_BOUND;
+ return 0;
+}
+
+static int renew(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len)
+{
+ uint16_t port = PICO_DHCP_CLIENT_PORT;
+
+ /* open and bind to currently acquired address */
+ if (!cli->socket){
+ cli->socket = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcp_wakeup);
+ if (!cli->socket) {
+ dbg("DHCPC: error opening socket on renew: %s\n", strerror(pico_err));
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_ERROR);
+ return -1;
+ }
+ if (pico_socket_bind(cli->socket, &cli->address, &port) != 0){
+ dbg("DHCPC: error binding socket on renew: %s\n", strerror(pico_err));
+ pico_socket_close(cli->socket);
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_ERROR);
+ return -1;
+ }
+ }
+ cli->state = DHCPSTATE_RENEWING;
+ dhclient_send(cli, PICO_DHCP_MSG_REQUEST);
+
+ return 0;
+}
+
+static int reset(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len)
+{
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_RESET);
+ //reset pretty much everything
+
+ if(cli->timer_param_1)
+ cli->timer_param_1->valid = 0;
+ if(cli->timer_param_2)
+ cli->timer_param_2->valid = 0;
+ if(cli->timer_param_lease)
+ cli->timer_param_lease->valid = 0;
+ if(cli->timer_param_retransmit)
+ cli->timer_param_retransmit->valid = 0;
+
+ pico_socket_close(cli->socket);
+ pico_ipv4_link_del(cli->device, cli->address);
+
+ //initiate negotiations again
+ init_cookie(cli);
+ pico_dhcp_retry(cli);
+ dhclient_send(cli, PICO_DHCP_MSG_DISCOVER);
+
+ return 0;
+
+}
+
+static int retransmit(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len)
+{
+ pico_dhcp_retry(cli);
+
+ if(cli->state == DHCPSTATE_DISCOVER)
+ dhclient_send(cli, PICO_DHCP_MSG_DISCOVER);
+ else if(cli->state == DHCPSTATE_RENEWING)
+ dhclient_send(cli, PICO_DHCP_MSG_REQUEST);
+ else
+ dbg("DHCPC: WARNING: should not get here in state %d!\n", cli->state);
+
+ return 0;
+
+}
+
+/**********************
+ * fsm implementation *
+ **********************/
+
+struct dhcp_action_entry {
+ uint16_t tcpstate;
+ int (*offer)(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+ int (*ack)(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+ int (*nak)(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+ int (*timer1)(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+ int (*timer_lease)(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+ int (*timer_retransmit)(struct pico_dhcp_client_cookie *cli, uint8_t *data, int len);
+};
+
+static struct dhcp_action_entry dhcp_fsm[] = {
+ /* State offer ack nak timer1 timer_lease timer_retransmit*/
+ { DHCPSTATE_DISCOVER, recv_offer, NULL, NULL, NULL, reset, retransmit},
+ { DHCPSTATE_OFFER, NULL, NULL, NULL, NULL, reset, NULL},
+ { DHCPSTATE_REQUEST, NULL, recv_ack, reset, NULL, reset, retransmit},
+ { DHCPSTATE_BOUND, NULL, NULL, reset, renew, reset, NULL},
+ { DHCPSTATE_RENEWING, NULL, recv_ack, reset, NULL, reset, retransmit},
+};
+
+
+static void pico_dhcp_state_machine(int type, struct pico_dhcp_client_cookie* cli, uint8_t* data, int len)
+{
+ dbg("DHCPC: received incoming event of type %d\n", type);
+ switch(type){
+ case PICO_DHCP_MSG_OFFER:
+ if(dhcp_fsm[cli->state].offer != NULL)
+ dhcp_fsm[cli->state].offer(cli, data, len);
+ break;
+ case PICO_DHCP_MSG_ACK:
+ if(dhcp_fsm[cli->state].ack != NULL){
+ dhcp_fsm[cli->state].ack(cli, data, len);
+ }
+ break;
+ case PICO_DHCP_MSG_NAK:
+ if(dhcp_fsm[cli->state].nak!= NULL){
+ dhcp_fsm[cli->state].nak(cli, data, len);
+ }
+ break;
+ case PICO_DHCP_EVENT_T1:
+ if(dhcp_fsm[cli->state].timer1!= NULL){
+ dhcp_fsm[cli->state].timer1(cli, NULL, 0);
+ }
+ break;
+ case PICO_DHCP_EVENT_LEASE:
+ if(dhcp_fsm[cli->state].timer_lease!= NULL){
+ dhcp_fsm[cli->state].timer_lease(cli, NULL, 0);
+ }
+ break;
+ case PICO_DHCP_EVENT_RETRANSMIT:
+ if(dhcp_fsm[cli->state].timer_retransmit!= NULL){
+ dhcp_fsm[cli->state].timer_retransmit(cli, NULL, 0);
+ }
+ break;
+ default:
+ dbg("DHCPC: event not supported yet!!\n");
+ break;
+ }
+}
+
+
+/*********************
+ * utility functions *
+ *********************/
+
+static void pico_dhcp_retry(struct pico_dhcp_client_cookie *cli)
+{
+ const int MAX_RETRY = 3;
+ uint32_t new_xid;
+ if (++cli->attempt > MAX_RETRY) {
+ cli->start_time = pico_tick;
+ cli->attempt = 0;
+ new_xid = pico_rand();
+ while(get_cookie_by_xid(new_xid) != NULL){
+ new_xid = pico_rand();
+ }
+ cli->xid = new_xid;
+ cli->state = DHCPSTATE_DISCOVER;
+ }
+}
+
+static int dhclient_send(struct pico_dhcp_client_cookie *cli, uint8_t msg_type)
+{
+ uint8_t buf_out[DHCPC_DATAGRAM_SIZE] = {0};
+ struct pico_dhcphdr *dh_out = (struct pico_dhcphdr *) buf_out;
+ int sent = 0;
+ int i = 0;
+ struct pico_ip4 destination;
+ uint16_t port = PICO_DHCPD_PORT;
+ if(cli->state == DHCPSTATE_BOUND || cli->state == DHCPSTATE_RENEWING){
+ destination.addr = cli->server_id.addr;
+ }else{
+ destination.addr = long_be(0xFFFFFFFF);
+ }
+
+ if(cli->device->eth == NULL){
+ pico_err = PICO_ERR_EOPNOTSUPP;
+ if(cli->cb != NULL){
+ cli->cb(cli, PICO_DHCP_ERROR);
+ }
+ return -1;
+ }
+ memcpy(dh_out->hwaddr, &cli->device->eth->mac, PICO_HLEN_ETHER);
+ dh_out->op = PICO_DHCP_OP_REQUEST;
+ dh_out->htype = PICO_HTYPE_ETHER;
+ dh_out->hlen = PICO_HLEN_ETHER;
+ dh_out->xid = cli->xid;
+ dh_out->secs = (msg_type == PICO_DHCP_MSG_REQUEST)?0:short_be((pico_tick - cli->start_time)/1000);
+ dh_out->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
+ if (cli->state == DHCPSTATE_RENEWING)
+ dh_out->ciaddr = cli->address.addr;
+
+ /* Option: msg type, len 1 */
+ dh_out->options[i++] = PICO_DHCPOPT_MSGTYPE;
+ dh_out->options[i++] = 1;
+ dh_out->options[i++] = msg_type;
+
+ if (( msg_type == PICO_DHCP_MSG_REQUEST) && ( cli->state != DHCPSTATE_RENEWING )) {
+ dh_out->options[i++] = PICO_DHCPOPT_REQIP;
+ dh_out->options[i++] = 4;
+ dh_out->options[i++] = (long_be(cli->address.addr) & 0xFF000000) >> 24;
+ dh_out->options[i++] = (long_be(cli->address.addr) & 0xFF0000) >> 16;
+ dh_out->options[i++] = (long_be(cli->address.addr) & 0xFF00) >> 8;
+ dh_out->options[i++] = (long_be(cli->address.addr) & 0xFF);
+ dh_out->options[i++] = PICO_DHCPOPT_SERVERID;
+ dh_out->options[i++] = 4;
+ dh_out->options[i++] = (long_be(cli->server_id.addr) & 0xFF000000) >> 24;
+ dh_out->options[i++] = (long_be(cli->server_id.addr) & 0xFF0000) >> 16;
+ dh_out->options[i++] = (long_be(cli->server_id.addr) & 0xFF00) >> 8;
+ dh_out->options[i++] = (long_be(cli->server_id.addr) & 0xFF);
+ }
+
+ /* Option: req list, len 4 */
+ dh_out->options[i++] = PICO_DHCPOPT_PARMLIST;
+ dh_out->options[i++] = 7;
+ dh_out->options[i++] = PICO_DHCPOPT_NETMASK;
+ dh_out->options[i++] = PICO_DHCPOPT_BCAST;
+ dh_out->options[i++] = PICO_DHCPOPT_TIME;
+ dh_out->options[i++] = PICO_DHCPOPT_ROUTER;
+ dh_out->options[i++] = PICO_DHCPOPT_HOSTNAME;
+ dh_out->options[i++] = PICO_DHCPOPT_RENEWALTIME;
+ dh_out->options[i++] = PICO_DHCPOPT_REBINDINGTIME;
+
+ /* Option : max message size */
+ if( msg_type == PICO_DHCP_MSG_REQUEST || msg_type == PICO_DHCP_MSG_DISCOVER){
+ uint16_t dds = DHCPC_DATAGRAM_SIZE;
+ dh_out->options[i++] = PICO_DHCPOPT_MAXMSGSIZE;
+ dh_out->options[i++] = 2;
+ dh_out->options[i++] = (dds & 0xFF00) >> 8;
+ dh_out->options[i++] = (dds & 0xFF);
+ }
+
+
+
+ dh_out->options[i] = PICO_DHCPOPT_END;
+ sent = pico_socket_sendto(cli->socket, buf_out, DHCPC_DATAGRAM_SIZE, &destination, port);
+ if (sent < 0) {
+ dbg("DHCPC: sendto failed: %s\n", strerror(pico_err));
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_ERROR);
+ }
+
+
+ //resend-timer :
+ if(cli->timer_param_retransmit != NULL)
+ cli->timer_param_retransmit->valid=0;
+
+ cli->timer_param_retransmit = pico_zalloc(sizeof(struct dhcp_timer_param));
+ if(!cli->timer_param_retransmit){
+ if(cli->cb != NULL)
+ pico_err = PICO_ERR_ENOMEM;
+ cli->cb(cli, PICO_DHCP_ERROR);
+ return -1;
+ }
+ cli->timer_param_retransmit->valid = 1;
+ cli->timer_param_retransmit->cli = cli;
+ cli->timer_param_retransmit->type = PICO_DHCP_EVENT_RETRANSMIT;
+ pico_timer_add(4000, dhcp_timer_cb, cli->timer_param_retransmit);
+
+ return 0;
+}
+
+//identifies type & does some preprocessing : checking if everything is valid
+static int pico_dhcp_verify_and_identify_type(uint8_t* data, int len, struct pico_dhcp_client_cookie *cli)
+{
+ struct pico_dhcphdr *dhdr = (struct pico_dhcphdr *) data;
+ uint8_t *nextopt, opt_data[20], opt_type;
+ int opt_len = 20;
+
+ if (dhdr->xid != cli->xid)
+ return 0;
+
+ if (!is_options_valid(dhdr->options, len - sizeof(struct pico_dhcphdr)))
+ return 0;
+
+ if( dhdr->dhcp_magic != PICO_DHCPD_MAGIC_COOKIE)
+ return 0;
+
+ opt_type = dhcp_get_next_option(dhdr->options, opt_data, &opt_len, &nextopt);
+ while (opt_type != PICO_DHCPOPT_END) {
+ /* parse interesting options here */
+ if (opt_type == PICO_DHCPOPT_MSGTYPE) {
+ return *opt_data;
+ }
+ opt_len = 20;
+ opt_type = dhcp_get_next_option(NULL, opt_data, &opt_len, &nextopt);
+ }
+ return 0;
+
+}
+
+static int init_cookie(struct pico_dhcp_client_cookie* cli)
+{
+ uint8_t n = 3;
+ uint16_t port = PICO_DHCP_CLIENT_PORT;
+ struct pico_ip4 address, netmask;
+
+ address.addr = long_be(0x00000000);
+ netmask.addr = long_be(0x00000000);
+
+ cli->state = DHCPSTATE_DISCOVER;
+ cli->start_time = pico_tick;
+ cli->attempt = 0;
+
+ cli->socket = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcp_wakeup);
+ if (!cli->socket) {
+ dbg("DHCPC: error opening socket: %s\n", strerror(pico_err));
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_ERROR);
+ return -1;
+ }
+ if (pico_socket_bind(cli->socket, &address, &port) != 0){
+ dbg("DHCPC: error binding socket: %s\n", strerror(pico_err));
+ pico_socket_close(cli->socket);
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_ERROR);
+ return -1;
+ }
+ cli->socket->dev = cli->device;
+
+ if(pico_ipv4_link_add(cli->device, address, netmask) != 0){
+ dbg("DHCPC: error adding link: %s\n", strerror(pico_err));
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_ERROR);
+ return -1;
+ }
+
+ /* attempt to generate a correct xid 3 times, then fail */
+ do {
+ cli->xid = pico_rand();
+ } while (!cli->xid && --n);
+ if (!cli->xid) {
+ if(cli->cb != NULL)
+ cli->cb(cli, PICO_DHCP_ERROR);
+ return -1;
+ }
+
+ return 0;
+}
+
+static struct pico_dhcp_client_cookie *get_cookie_by_xid(uint32_t xid)
+{
+ struct pico_dhcp_client_cookie test = { }, *cookie = NULL;
+
+ if (!xid)
+ return NULL;
+
+ test.xid = xid;
+ cookie = pico_tree_findKey(&DHCPCookies, &test);
+ if (!cookie)
+ return NULL;
+ else
+ return cookie;
+}
+
+void *pico_dhcp_get_identifier(uint32_t xid)
+{
+ return (void *) get_cookie_by_xid(xid);
+}
+
+static uint32_t get_xid(uint8_t* data)
+{
+ struct pico_dhcphdr *dhdr = (struct pico_dhcphdr *) data;
+ return dhdr->xid;
+}
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dhcp_client.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_dhcp_client.h Sat Aug 03 13:16:14 2013 +0000 @@ -0,0 +1,36 @@ +/********************************************************************* +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 + +/* DHCP EVENT TYPE + * these come after the message types, used for the state machine*/ +#define PICO_DHCP_EVENT_T1 9 +#define PICO_DHCP_EVENT_T2 10 +#define PICO_DHCP_EVENT_LEASE 11 +#define PICO_DHCP_EVENT_RETRANSMIT 12 + +#endif
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dhcp_common.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_common.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,67 @@
+/*********************************************************************
+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)
+//this function should only be used after you checked if the options are valid! otherwise it could read from bad memory!
+uint8_t dhcp_get_next_option(uint8_t *begin, uint8_t *data, int *len, uint8_t **nextopt)
+{
+ uint8_t *p;
+ uint8_t type;
+ uint8_t opt_len;
+
+ if (!begin)
+ p = *nextopt;
+ else
+ p = begin;
+
+ type = *p;
+ *nextopt = ++p;
+ if ((type == PICO_DHCPOPT_END) || (type == PICO_DHCPOPT_PAD)) {
+ memset(data, 0, *len);
+ len = 0;
+ return type;
+ }
+ opt_len = *p;
+ p++;
+ if (*len > opt_len)
+ *len = opt_len;
+ memcpy(data, p, *len);
+ *nextopt = p + opt_len;
+ return type;
+}
+
+int is_options_valid(uint8_t *opt_buffer, int len)
+{
+ uint8_t *p = opt_buffer;
+ while (len > 0) {
+ if (*p == PICO_DHCPOPT_END)
+ return 1;
+ else if (*p == PICO_DHCPOPT_PAD) {
+ p++;
+ len--;
+ } else {
+ uint8_t opt_len;
+ p++;
+ len--;
+ if(len > 0) {
+ opt_len = *p;
+ p += opt_len + 1;
+ len -= opt_len;
+ }else
+ return 0;
+ }
+ }
+ return 0;
+}
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dhcp_common.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_common.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,102 @@
+/*********************************************************************
+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 <stdint.h>
+
+//minimum size is 576, cfr RFC
+#define DHCPC_DATAGRAM_SIZE 576
+#define DHCPD_DATAGRAM_SIZE 576
+
+
+#define PICO_DHCPD_PORT (short_be(67))
+#define PICO_DHCP_CLIENT_PORT (short_be(68))
+
+#define PICO_DHCP_OP_REQUEST 1
+#define PICO_DHCP_OP_REPLY 2
+
+#define PICO_HTYPE_ETHER 1
+#define PICO_HLEN_ETHER 6
+
+#define PICO_DHCPD_MAGIC_COOKIE (long_be(0x63825363))
+
+/* DHCP OPTIONS, RFC2132 */
+#define PICO_DHCPOPT_PAD 0x00
+#define PICO_DHCPOPT_NETMASK 0x01
+#define PICO_DHCPOPT_TIME 0x02
+#define PICO_DHCPOPT_ROUTER 0x03
+#define PICO_DHCPOPT_DNS 0x06
+#define PICO_DHCPOPT_HOSTNAME 0x0c
+#define PICO_DHCPOPT_DOMAINNAME 0x0f
+#define PICO_DHCPOPT_MTU 0x1a
+#define PICO_DHCPOPT_BCAST 0x1c
+#define PICO_DHCPOPT_NETBIOSNS 0x2c
+#define PICO_DHCPOPT_NETBIOSSCOPE 0x2f
+
+#define PICO_DHCPOPT_REQIP 0x32
+#define PICO_DHCPOPT_LEASETIME 0x33
+#define PICO_DHCPOPT_OPTIONOVERLOAD 0x34
+#define PICO_DHCPOPT_MSGTYPE 0x35
+#define PICO_DHCPOPT_SERVERID 0x36
+#define PICO_DHCPOPT_PARMLIST 0x37
+#define PICO_DHCPOPT_MAXMSGSIZE 0x39
+#define PICO_DHCPOPT_RENEWALTIME 0x3a
+#define PICO_DHCPOPT_REBINDINGTIME 0x3b
+#define PICO_DHCPOPT_DOMAINSEARCH 0x77
+#define PICO_DHCPOPT_STATICROUTE 0x79
+#define PICO_DHCPOPT_END 0xFF
+
+/* DHCP MESSAGE TYPE */
+#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
+
+
+enum dhcp_negotiation_state {
+ DHCPSTATE_DISCOVER = 0,
+ DHCPSTATE_OFFER,
+ DHCPSTATE_REQUEST,
+ DHCPSTATE_BOUND,
+ DHCPSTATE_RENEWING
+};
+
+
+struct __attribute__((packed)) pico_dhcphdr
+{
+ 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];
+};
+
+
+//common functions for client and server
+
+uint8_t dhcp_get_next_option(uint8_t *begin, uint8_t *data, int *len, uint8_t **nextopt);
+int is_options_valid(uint8_t *opt_buffer, int len);
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dhcp_server.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_server.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,339 @@
+/*********************************************************************
+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_stack.h"
+#include "pico_config.h"
+#include "pico_addressing.h"
+#include "pico_socket.h"
+#include "pico_arp.h"
+#include <stdlib.h>
+
+# define dhcpd_dbg(...) do{}while(0)
+//# define dhcpd_dbg dbg
+
+#define dhcpd_make_offer(x) dhcpd_make_reply(x, PICO_DHCP_MSG_OFFER)
+#define dhcpd_make_ack(x) dhcpd_make_reply(x, PICO_DHCP_MSG_ACK)
+#define ip_inrange(x) ((long_be(x) >= long_be(dn->settings->pool_start)) && (long_be(x) <= long_be(dn->settings->pool_end)))
+
+static int dhcp_settings_cmp(void *ka, void *kb)
+{
+ struct pico_dhcpd_settings *a = ka, *b = kb;
+ if (a->dev < b->dev)
+ return -1;
+ else if (a->dev > b->dev)
+ return 1;
+ else
+ return 0;
+}
+PICO_TREE_DECLARE(DHCPSettings, dhcp_settings_cmp);
+
+static int dhcp_negotiations_cmp(void *ka, void *kb)
+{
+ struct pico_dhcp_negotiation *a = ka, *b = kb;
+ if (a->xid < b->xid)
+ return -1;
+ else if (a->xid > b->xid)
+ return 1;
+ else
+ return 0;
+}
+PICO_TREE_DECLARE(DHCPNegotiations, dhcp_negotiations_cmp);
+
+static struct pico_dhcp_negotiation *get_negotiation_by_xid(uint32_t xid)
+{
+ struct pico_dhcp_negotiation test = { }, *neg = NULL;
+
+ test.xid = xid;
+ neg = pico_tree_findKey(&DHCPNegotiations, &test);
+ if (!neg)
+ return NULL;
+ else
+ return neg;
+}
+
+static void dhcpd_make_reply(struct pico_dhcp_negotiation *dn, uint8_t reply_type)
+{
+ uint8_t buf_out[DHCPD_DATAGRAM_SIZE] = {0};
+ struct pico_dhcphdr *dh_out = (struct pico_dhcphdr *) buf_out;
+ struct pico_ip4 destination = { };
+ uint32_t bcast = dn->settings->my_ip.addr | ~(dn->settings->netmask.addr);
+ uint32_t dns_server = OPENDNS;
+ uint16_t port = PICO_DHCP_CLIENT_PORT;
+ int sent = 0;
+
+ memcpy(dh_out->hwaddr, dn->eth.addr, PICO_HLEN_ETHER);
+ dh_out->op = PICO_DHCP_OP_REPLY;
+ dh_out->htype = PICO_HTYPE_ETHER;
+ dh_out->hlen = PICO_HLEN_ETHER;
+ dh_out->xid = dn->xid;
+ dh_out->yiaddr = dn->ipv4.addr;
+ dh_out->siaddr = dn->settings->my_ip.addr;
+ dh_out->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
+
+ /* Option: msg type, len 1 */
+ dh_out->options[0] = PICO_DHCPOPT_MSGTYPE;
+ dh_out->options[1] = 1;
+ dh_out->options[2] = reply_type;
+
+ /* Option: server id, len 4 */
+ dh_out->options[3] = PICO_DHCPOPT_SERVERID;
+ dh_out->options[4] = 4;
+ memcpy(dh_out->options + 5, &dn->settings->my_ip.addr, 4);
+
+ /* Option: Lease time, len 4 */
+ dh_out->options[9] = PICO_DHCPOPT_LEASETIME;
+ dh_out->options[10] = 4;
+ memcpy(dh_out->options + 11, &dn->settings->lease_time, 4);
+
+ /* Option: Netmask, len 4 */
+ dh_out->options[15] = PICO_DHCPOPT_NETMASK;
+ dh_out->options[16] = 4;
+ memcpy(dh_out->options + 17, &dn->settings->netmask.addr, 4);
+
+ /* Option: Router, len 4 */
+ dh_out->options[21] = PICO_DHCPOPT_ROUTER;
+ dh_out->options[22] = 4;
+ memcpy(dh_out->options + 23, &dn->settings->my_ip.addr, 4);
+
+ /* Option: Broadcast, len 4 */
+ dh_out->options[27] = PICO_DHCPOPT_BCAST;
+ dh_out->options[28] = 4;
+ memcpy(dh_out->options + 29, &bcast, 4);
+
+ /* Option: DNS, len 4 */
+ dh_out->options[33] = PICO_DHCPOPT_DNS;
+ dh_out->options[34] = 4;
+ memcpy(dh_out->options + 35, &dns_server, 4);
+
+ dh_out->options[40] = PICO_DHCPOPT_END;
+
+ destination.addr = dh_out->yiaddr;
+
+ sent = pico_socket_sendto(dn->settings->s, buf_out, DHCPD_DATAGRAM_SIZE, &destination, port);
+ if (sent < 0) {
+ dhcpd_dbg("DHCPD: sendto failed with code %d!\n", pico_err);
+ }
+}
+
+static void dhcp_recv(struct pico_socket *s, uint8_t *buffer, int len)
+{
+ struct pico_dhcphdr *dhdr = (struct pico_dhcphdr *) buffer;
+ struct pico_dhcp_negotiation *dn = get_negotiation_by_xid(dhdr->xid);
+ struct pico_ip4* ipv4 = NULL;
+ struct pico_dhcpd_settings test, *settings = NULL;
+ uint8_t *nextopt, opt_data[20], opt_type;
+ int opt_len = 20;
+ uint8_t msg_type;
+ uint32_t msg_reqIP = 0;
+ uint32_t msg_servID = 0;
+
+ if (!is_options_valid(dhdr->options, len - sizeof(struct pico_dhcphdr))) {
+ dhcpd_dbg("DHCPD WARNING: invalid options in dhcp message\n");
+ return;
+ }
+
+ if (!dn) {
+ dn = pico_zalloc(sizeof(struct pico_dhcp_negotiation));
+ if (!dn) {
+ pico_err = PICO_ERR_ENOMEM;
+ return;
+ }
+ dn->xid = dhdr->xid;
+ dn->state = DHCPSTATE_DISCOVER;
+ memcpy(dn->eth.addr, dhdr->hwaddr, PICO_HLEN_ETHER);
+
+ test.dev = pico_ipv4_link_find(&s->local_addr.ip4);
+ settings = pico_tree_findKey(&DHCPSettings, &test);
+ if (settings) {
+ dn->settings = settings;
+ } else {
+ dhcpd_dbg("DHCPD WARNING: received DHCP message on unconfigured link %s\n", test.dev->name);
+ pico_free(dn);
+ return;
+ }
+
+ ipv4 = pico_arp_reverse_lookup(&dn->eth);
+ if (!ipv4) {
+ dn->ipv4.addr = settings->pool_next;
+ pico_arp_create_entry(dn->eth.addr, dn->ipv4, settings->dev);
+ settings->pool_next = long_be(long_be(settings->pool_next) + 1);
+ } else {
+ dn->ipv4.addr = ipv4->addr;
+ }
+
+ if (pico_tree_insert(&DHCPNegotiations, dn)) {
+ dhcpd_dbg("DHCPD WARNING: tried creating new negotation for existing xid %u\n", dn->xid);
+ pico_free(dn);
+ return; /* Element key already exists */
+ }
+ }
+
+ if (!ip_inrange(dn->ipv4.addr))
+ return;
+
+ opt_type = dhcp_get_next_option(dhdr->options, opt_data, &opt_len, &nextopt);
+ while (opt_type != PICO_DHCPOPT_END) {
+ /* parse interesting options here */
+ //dhcpd_dbg("DHCPD sever: opt_type %x, opt_data[0]%d\n", opt_type, opt_data[0]);
+ switch(opt_type){
+ case PICO_DHCPOPT_MSGTYPE:
+ msg_type = opt_data[0];
+ break;
+ case PICO_DHCPOPT_REQIP:
+ //dhcpd_dbg("DHCPD sever: opt_type %x, opt_len%d\n", opt_type, opt_len);
+ if( opt_len == 4)
+ {
+ msg_reqIP = ( opt_data[0] << 24 );
+ msg_reqIP |= ( opt_data[1] << 16 );
+ msg_reqIP |= ( opt_data[2] << 8 );
+ msg_reqIP |= ( opt_data[3] );
+ //dhcpd_dbg("DHCPD sever: msg_reqIP %x, opt_data[0] %x,[1] %x,[2] %x,[3] %x\n", msg_reqIP, opt_data[0],opt_data[1],opt_data[2],opt_data[3]);
+ };
+ break;
+ case PICO_DHCPOPT_SERVERID:
+ //dhcpd_dbg("DHCPD sever: opt_type %x, opt_len%d\n", opt_type, opt_len);
+ if( opt_len == 4)
+ {
+ msg_servID = ( opt_data[0] << 24 );
+ msg_servID |= ( opt_data[1] << 16 );
+ msg_servID |= ( opt_data[2] << 8 );
+ msg_servID |= ( opt_data[3] );
+ //dhcpd_dbg("DHCPD sever: msg_servID %x, opt_data[0] %x,[1] %x,[2] %x,[3] %x\n", msg_servID, opt_data[0],opt_data[1],opt_data[2],opt_data[3]);
+ };
+ break;
+ default:
+ break;
+ }
+
+ opt_len = 20;
+ opt_type = dhcp_get_next_option(NULL, opt_data, &opt_len, &nextopt);
+ }
+
+ //dhcpd_dbg("DHCPD sever: msg_type %d, dn->state %d\n", msg_type, dn->state);
+ //dhcpd_dbg("DHCPD sever: msg_reqIP %x, dn->msg_servID %x\n", msg_reqIP, msg_servID);
+ //dhcpd_dbg("DHCPD sever: dhdr->ciaddr %x, dhdr->yiaddr %x, dn->ipv4.addr %x\n", dhdr->ciaddr,dhdr->yiaddr,dn->ipv4.addr);
+
+ if (msg_type == PICO_DHCP_MSG_DISCOVER)
+ {
+ dhcpd_make_offer(dn);
+ dn->state = DHCPSTATE_OFFER;
+ return;
+ }
+ else if ((msg_type == PICO_DHCP_MSG_REQUEST)&&( dn->state == DHCPSTATE_OFFER))
+ {
+ dhcpd_make_ack(dn);
+ dn->state = DHCPSTATE_BOUND;
+ return;
+ }
+ else if ((msg_type == PICO_DHCP_MSG_REQUEST)&&( dn->state == DHCPSTATE_BOUND))
+ {
+ if( ( msg_servID == 0 )
+ &&( msg_reqIP == 0 )
+ &&( dhdr->ciaddr == dn->ipv4.addr)
+ )
+ {
+ dhcpd_make_ack(dn);
+ return;
+ }
+ }
+}
+
+static void pico_dhcpd_wakeup(uint16_t ev, struct pico_socket *s)
+{
+ uint8_t buf[DHCPD_DATAGRAM_SIZE] = { };
+ int r = 0;
+ uint32_t peer = 0;
+ uint16_t port = 0;
+
+ dhcpd_dbg("DHCPD: called dhcpd_wakeup\n");
+ if (ev == PICO_SOCK_EV_RD) {
+ do {
+ r = pico_socket_recvfrom(s, buf, DHCPD_DATAGRAM_SIZE, &peer, &port);
+ if (r > 0 && port == PICO_DHCP_CLIENT_PORT) {
+ dhcp_recv(s, buf, r);
+ }
+ } while(r>0);
+ }
+}
+
+int pico_dhcp_server_initiate(struct pico_dhcpd_settings *setting)
+{
+ struct pico_dhcpd_settings *settings = NULL;
+ struct pico_ipv4_link *link = NULL;
+ uint16_t port = PICO_DHCPD_PORT;
+
+ if (!setting) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ if (!setting->my_ip.addr) {
+ pico_err = PICO_ERR_EINVAL;
+ dhcpd_dbg("DHCPD: IP address of interface was not supplied\n");
+ return -1;
+ }
+
+ link = pico_ipv4_link_get(&setting->my_ip);
+ if (!link) {
+ pico_err = PICO_ERR_EINVAL;
+ dhcpd_dbg("DHCPD: no link with IP %X found\n", setting->my_ip.addr);
+ return -1;
+ }
+
+ settings = pico_zalloc(sizeof(struct pico_dhcpd_settings));
+ if (!settings) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ memcpy(settings, setting, sizeof(struct pico_dhcpd_settings));
+
+ settings->dev = link->dev;
+ dhcpd_dbg("DHCPD: configuring DHCP server for link %s\n", link->dev->name);
+ settings->my_ip.addr = link->address.addr;
+ dhcpd_dbg("DHCPD: using server addr %X\n", long_be(settings->my_ip.addr));
+ settings->netmask.addr = link->netmask.addr;
+ dhcpd_dbg("DHCPD: using netmask %X\n", long_be(settings->netmask.addr));
+
+ /* default values if not provided */
+ if (settings->pool_start == 0)
+ settings->pool_start = (settings->my_ip.addr & settings->netmask.addr) | POOL_START;
+ dhcpd_dbg("DHCPD: using pool_start %X\n", long_be(settings->pool_start));
+ if (settings->pool_end == 0)
+ settings->pool_end = (settings->my_ip.addr & settings->netmask.addr) | POOL_END;
+ dhcpd_dbg("DHCPD: using pool_end %x\n", long_be(settings->pool_end));
+ if (settings->lease_time == 0)
+ settings->lease_time = LEASE_TIME;
+ dhcpd_dbg("DHCPD: using lease time %x\n", long_be(settings->lease_time));
+ settings->pool_next = settings->pool_start;
+
+ settings->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcpd_wakeup);
+ if (!settings->s) {
+ dhcpd_dbg("DHCP: could not open client socket\n");
+ pico_free(settings);
+ return -1;
+ }
+ if (pico_socket_bind(settings->s, &settings->my_ip, &port) != 0) {
+ dhcpd_dbg("DHCP: could not bind server socket (%s)\n", strerror(pico_err));
+ pico_free(settings);
+ return -1;
+ }
+
+ if (pico_tree_insert(&DHCPSettings, settings)) {
+ dhcpd_dbg("DHCPD ERROR: link %s already configured\n", link->dev->name);
+ pico_err = PICO_ERR_EINVAL;
+ pico_free(settings);
+ return -1; /* Element key already exists */
+ }
+ dhcpd_dbg("DHCPD: configured DHCP server for link %s\n", link->dev->name);
+
+ return 0;
+}
+#endif /* PICO_SUPPORT_DHCP */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dhcp_server.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dhcp_server.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,43 @@
+/*********************************************************************
+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"
+
+/* default configuration */
+#define OPENDNS (long_be(0xd043dede)) /* OpenDNS DNS server 208.67.222.222 */
+#define POOL_START long_be(0x00000064)
+#define POOL_END long_be(0x000000fe)
+#define LEASE_TIME long_be(0x00000078)
+
+struct pico_dhcpd_settings
+{
+ struct pico_device *dev;
+ struct pico_socket *s;
+ struct pico_ip4 my_ip;
+ struct pico_ip4 netmask;
+ uint32_t pool_start;
+ uint32_t pool_next;
+ uint32_t pool_end;
+ uint32_t lease_time;
+ uint8_t flags; /* unused atm */
+};
+
+struct pico_dhcp_negotiation {
+ struct pico_dhcpd_settings *settings;
+ struct pico_ip4 ipv4;
+ struct pico_eth eth;
+ enum dhcp_negotiation_state state;
+ uint32_t xid;
+ uint32_t assigned_address;
+};
+
+/* required settings field: IP address of the interface to serve, only IPs of this network will be served. */
+int pico_dhcp_server_initiate(struct pico_dhcpd_settings *setting);
+
+#endif /* _INCLUDE_PICO_DHCP_SERVER */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dns_client.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_dns_client.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,767 @@
+/*********************************************************************
+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 */
+#define PICO_DNS_NS_GOOGLE "8.8.8.8"
+
+/* Nameserver port */
+#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
+
+/* TTL values */
+#define PICO_DNS_MAX_TTL 604800 /* one week */
+
+/* Header flags */
+#define FLAG_QR(hdr, x) ((hdr)->flags = ((hdr)->flags & ~(0x1 << 15)) | (x << 15))
+#define FLAG_OPCODE(hdr, x) ((hdr)->flags = ((hdr)->flags & ~(0xF << 11)) | (x << 11))
+#define FLAG_AA(hdr, x) ((hdr)->flags = ((hdr)->flags & ~(0x1 << 10)) | (x << 10))
+#define FLAG_TC(hdr, x) ((hdr)->flags = ((hdr)->flags & ~(0x1 << 9)) | (x << 9))
+#define FLAG_RD(hdr, x) ((hdr)->flags = ((hdr)->flags & ~(0x1 << 8)) | (x << 8))
+#define FLAG_RA(hdr, x) ((hdr)->flags = ((hdr)->flags & ~(0x1 << 7)) | (x << 7))
+#define FLAG_Z(hdr, x) ((hdr)->flags = ((hdr)->flags & ~(0x7 << 4)) | (x << 4))
+#define FLAG_RCODE(hdr, x) ((hdr)->flags = ((hdr)->flags & ~(0xF)) | x)
+
+#define GET_FLAG_QR(hdr) ((((hdr)->flags) & (1 << 15)) != 0)
+#define GET_FLAG_OPCODE(hdr) ((((hdr)->flags) & (0xF << 11)) >> 11)
+#define GET_FLAG_AA(hdr) ((((hdr)->flags) & (1 << 10)) != 0)
+#define GET_FLAG_TC(hdr) ((((hdr)->flags) & (1 << 9)) != 0)
+#define GET_FLAG_RD(hdr) ((((hdr)->flags) & (1 << 8)) != 0)
+#define GET_FLAG_RA(hdr) ((((hdr)->flags) & (1 << 7)) != 0)
+#define GET_FLAG_Z(hdr) ((((hdr)->flags) & (0x7 << 4)) >> 4)
+#define GET_FLAG_RCODE(hdr) (((hdr)->flags) & (0x0F))
+
+/* RFC 1025 section 4. MESSAGES */
+struct __attribute__((packed)) dns_message_hdr
+{
+ uint16_t id;
+ uint16_t flags;
+ uint16_t qdcount;
+ uint16_t ancount;
+ uint16_t nscount;
+ uint16_t arcount;
+};
+
+struct __attribute__((packed)) dns_query_suffix
+{
+ /* NAME - domain name to which this resource record pertains */
+ uint16_t qtype;
+ uint16_t qclass;
+};
+
+struct __attribute__((packed)) dns_answer_suffix
+{
+ /* NAME - domain name to which this resource record pertains */
+ uint16_t qtype;
+ uint16_t qclass;
+ uint32_t ttl;
+ uint16_t rdlength;
+ /* RDATA - variable length string of octets that describes the resource */
+};
+
+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 -1;
+ else if (a->ns.addr > b->ns.addr)
+ return 1;
+ else
+ return 0;
+}
+
+PICO_TREE_DECLARE(NSTable,dns_ns_cmp);
+
+int pico_dns_client_nameserver(struct pico_ip4 *ns, uint8_t flag)
+{
+ struct pico_dns_ns test, *key = NULL;
+
+ if (!ns) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ switch (flag)
+ {
+ case PICO_DNS_NS_ADD:
+ key = pico_zalloc(sizeof(struct pico_dns_ns));
+ if (!key) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ key->ns = *ns;
+
+ if(pico_tree_insert(&NSTable,key)){
+ dns_dbg("DNS WARNING: nameserver %08X already added\n",ns->addr);
+ pico_err = PICO_ERR_EINVAL;
+ pico_free(key);
+ return -1; /* Element key already exists */
+ }
+ dns_dbg("DNS: nameserver %08X added\n", ns->addr);
+ /* If default NS found, remove it */
+ pico_string_to_ipv4(PICO_DNS_NS_GOOGLE, &test.ns.addr);
+ if (ns->addr != test.ns.addr) {
+
+ key = pico_tree_findKey(&NSTable,&test);
+ if (key) {
+ if(pico_tree_delete(&NSTable,key)) {
+ dns_dbg("DNS: default nameserver %08X removed\n", test.ns.addr);
+ pico_free(key);
+ } else {
+ pico_err = PICO_ERR_EAGAIN;
+ return -1;
+ }
+ }
+ }
+ break;
+
+ case PICO_DNS_NS_DEL:
+ test.ns = *ns;
+
+ key = pico_tree_findKey(&NSTable,&test);
+ if (!key) {
+ dns_dbg("DNS WARNING: nameserver %08X not found\n", ns->addr);
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ /* RB_REMOVE returns pointer to removed element, NULL to indicate error */
+
+ if(pico_tree_delete(&NSTable,key)) {
+ dns_dbg("DNS: nameserver %08X removed\n",key->ns.addr);
+ pico_free(key);
+ } else {
+ pico_err = PICO_ERR_EAGAIN;
+ return -1;
+ }
+ /* If no NS left, add default NS */
+ if(pico_tree_first(&NSTable) == NULL){
+ dns_dbg("DNS: add default nameserver\n");
+ return pico_dns_client_init();
+ }
+ break;
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ return 0;
+}
+
+int pico_dns_client_init()
+{
+ struct pico_ip4 default_ns;
+ if (pico_string_to_ipv4(PICO_DNS_NS_GOOGLE, &default_ns.addr) != 0) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ return pico_dns_client_nameserver(&default_ns, PICO_DNS_NS_ADD);
+}
+
+struct pico_dns_key
+{
+ char *q_hdr;
+ 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_cmp(void *ka, void *kb)
+{
+ struct pico_dns_key *a = ka,*b = kb;
+ if (a->id < b->id)
+ return -1;
+ else if (a->id > b->id)
+ return 1;
+ else
+ return 0;
+}
+
+PICO_TREE_DECLARE(DNSTable,dns_cmp);
+
+static int pico_dns_client_strlen(const char *url)
+{
+ uint16_t len = 0;
+ int p;
+
+ if (!url)
+ return -1;
+
+ while ((p = *url++) != 0) {
+ len++;
+ }
+ return len;
+}
+
+/* Replace '.' by the label length */
+static int pico_dns_client_label(char *ptr)
+{
+ char *l;
+ uint8_t lbl_len = 0;
+ int p;
+
+ if (!ptr)
+ return -1;
+
+ l = ptr++;
+ while ((p = *ptr++) != 0){
+ if (p == '.') {
+ *l = lbl_len;
+ l = ptr - 1;
+ lbl_len = 0;
+ } else {
+ lbl_len++;
+ }
+ }
+ *l = lbl_len;
+ return 0;
+}
+
+/* Replace the label length by '.' */
+static int pico_dns_client_reverse_label(char *ptr)
+{
+ char *l;
+ int p;
+
+ if(!ptr)
+ return -1;
+
+ l = ptr;
+ while ((p = *ptr++) != 0){
+ ptr += p;
+ *l = '.';
+ l = ptr;
+ }
+ return 0;
+}
+
+/* Seek the end of a string */
+static char *pico_dns_client_seek(char *ptr)
+{
+ int p;
+
+ if (!ptr)
+ return NULL;
+
+ while ((p = *ptr++) != 0);
+
+ return ptr++;
+}
+
+static inline void pico_dns_client_construct_hdr(struct dns_message_hdr *hdr, uint16_t id)
+{
+ hdr->id = short_be(id);
+ FLAG_QR(hdr, PICO_DNS_QR_QUERY);
+ FLAG_OPCODE(hdr, PICO_DNS_OPCODE_QUERY);
+ FLAG_AA(hdr, PICO_DNS_AA_NO_AUTHORITY);
+ FLAG_TC(hdr, PICO_DNS_TC_NO_TRUNCATION);
+ FLAG_RD(hdr, PICO_DNS_RD_IS_DESIRED);
+ FLAG_RA(hdr, PICO_DNS_RA_NO_SUPPORT);
+ FLAG_Z(hdr, 0);
+ FLAG_RCODE(hdr, PICO_DNS_RCODE_NO_ERROR);
+ hdr->flags = short_be(hdr->flags);
+ hdr->qdcount = short_be(1);
+ hdr->ancount = short_be(0);
+ hdr->nscount = short_be(0);
+ hdr->arcount = short_be(0);
+}
+
+static inline void pico_dns_client_hdr_ntoh(struct dns_message_hdr *hdr)
+{
+ hdr->id = short_be(hdr->id);
+ hdr->flags = short_be(hdr->flags);
+ hdr->qdcount = short_be(hdr->qdcount);
+ hdr->ancount = short_be(hdr->ancount);
+ hdr->nscount = short_be(hdr->nscount);
+ hdr->arcount = short_be(hdr->arcount);
+}
+
+
+static int pico_dns_client_mirror(char *ptr)
+{
+ unsigned char buf[4] = {0};
+ char *m;
+ int cnt = 0;
+ int p, i;
+
+ if (!ptr)
+ return -1;
+
+ m = ptr;
+ while ((p = *ptr++) != 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;
+ }
+
+ ptr = m;
+ for(i = 3; i >= 0; i--)
+ {
+ if(buf[i] > 99){
+ *ptr++ = '0' + (buf[i] / 100);
+ *ptr++ = '0' + ((buf[i] % 100) / 10);
+ *ptr++ = '0' + ((buf[i] % 100) % 10);
+ }else if(buf[i] > 9){
+ *ptr++ = '0' + (buf[i] / 10);
+ *ptr++ = '0' + (buf[i] % 10);
+ }else{
+ *ptr++ = '0' + buf[i];
+ }
+ if(i > 0)
+ *ptr++ = '.';
+ }
+
+ return 0;
+}
+
+static struct pico_dns_key *pico_dns_client_idcheck(uint16_t id)
+{
+ struct pico_dns_key test;
+
+ test.id = id;
+ return pico_tree_findKey(&DNSTable,&test);
+}
+
+static void pico_dns_client_callback(uint16_t ev, struct pico_socket *s);
+
+static int pico_dns_client_send(struct pico_dns_key *key)
+{
+ struct pico_socket *s;
+ int w = 0;
+
+ dns_dbg("DNS: sending query to %08X\n", key->q_ns.ns.addr);
+ s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dns_client_callback);
+ if (!s)
+ return -1;
+ key->s = s;
+ if (pico_socket_connect(s, &key->q_ns.ns, short_be(PICO_DNS_NS_PORT)) != 0)
+ return -1;
+ w = pico_socket_send(s, key->q_hdr, key->len);
+ if (w <= 0)
+ return -1;
+
+ return 0;
+}
+
+static void pico_dns_client_retransmission(unsigned long now, void *arg)
+{
+ struct pico_dns_key *key = (struct pico_dns_key *)arg;
+ struct pico_dns_ns *q_ns = NULL;
+
+ if (!key->retrans) {
+ dns_dbg("DNS: no retransmission!\n");
+ pico_free(key->q_hdr);
+
+ if(pico_tree_delete(&DNSTable,key))
+ pico_free(key);
+ }
+ else if (key->retrans <= PICO_DNS_CLIENT_MAX_RETRANS) {
+ key->retrans++;
+ dns_dbg("DNS: retransmission! (%u attempts)\n", key->retrans);
+ // ugly hack
+ q_ns = pico_tree_next(pico_tree_findNode(&NSTable,&key->q_ns))->keyValue;
+ if (q_ns)
+ key->q_ns = *q_ns;
+ else
+ key->q_ns = *((struct pico_dns_ns *)pico_tree_first(&NSTable));
+ pico_dns_client_send(key);
+ pico_timer_add(PICO_DNS_CLIENT_RETRANS, pico_dns_client_retransmission, key);
+ } else {
+ dns_dbg("DNS ERROR: no reply from nameservers! (%u attempts)\n", key->retrans);
+ pico_socket_close(key->s);
+ pico_err = PICO_ERR_EIO;
+ key->callback(NULL, key->arg);
+ pico_free(key->q_hdr);
+ /* RB_REMOVE returns pointer to removed element, NULL to indicate error */
+
+ if(pico_tree_delete(&DNSTable,key))
+ pico_free(key);
+ }
+}
+
+static void pico_dns_client_callback(uint16_t ev, struct pico_socket *s)
+{
+ char *q_qname, *q_suf, *a_hdr, *a_qname, *a_suf, *a_rdata;
+ struct dns_message_hdr *hdr;
+ struct dns_query_suffix query_suf;
+ struct dns_answer_suffix answer_suf;
+ struct pico_dns_key test, *key;
+ char *answer;
+ char dns_answer[PICO_DNS_MAX_RESPONSE_LEN] = {0};
+ uint8_t valid_suffix = 0;
+ uint16_t compression = 0;
+ int i = 0, r = 0;
+
+ if (ev & PICO_SOCK_EV_RD) {
+ r = pico_socket_read(s, dns_answer, PICO_DNS_MAX_RESPONSE_LEN);
+ pico_socket_close(s);
+ if (r == PICO_DNS_MAX_RESPONSE_LEN || r < (int)sizeof(struct dns_message_hdr)) {
+ dns_dbg("DNS ERROR: received incorrect number(%d) of bytes\n", r);
+ return;
+ }
+
+ /* Check header validity */
+ a_hdr = dns_answer;
+ hdr = (struct dns_message_hdr *) a_hdr;
+ pico_dns_client_hdr_ntoh(hdr);
+ if (GET_FLAG_QR(hdr) != PICO_DNS_QR_RESPONSE || GET_FLAG_OPCODE(hdr) != PICO_DNS_OPCODE_QUERY
+ || GET_FLAG_TC(hdr) == PICO_DNS_TC_IS_TRUNCATED || GET_FLAG_RCODE(hdr) != PICO_DNS_RCODE_NO_ERROR) {
+ dns_dbg("DNS ERROR: OPCODE %d | TC %d | RCODE %d\n", GET_FLAG_OPCODE(hdr), GET_FLAG_TC(hdr), GET_FLAG_RCODE(hdr));
+ return;
+ }
+
+ if (hdr->ancount < 1 || r < (int)(sizeof(struct dns_message_hdr) + hdr->qdcount * sizeof(struct dns_query_suffix)
+ + hdr->ancount * sizeof(struct dns_answer_suffix))) {
+ dns_dbg("DNS ERROR: ancount < 1 OR received number(%d) of bytes too low\n", r);
+ return;
+ }
+
+ /* Find DNS key */
+ test.id = hdr->id;
+
+ key = pico_tree_findKey(&DNSTable,&test);
+ if (!key) {
+ dns_dbg("DNS WARNING: key with id %u not found\n", hdr->id);
+ return;
+ }
+ key->retrans = 0;
+
+ /* Check query suffix validity */
+ q_qname = a_hdr + sizeof(struct dns_message_hdr);
+ q_suf = pico_dns_client_seek(q_qname);
+ query_suf = *(struct dns_query_suffix *) q_suf;
+ if (short_be(query_suf.qtype) != key->qtype || short_be(query_suf.qclass) != key->qclass) {
+ dns_dbg("DNS ERROR: received qtype (%u) or qclass (%u) incorrect\n", short_be(query_suf.qtype), short_be(query_suf.qclass));
+ return;
+ }
+
+ /* Seek answer suffix */
+ a_qname = q_suf + sizeof(struct dns_query_suffix);
+ a_suf = a_qname;
+ while(i++ < hdr->ancount) {
+ uint16_t comp_h = short_from(a_suf);
+ compression = short_be(comp_h);
+ switch (compression >> 14)
+ {
+ case PICO_DNS_POINTER:
+ while (compression >> 14 == PICO_DNS_POINTER) {
+ dns_dbg("DNS: pointer\n");
+ a_suf += sizeof(uint16_t);
+ comp_h = short_from(a_suf);
+ compression = short_be(comp_h);
+ }
+ break;
+
+ case PICO_DNS_LABEL:
+ dns_dbg("DNS: label\n");
+ a_suf = pico_dns_client_seek(a_qname);
+ break;
+
+ default:
+ dns_dbg("DNS ERROR: incorrect compression (%u) value\n", compression);
+ return;
+ }
+
+ /* Check answer suffix validity */
+ answer_suf = *(struct dns_answer_suffix *)a_suf;
+ if (short_be(answer_suf.qtype) != key->qtype || short_be(answer_suf.qclass) != key->qclass) {
+ dns_dbg("DNS WARNING: received qtype (%u) or qclass (%u) incorrect\n", short_be(answer_suf.qtype), short_be(answer_suf.qclass));
+ a_suf = a_suf + sizeof(struct dns_answer_suffix) + short_be(answer_suf.rdlength);
+ continue;
+ }
+
+ if (short_be(answer_suf.ttl) > PICO_DNS_MAX_TTL) {
+ dns_dbg("DNS WARNING: received TTL (%u) > MAX (%u)\n", short_be(answer_suf.ttl), PICO_DNS_MAX_TTL);
+ a_suf = a_suf + sizeof(struct dns_answer_suffix) + short_be(answer_suf.rdlength);
+ continue;
+ }
+
+ valid_suffix = 1;
+ break;
+ }
+
+ if (!valid_suffix) {
+ dns_dbg("DNS ERROR: invalid dns answer suffix\n");
+ return;
+ }
+
+ a_rdata = a_suf + sizeof(struct dns_answer_suffix);
+ if (key->qtype == PICO_DNS_TYPE_A) {
+ uint32_t ip_h = long_from(a_rdata);
+ dns_dbg("DNS: length %u | ip %08X\n", short_be(answer_suf.rdlength), long_be(ip_h));
+ answer = pico_zalloc(16);
+ pico_ipv4_to_string(answer, ip_h);
+ key->callback(answer, key->arg);
+ } else if (key->qtype == PICO_DNS_TYPE_PTR) {
+ pico_dns_client_reverse_label((char *) a_rdata);
+ dns_dbg("DNS: length %u | name %s\n", short_be(answer_suf.rdlength), (char *)a_rdata + 1);
+ answer = pico_zalloc(answer_suf.rdlength - 1);
+ memcpy(answer, (char *)a_rdata + 1, short_be(answer_suf.rdlength) - 1);
+ key->callback(answer, key->arg);
+ } else {
+ dns_dbg("DNS ERROR: incorrect qtype (%u)\n", key->qtype);
+ return;
+ }
+ }
+
+ if (ev == PICO_SOCK_EV_ERR) {
+ dns_dbg("DNS: socket error received\n");
+ }
+}
+
+int pico_dns_client_getaddr(const char *url, void (*callback)(char *, void *), void *arg)
+{
+ char *q_hdr, *q_qname, *q_suf;
+ struct dns_message_hdr *hdr;
+ struct dns_query_suffix query_suf;
+ struct pico_dns_key *key;
+ uint16_t url_len = 0;
+ uint16_t id = 0;
+
+ if (!url || !callback) {
+ dns_dbg("DNS ERROR: NULL parameters\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ url_len = pico_dns_client_strlen(url);
+ /* 2 extra bytes for url_len to account for 2 extra label length octets */
+ q_hdr = pico_zalloc(sizeof(struct dns_message_hdr) + (1 + url_len + 1) + sizeof(struct dns_query_suffix));
+ if (!q_hdr) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ q_qname = q_hdr + sizeof(struct dns_message_hdr);
+ q_suf = q_qname + (1 + url_len + 1);
+
+ /* Construct query header */
+ hdr = (struct dns_message_hdr *) q_hdr;
+ do {
+ id = (uint16_t) (pico_rand() & 0xFFFFU);
+ dns_dbg("DNS: generated id %u\n", id);
+ } while (pico_dns_client_idcheck(id));
+ pico_dns_client_construct_hdr(hdr, id);
+ /* Add and manipulate domain name */
+ memcpy(q_qname + 1, url, url_len + 1);
+ pico_dns_client_label(q_qname);
+ /* Add type and class of query */
+ query_suf.qtype = short_be(PICO_DNS_TYPE_A);
+ query_suf.qclass = short_be(PICO_DNS_CLASS_IN);
+ memcpy(q_suf, &query_suf, sizeof(struct dns_query_suffix));
+ /* Create RB entry */
+ key = pico_zalloc(sizeof(struct pico_dns_key));
+ if (!key) {
+ pico_free(q_hdr);
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ key->q_hdr = q_hdr;
+ key->len = sizeof(struct dns_message_hdr) + (1 + url_len + 1) + sizeof(struct dns_query_suffix);
+ key->id = id;
+ key->qtype = PICO_DNS_TYPE_A;
+ key->qclass = PICO_DNS_CLASS_IN;
+ key->retrans = 1;
+
+ key->q_ns = *((struct pico_dns_ns *)pico_tree_first(&NSTable));
+ key->s = NULL;
+ key->callback = callback;
+ key->arg = arg;
+ /* Send query */
+ if (pico_dns_client_send(key) < 0) {
+ pico_free(q_hdr);
+ if (key->s)
+ pico_socket_close(key->s);
+ pico_free(key);
+ pico_err = PICO_ERR_EAGAIN;
+ return -1;
+ }
+ /* Insert RB entry */
+
+ if(pico_tree_insert(&DNSTable,key)) {
+ pico_free(q_hdr);
+ pico_free(key);
+ pico_err = PICO_ERR_EAGAIN;
+ return -1; /* Element key already exists */
+ }
+
+ pico_timer_add(PICO_DNS_CLIENT_RETRANS, pico_dns_client_retransmission, key);
+ return 0;
+}
+
+int pico_dns_client_getname(const char *ip, void (*callback)(char *, void *), void *arg)
+{
+ char *q_hdr, *q_qname, *q_suf;
+ struct dns_message_hdr *hdr;
+ struct dns_query_suffix query_suf;
+ struct pico_dns_key *key;
+ uint16_t ip_len = 0;
+ uint16_t arpa_len = 0;
+ uint16_t id = 0;
+
+ if (!ip || !callback) {
+ dns_dbg("DNS ERROR: NULL parameters\n");
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ ip_len = pico_dns_client_strlen(ip);
+ arpa_len = pico_dns_client_strlen(".in-addr.arpa");
+ /* 2 extra bytes for ip_len and arpa_len to account for 2 extra length octets */
+ q_hdr = pico_zalloc(sizeof(struct dns_message_hdr) + (1 + ip_len + arpa_len + 1) + sizeof(struct dns_query_suffix));
+ if (!q_hdr) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ q_qname = q_hdr + sizeof(struct dns_message_hdr);
+ q_suf = q_qname + (1 + ip_len + arpa_len + 1);
+
+ /* Construct query header */
+ hdr = (struct dns_message_hdr *)q_hdr;
+ do {
+ id = (uint16_t) (pico_rand() & 0xFFFFU);
+ dns_dbg("DNS: generated id %u\n", id);
+ } while (pico_dns_client_idcheck(id));
+ pico_dns_client_construct_hdr(hdr, id);
+ /* Add and manipulate domain name */
+ memcpy(q_qname + 1, ip, ip_len + 1);
+ pico_dns_client_mirror(q_qname + 1);
+ memcpy(q_qname + 1 + ip_len, ".in-addr.arpa", arpa_len);
+ pico_dns_client_label(q_qname);
+ /* Add type and class of query */
+ query_suf.qtype = short_be(PICO_DNS_TYPE_PTR);
+ query_suf.qclass = short_be(PICO_DNS_CLASS_IN);
+ memcpy(q_suf, &query_suf, sizeof(struct dns_query_suffix));
+ /* Create RB entry */
+ key = pico_zalloc(sizeof(struct pico_dns_key));
+ if (!key) {
+ pico_free(q_hdr);
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+ key->q_hdr = q_hdr;
+ key->len = sizeof(struct dns_message_hdr) + (1 + ip_len + arpa_len + 1) + sizeof(struct dns_query_suffix);
+ key->id = id;
+ key->qtype = PICO_DNS_TYPE_PTR;
+ key->qclass = PICO_DNS_CLASS_IN;
+ key->retrans = 1;
+ key->q_ns = *((struct pico_dns_ns *)pico_tree_first(&NSTable));
+ key->s = NULL;
+ key->callback = callback;
+ key->arg = arg;
+ /* Send query */
+ if (pico_dns_client_send(key) < 0) {
+ pico_free(q_hdr);
+ if (key->s)
+ pico_socket_close(key->s);
+ pico_free(key);
+ pico_err = PICO_ERR_EAGAIN;
+ return -1;
+ }
+ /* Insert RB entry */
+
+ if(pico_tree_insert(&DNSTable,key)) {
+ pico_free(q_hdr);
+ pico_free(key);
+ pico_err = PICO_ERR_EAGAIN;
+ return -1; /* Element key already exists */
+ }
+
+ pico_timer_add(PICO_DNS_CLIENT_RETRANS, pico_dns_client_retransmission, key);
+ return 0;
+}
+
+#ifdef PICO_DNS_CLIENT_MAIN
+int main(int argc, char *argv[])
+{
+ dns_dbg(">>>>> DNS GET ADDR\n");
+ pico_dns_client_getaddr("www.google.be");
+ dns_dbg(">>>>> DNS GET NAME\n");
+ pico_dns_client_getname("173.194.67.94");
+
+ return 0;
+}
+#endif /* PICO_DNS_CLIENT_MAIN */
+#endif /* PICO_SUPPORT_DNS_CLIENT */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_dns_client.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_dns_client.h Sat Aug 03 13:16: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 */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_http_client.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_client.c Sat Aug 03 13:16: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 < size-lenRead ? client->header->contentLengthOrChunk : 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 = {};
+ 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;
+ int 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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_http_client.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_client.h Sat Aug 03 13:16: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_ */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_http_server.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_server.c Sat Aug 03 13:16: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 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 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 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 = {};
+
+/*
+ * 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,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 = {};
+
+ 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,returnOkHeader,sizeof(returnOkHeader)-1);//remove \0
+ }
+ else
+ {
+ int length;
+
+ length = pico_socket_write(client->sck,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];
+ int 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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_http_server.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_http_server.h Sat Aug 03 13:16: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_ */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_http_util.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_util.c Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_http_util.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_http_util.h Sat Aug 03 13:16: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_ */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_icmp4.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_icmp4.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,307 @@
+/*********************************************************************
+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 = {};
+static struct pico_queue icmp_out = {};
+
+
+/* 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;
+ 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)
+{
+ 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;
+ 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;
+
+ 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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_icmp4.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_icmp4.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_igmp.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_igmp.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,1120 @@
+/*********************************************************************
+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 = {};
+static struct pico_queue igmp_out = {};
+
+/* 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};
+
+ 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;
+
+ 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 */
+ 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;
+}
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_igmp.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_igmp.h Sat Aug 03 13:16: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 */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_ipfilter.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipfilter.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,267 @@
+/*********************************************************************
+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) {return 0;}
+
+static int fp_priority(struct filter_node *filter, struct pico_frame *f) {
+
+ //TODO do priority-stuff
+ return 0;
+}
+
+static int fp_reject(struct filter_node *filter, struct pico_frame *f) {
+// TODO check first if sender is pico itself or not
+ 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) {
+
+ 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 || action < 0) {
+ 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;
+}
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_ipfilter.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipfilter.h Sat Aug 03 13:16: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 */
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_ipv4.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipv4.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,1418 @@
+/*********************************************************************
+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 = {};
+static struct pico_queue out = {};
+
+/* 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 = (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) & (1 << 31)){
+ 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)
+{
+ 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_isenabled_in(f) == 0) { /* if NAT enabled (dst port registerd), do NAT */
+ if(pico_ipv4_nat(f, hdr->dst) != 0) {
+ return -1;
+ }
+ pico_ipv4_forward(f); /* Local packet became forward packet after NAT */
+ } else { /* no NAT so enqueue to next layer */
+ 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
+ pico_enqueue(pico_proto_udp.q_in, f);
+#endif
+ } else {
+ /* Packet is not local. Try to forward. */
+ if (pico_ipv4_forward(f) != 0) {
+ pico_frame_discard(f);
+ }
+ }
+ return 0;
+}
+
+PICO_TREE_DECLARE(Routes, ipv4_route_compare);
+
+
+static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f)
+{
+ 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);
+ 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(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(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;
+ break;
+
+ 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;
+ break;
+
+ default:
+ return -1;
+ break;
+ }
+ } 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;
+ 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;
+ 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;
+ }
+
+ //dbg("IP> FORWARDING.\n");
+ rt = route_find(&hdr->dst);
+ if (!rt) {
+ pico_notify_dest_unreachable(f);
+ return -1;
+ }
+ //dbg("ROUTE: valid..\n");
+ f->dev = rt->link->dev;
+ hdr->ttl-=1;
+ if (hdr->ttl < 1) {
+ pico_notify_ttl_expired(f);
+ return -1;
+ }
+ hdr->crc++;
+
+ /* check if NAT enbled on link and do NAT if so */
+ if (pico_ipv4_nat_isenabled_out(rt->link) == 0)
+ pico_ipv4_nat(f, rt->link->address);
+
+ //dbg("Routing towards %s\n", f->dev->name);
+ 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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_ipv4.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipv4.h Sat Aug 03 13:16: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(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 */
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_ipv6.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_ipv6.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_nat.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_nat.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,683 @@
+/*********************************************************************
+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 NAT_TCP_TIMEWAIT 240000 /* 4mins (in msec) */
+//#define NAT_TCP_TIMEWAIT 10000 /* 10 sec (in msec) - for testing purposes only*/
+
+
+struct pico_nat_key {
+ struct pico_ip4 pub_addr;
+ uint16_t pub_port;
+ struct pico_ip4 priv_addr;
+ uint16_t priv_port;
+ uint8_t proto;
+ /*
+ del_flags:
+ 1 0
+ 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ |F|B|S|R|P|~| CONNECTION ACTIVE |
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+
+ F: FIN from Forwarding packet
+ B: FIN from Backwarding packet
+ S: SYN
+ R: RST
+ P: Persistant
+
+ */
+ uint16_t del_flags;
+ /* Connector for trees */
+};
+
+static struct pico_ipv4_link pub_link;
+static uint8_t enable_nat_flag = 0;
+
+static int nat_cmp_backward(void * ka, void * kb)
+{
+ struct pico_nat_key *a = ka, *b = kb;
+ if (a->pub_port < b->pub_port) {
+ return -1;
+ }
+ else if (a->pub_port > b->pub_port) {
+ return 1;
+ }
+ else {
+ if (a->proto < b->proto) {
+ return -1;
+ }
+ else if (a->proto > b->proto) {
+ return 1;
+ }
+ else {
+ /* a and b are identical */
+ return 0;
+ }
+ }
+}
+
+static int nat_cmp_forward(void * ka, void * kb)
+{
+ struct pico_nat_key *a =ka, *b = kb;
+ if (a->priv_addr.addr < b->priv_addr.addr) {
+ return -1;
+ }
+ else if (a->priv_addr.addr > b->priv_addr.addr) {
+ return 1;
+ }
+ else {
+ if (a->priv_port < b->priv_port) {
+ return -1;
+ }
+ else if (a->priv_port > b->priv_port) {
+ return 1;
+ }
+ else {
+ if (a->proto < b->proto) {
+ return -1;
+ }
+ else if (a->proto > b->proto) {
+ return 1;
+ }
+ else {
+ /* a and b are identical */
+ return 0;
+ }
+ }
+ }
+}
+
+PICO_TREE_DECLARE(KEYTable_forward,nat_cmp_forward);
+PICO_TREE_DECLARE(KEYTable_backward,nat_cmp_backward);
+
+/*
+ 2 options:
+ find on proto and pub_port
+ find on priv_addr, priv_port and proto
+ zero the unused parameters
+*/
+static struct pico_nat_key *pico_ipv4_nat_find_key(uint16_t pub_port, struct pico_ip4 *priv_addr, uint16_t priv_port, uint8_t proto)
+{
+ struct pico_nat_key test;
+ test.pub_port = pub_port;
+ test.priv_port = priv_port;
+ test.proto = proto;
+ if (priv_addr)
+ test.priv_addr = *priv_addr;
+ else
+ test.priv_addr.addr = 0;
+
+ /* returns NULL if test can not be found */
+ if (!pub_port)
+ return pico_tree_findKey(&KEYTable_forward,&test);
+ else
+ return pico_tree_findKey(&KEYTable_backward, &test);
+}
+
+int pico_ipv4_nat_find(uint16_t pub_port, struct pico_ip4 *priv_addr, uint16_t priv_port, uint8_t proto)
+{
+ struct pico_nat_key *k = NULL;
+
+ k = pico_ipv4_nat_find_key(pub_port, priv_addr, priv_port, proto);
+ if (k)
+ return 0;
+ else
+ return -1;
+}
+
+int pico_ipv4_nat_snif_forward(struct pico_nat_key *nk, struct pico_frame *f)
+{
+ uint8_t proto;
+ struct pico_ipv4_hdr *ipv4_hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ struct pico_tcp_hdr *tcp_hdr;
+
+ if (!ipv4_hdr)
+ return -1;
+ proto = ipv4_hdr->proto;
+
+ if (proto == PICO_PROTO_TCP) {
+ tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ if (!tcp_hdr)
+ return -1;
+ if (tcp_hdr->flags & PICO_TCP_FIN) {
+ nk->del_flags |= PICO_DEL_FLAGS_FIN_FORWARD; //FIN from forwarding packet
+ }
+ if (tcp_hdr->flags & PICO_TCP_SYN) {
+ nk->del_flags |= PICO_DEL_FLAGS_SYN;
+ }
+ if (tcp_hdr->flags & PICO_TCP_RST) {
+ nk->del_flags |= PICO_DEL_FLAGS_RST;
+ }
+ } else if (proto == PICO_PROTO_UDP) {
+ /* set conn active to 1 */
+ nk->del_flags &= 0xFE00;
+ nk->del_flags++;
+ }
+ return 0;
+}
+
+
+int pico_ipv4_nat_snif_backward(struct pico_nat_key *nk, struct pico_frame *f)
+{
+ uint8_t proto;
+ struct pico_ipv4_hdr *ipv4_hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ struct pico_tcp_hdr *tcp_hdr;
+
+ if (!ipv4_hdr)
+ return -1;
+ proto = ipv4_hdr->proto;
+
+ if (proto == PICO_PROTO_TCP) {
+ tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ if (!tcp_hdr)
+ return -1;
+ if (tcp_hdr->flags & PICO_TCP_FIN) {
+ nk->del_flags |= PICO_DEL_FLAGS_FIN_BACKWARD; //FIN from backwarding packet
+ }
+ if (tcp_hdr->flags & PICO_TCP_SYN) {
+ nk->del_flags |= PICO_DEL_FLAGS_SYN;
+ }
+ if (tcp_hdr->flags & PICO_TCP_RST) {
+ nk->del_flags |= PICO_DEL_FLAGS_RST;
+ }
+ } else if (proto == PICO_PROTO_UDP) {
+ /* set conn active to 1 */
+ nk->del_flags &= 0xFE00;
+ nk->del_flags++;
+ }
+ return 0;
+}
+
+void pico_ipv4_nat_table_cleanup(unsigned long now, void *_unused)
+{
+ struct pico_tree_node * idx, * safe;
+ struct pico_nat_key *k = NULL;
+ nat_dbg("NAT: before table cleanup:\n");
+ pico_ipv4_nat_print_table();
+
+ //struct pico_nat_key *tmp;
+ pico_tree_foreach_reverse_safe(idx,&KEYTable_forward,safe){
+ k = idx->keyValue;
+ switch (k->proto)
+ {
+ case PICO_PROTO_TCP:
+ if ((k->del_flags & 0x0800) >> 11) {
+ /* entry is persistant */
+ break;
+ }
+ else if ((k->del_flags & 0x01FF) == 0) {
+ /* conn active is zero, delete entry */
+ pico_ipv4_nat_del(k->pub_port, k->proto);
+ }
+ else if ((k->del_flags & 0x1000) >> 12) {
+ /* RST flag set, set conn active to zero */
+ k->del_flags &= 0xFE00;
+ }
+ else if (((k->del_flags & 0x8000) >> 15) && ((k->del_flags & 0x4000) >> 14)) {
+ /* FIN1 and FIN2 set, set conn active to zero */
+ k->del_flags &= 0xFE00;
+ }
+ else if ((k->del_flags & 0x01FF) > 360) {
+ /* conn is active for 24 hours, delete entry */
+ pico_ipv4_nat_del(k->pub_port, k->proto);
+ }
+ else {
+ k->del_flags++;
+ }
+ break;
+
+ case PICO_PROTO_UDP:
+ if ((k->del_flags & 0x0800) >> 11) {
+ /* entry is persistant */
+ break;
+ }
+ else if ((k->del_flags & 0x01FF) > 1) {
+ /* Delete entry when it has existed NAT_TCP_TIMEWAIT */
+ pico_ipv4_nat_del(k->pub_port, k->proto);
+ }
+ else {
+ k->del_flags++;
+ }
+ break;
+
+ default:
+ /* Unknown protocol in NAT table, delete when it has existed NAT_TCP_TIMEWAIT */
+ if ((k->del_flags & 0x01FF) > 1) {
+ pico_ipv4_nat_del(k->pub_port, k->proto);
+ }
+ else {
+ k->del_flags++;
+ }
+ }
+ }
+
+ nat_dbg("NAT: after table cleanup:\n");
+ pico_ipv4_nat_print_table();
+ pico_timer_add(NAT_TCP_TIMEWAIT, pico_ipv4_nat_table_cleanup, NULL);
+}
+
+int pico_ipv4_nat_add(struct pico_ip4 pub_addr, uint16_t pub_port, struct pico_ip4 priv_addr, uint16_t priv_port, uint8_t proto)
+{
+ struct pico_nat_key *key = pico_zalloc(sizeof(struct pico_nat_key));
+ if (!key) {
+ pico_err = PICO_ERR_ENOMEM;
+ return -1;
+ }
+
+ key->pub_addr = pub_addr;
+ key->pub_port = pub_port;
+ key->priv_addr = priv_addr;
+ key->priv_port = priv_port;
+ key->proto = proto;
+ key->del_flags = 0x0001; /* set conn active to 1, other flags to 0 */
+
+ /* RB_INSERT returns NULL when element added, pointer to the element if already in tree */
+ if(!pico_tree_insert(&KEYTable_forward, key) && !pico_tree_insert(&KEYTable_backward, key)){
+ return 0; /* New element added */
+ }
+ else {
+ pico_free(key);
+ pico_err = PICO_ERR_EINVAL;
+ return -1; /* Element key already exists */
+ }
+}
+
+
+int pico_ipv4_nat_del(uint16_t pub_port, uint8_t proto)
+{
+ struct pico_nat_key *key = NULL;
+ key = pico_ipv4_nat_find_key(pub_port, NULL, 0, proto);
+ if (!key) {
+ nat_dbg("NAT: key to delete not found: proto %u | pub_port %u\n", proto, pub_port);
+ return -1;
+ }
+ else {
+ nat_dbg("NAT: key to delete found: proto %u | pub_port %u\n", proto, pub_port);
+ /* RB_REMOVE returns pointer to removed element, NULL to indicate error */
+ if(pico_tree_delete(&KEYTable_forward, key) && pico_tree_delete(&KEYTable_backward, key))
+ pico_free(key);
+ else
+ return -1; /* Error on removing element, do not free! */
+ }
+ return 0;
+}
+
+int pico_ipv4_port_forward(struct pico_ip4 pub_addr, uint16_t pub_port, struct pico_ip4 priv_addr, uint16_t priv_port, uint8_t proto, uint8_t persistant)
+{
+ struct pico_nat_key *key = NULL;
+
+ switch (persistant)
+ {
+ case PICO_IPV4_FORWARD_ADD:
+ if (pico_ipv4_nat_add(pub_addr, pub_port, priv_addr, priv_port, proto) != 0)
+ return -1; /* pico_err set in nat_add */
+ key = pico_ipv4_nat_find_key(pub_port, &priv_addr, priv_port, proto);
+ if (!key) {
+ pico_err = PICO_ERR_EAGAIN;
+ return -1;
+ }
+ key->del_flags = (key->del_flags & ~(0x1 << 11)) | (persistant << 11);
+ break;
+
+ case PICO_IPV4_FORWARD_DEL:
+ return pico_ipv4_nat_del(pub_port, proto);
+
+ default:
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+ pico_ipv4_nat_print_table();
+ return 0;
+}
+
+
+void pico_ipv4_nat_print_table(void)
+{
+ struct pico_nat_key __attribute__((unused)) *k = NULL ;
+ struct pico_tree_node * index;
+ uint16_t i = 0;
+
+ nat_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+ nat_dbg("+ NAT table +\n");
+ nat_dbg("+-----------------------------------------------------------------------------------------------------------------------+\n");
+ nat_dbg("+ pointer | private_addr | private_port | proto | pub_addr | pub_port | conn active | FIN1 | FIN2 | SYN | RST | PERS +\n");
+ nat_dbg("+-----------------------------------------------------------------------------------------------------------------------+\n");
+
+ pico_tree_foreach(index,&KEYTable_forward){
+ k = index->keyValue;
+ nat_dbg("+ %10p | %08X | %05u | %04u | %08X | %05u | %03u | %u | %u | %u | %u | %u +\n",
+ k, k->priv_addr.addr, k->priv_port, k->proto, k->pub_addr.addr, k->pub_port, (k->del_flags)&0x01FF, ((k->del_flags)&0x8000)>>15,
+ ((k->del_flags)&0x4000)>>14, ((k->del_flags)&0x2000)>>13, ((k->del_flags)&0x1000)>>12, ((k->del_flags)&0x0800)>>11);
+ i++;
+ }
+ nat_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+}
+
+int pico_ipv4_nat_generate_key(struct pico_nat_key* nk, struct pico_frame* f, struct pico_ip4 pub_addr)
+{
+ uint16_t pub_port = 0;
+ uint8_t proto;
+ struct pico_tcp_hdr *tcp_hdr = NULL; /* forced to use pico_trans */
+ struct pico_udp_hdr *udp_hdr = NULL; /* forced to use pico_trans */
+ struct pico_ipv4_hdr *ipv4_hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ if (!ipv4_hdr)
+ return -1;
+ proto = ipv4_hdr->proto;
+ do {
+ /* 1. generate valid new NAT port entry */
+ uint32_t rand = pico_rand();
+ pub_port = (uint16_t) (rand & 0xFFFFU);
+ pub_port = (uint16_t)(pub_port % (65535 - 1024)) + 1024U;
+ pub_port = short_be(pub_port);
+
+ /* 2. check if already in table, if no exit */
+ nat_dbg("NAT: check if generated port %u is free\n", short_be(pub_port));
+ if (pico_is_port_free(proto, pub_port, NULL, &pico_proto_ipv4))
+ break;
+
+ } while (1);
+ nat_dbg("NAT: port %u is free\n", short_be(pub_port));
+
+ if (proto == PICO_PROTO_TCP) {
+ tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ if (!tcp_hdr)
+ return -1;
+ nk->priv_port = tcp_hdr->trans.sport;
+ } else if (proto == PICO_PROTO_UDP) {
+ udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ if (!udp_hdr)
+ return -1;
+ nk->priv_port = udp_hdr->trans.sport;
+ } else if (proto == PICO_PROTO_ICMP4) {
+ nk->priv_port = (uint16_t)(ipv4_hdr->src.addr & 0x00FF);
+ pub_port = (uint16_t)(ipv4_hdr->dst.addr & 0x00FF);
+ if (!pico_is_port_free(proto, pub_port, NULL, &pico_proto_ipv4))
+ return -1;
+ }
+
+ nk->pub_addr = pub_addr; /* get public ip address from device */
+ nk->pub_port = pub_port;
+ nk->priv_addr = ipv4_hdr->src;
+ nk->proto = ipv4_hdr->proto;
+ nk->del_flags = 0x0001; /* set conn active to 1 */
+ if (pico_ipv4_nat_add(nk->pub_addr, nk->pub_port, nk->priv_addr, nk->priv_port, nk->proto) < 0) {
+ return -1;
+ } else {
+ return 0;
+ }
+}
+
+
+static int pico_nat_tcp_checksum(struct pico_frame *f)
+{
+ struct pico_tcp_hdr *trans_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ struct pico_ipv4_hdr *net_hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+
+ struct tcp_pseudo_hdr_ipv4 pseudo;
+ if (!trans_hdr || !net_hdr)
+ return -1;
+
+ pseudo.src.addr = net_hdr->src.addr;
+ pseudo.dst.addr = net_hdr->dst.addr;
+ pseudo.res = 0;
+ pseudo.proto = PICO_PROTO_TCP;
+ pseudo.tcp_len = short_be(f->transport_len);
+
+ trans_hdr->crc = 0;
+ trans_hdr->crc = pico_dualbuffer_checksum(&pseudo, sizeof(struct tcp_pseudo_hdr_ipv4), trans_hdr, f->transport_len);
+ trans_hdr->crc = short_be(trans_hdr->crc);
+ return 0;
+}
+
+
+int pico_ipv4_nat_translate(struct pico_nat_key* nk, struct pico_frame* f)
+{
+ uint8_t proto;
+ struct pico_tcp_hdr *tcp_hdr = NULL; /* forced to use pico_trans */
+ struct pico_udp_hdr *udp_hdr = NULL; /* forced to use pico_trans */
+
+ struct pico_ipv4_hdr* ipv4_hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ if (!ipv4_hdr)
+ return -1;
+ proto = ipv4_hdr->proto;
+
+ if (proto == PICO_PROTO_TCP) {
+ tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ if (!tcp_hdr)
+ return -1;
+ tcp_hdr->trans.sport = nk->pub_port;
+ } else if (proto == PICO_PROTO_UDP) {
+ udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ if (!udp_hdr)
+ return -1;
+ udp_hdr->trans.sport = nk->pub_port;
+ }
+
+ //if(f->proto == PICO_PROTO_ICMP){
+ //} XXX no action
+
+ ipv4_hdr->src = nk->pub_addr;
+
+ if (proto == PICO_PROTO_TCP) {
+ pico_nat_tcp_checksum(f);
+ } else if (proto == PICO_PROTO_UDP){
+ udp_hdr->crc = 0;
+ udp_hdr->crc = short_be(pico_udp_checksum_ipv4(f));
+ }
+
+ // pico_ipv4_checksum(f);
+ ipv4_hdr->crc = 0;
+ ipv4_hdr->crc = short_be(pico_checksum(ipv4_hdr, f->net_len));
+
+ return 0;
+}
+
+
+int pico_ipv4_nat_port_forward(struct pico_frame* f)
+{
+ struct pico_nat_key *nk = NULL;
+ struct pico_tcp_hdr *tcp_hdr = NULL;
+ struct pico_udp_hdr *udp_hdr = NULL;
+ struct pico_icmp4_hdr *icmp_hdr = NULL;
+ struct pico_ipv4_hdr* ipv4_hdr;
+ uint16_t pub_port = 0;
+ uint8_t proto;
+
+ ipv4_hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+ if (!ipv4_hdr)
+ return -1;
+ proto = ipv4_hdr->proto;
+
+ if (proto == PICO_PROTO_TCP) {
+ tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ if (!tcp_hdr)
+ return -1;
+ pub_port = tcp_hdr->trans.dport;
+ } else if (proto == PICO_PROTO_UDP) {
+ udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ if (!udp_hdr)
+ return -1;
+ pub_port = udp_hdr->trans.dport;
+ } else if (proto == PICO_PROTO_ICMP4) {
+ icmp_hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
+ if (!icmp_hdr)
+ return -1;
+ /* XXX PRELIMINARY ONLY LAST 16 BITS OF IP */
+ pub_port = (uint16_t)(ipv4_hdr->src.addr & 0x00FF);
+ }
+
+ nk = pico_ipv4_nat_find_key(pub_port, 0, 0, proto);
+
+ if (!nk) {
+ nat_dbg("\nNAT: ERROR key not found in table\n");
+ return -1;
+ } else {
+ pico_ipv4_nat_snif_forward(nk,f);
+ ipv4_hdr->dst.addr = nk->priv_addr.addr;
+
+ if (proto == PICO_PROTO_TCP) {
+ tcp_hdr->trans.dport = nk->priv_port;
+ pico_nat_tcp_checksum(f);
+ } else if (proto == PICO_PROTO_UDP) {
+ udp_hdr->trans.dport = nk->priv_port;
+ udp_hdr->crc = 0;
+ udp_hdr->crc = short_be(pico_udp_checksum_ipv4(f));
+ }
+ }
+
+ ipv4_hdr->crc = 0;
+ ipv4_hdr->crc = short_be(pico_checksum(ipv4_hdr, f->net_len));
+
+ return 0;
+}
+
+
+
+int pico_ipv4_nat(struct pico_frame *f, struct pico_ip4 pub_addr)
+{
+ /*do nat---------*/
+ struct pico_nat_key *nk = NULL;
+ struct pico_nat_key key;
+ struct pico_ipv4_hdr *net_hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ struct pico_tcp_hdr *tcp_hdr = NULL;
+ struct pico_udp_hdr *udp_hdr = NULL;
+ int ret;
+ uint8_t proto = net_hdr->proto;
+ uint16_t priv_port = 0;
+ struct pico_ip4 priv_addr= net_hdr->src;
+
+ nk= &key;
+
+ /* TODO DELME check if IN */
+ if (pub_addr.addr == net_hdr->dst.addr) {
+ nat_dbg("NAT: backward translation {dst.addr, dport}: {%08X,%u} ->", net_hdr->dst.addr, ((struct pico_trans *)f->transport_hdr)->dport);
+ ret = pico_ipv4_nat_port_forward(f); /* our IN definition */
+ nat_dbg(" {%08X,%u}\n", net_hdr->dst.addr, short_be(((struct pico_trans *)f->transport_hdr)->dport));
+ } else {
+ if (net_hdr->proto == PICO_PROTO_TCP) {
+ tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ priv_port = tcp_hdr->trans.sport;
+ } else if (net_hdr->proto == PICO_PROTO_UDP) {
+ udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ priv_port = udp_hdr->trans.sport;
+ } else if (net_hdr->proto == PICO_PROTO_ICMP4) {
+ //udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ priv_port = (uint16_t)(net_hdr->src.addr & 0x00FF);
+ }
+
+ ret = pico_ipv4_nat_find(0, &priv_addr, priv_port, proto);
+ if (ret >= 0) {
+ // Key is available in table
+ nk = pico_ipv4_nat_find_key(0, &priv_addr, priv_port, proto);
+ } else {
+ nat_dbg("NAT: key not found in NAT table -> generate key\n");
+ pico_ipv4_nat_generate_key(nk, f, pub_addr);
+ }
+ pico_ipv4_nat_snif_backward(nk,f);
+ nat_dbg("NAT: forward translation {src.addr, sport}: {%08X,%u} ->", net_hdr->src.addr, short_be(((struct pico_trans *)f->transport_hdr)->sport));
+ pico_ipv4_nat_translate(nk, f); /* our OUT definition */
+ nat_dbg(" {%08X,%u}\n", net_hdr->src.addr, short_be(((struct pico_trans *)f->transport_hdr)->sport));
+ }
+ return 0;
+}
+
+
+int pico_ipv4_nat_enable(struct pico_ipv4_link *link)
+{
+ if (link == NULL) {
+ pico_err = PICO_ERR_EINVAL;
+ return -1;
+ }
+
+ pub_link = *link;
+ pico_timer_add(NAT_TCP_TIMEWAIT, pico_ipv4_nat_table_cleanup, NULL);
+ enable_nat_flag = 1;
+ return 0;
+}
+
+int pico_ipv4_nat_disable(void)
+{
+ pub_link.address.addr = 0;
+ enable_nat_flag = 0;
+ return 0;
+}
+
+
+int pico_ipv4_nat_isenabled_out(struct pico_ipv4_link *link)
+{
+ if (enable_nat_flag) {
+ // is pub_link = *link
+ if (pub_link.address.addr == link->address.addr)
+ return 0;
+ else
+ return -1;
+ } else {
+ return -1;
+ }
+}
+
+
+int pico_ipv4_nat_isenabled_in(struct pico_frame *f)
+{
+ if (enable_nat_flag) {
+ struct pico_tcp_hdr *tcp_hdr = NULL;
+ struct pico_udp_hdr *udp_hdr = NULL;
+ uint16_t pub_port = 0;
+ int ret;
+ uint8_t proto;
+
+ struct pico_ipv4_hdr *ipv4_hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+ if (!ipv4_hdr)
+ return -1;
+ proto = ipv4_hdr->proto;
+
+ if (proto == PICO_PROTO_TCP) {
+ tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
+ if (!tcp_hdr)
+ return -1;
+ pub_port= tcp_hdr->trans.dport;
+ } else if (proto == PICO_PROTO_UDP) {
+ udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
+ if (!udp_hdr)
+ return -1;
+ pub_port= udp_hdr->trans.dport;
+ } else if (proto == PICO_PROTO_ICMP4) {
+ //icmp_hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
+ //if (!icmp_hdr)
+ // return -1;
+ /* XXX PRELIMINARY ONLY LAST 16 BITS OF IP */
+ pub_port = (uint16_t)(ipv4_hdr->src.addr & 0x00FF);
+ }
+ ret = pico_ipv4_nat_find(pub_port, NULL, 0, proto);
+ if (ret == 0)
+ return 0;
+ else
+ return -1;
+ } else {
+ return -1;
+ }
+}
+#endif
+#endif
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_nat.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_nat.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,88 @@
+/*********************************************************************
+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_DEL_FLAGS_FIN_FORWARD (0x8000)
+#define PICO_DEL_FLAGS_FIN_BACKWARD (0x4000)
+#define PICO_DEL_FLAGS_SYN (0x2000)
+#define PICO_DEL_FLAGS_RST (0x1000)
+
+#define PICO_IPV4_FORWARD_DEL 0
+#define PICO_IPV4_FORWARD_ADD 1
+
+#ifdef PICO_SUPPORT_NAT
+void pico_ipv4_nat_print_table(void);
+int pico_ipv4_nat_add(struct pico_ip4 pub_addr, uint16_t pub_port, struct pico_ip4 priv_addr, uint16_t priv_port, uint8_t proto);
+int pico_ipv4_nat_del(uint16_t pub_port, uint8_t proto);
+int pico_ipv4_nat_find(uint16_t pub_port, struct pico_ip4 *priv_addr, uint16_t priv_port, uint8_t proto);
+int pico_ipv4_port_forward(struct pico_ip4 pub_addr, uint16_t pub_port, struct pico_ip4 priv_addr, uint16_t priv_port, uint8_t proto, uint8_t persistant);
+
+int pico_ipv4_nat(struct pico_frame* f, struct pico_ip4 pub_addr);
+int pico_ipv4_nat_enable(struct pico_ipv4_link *link);
+int pico_ipv4_nat_isenabled_out(struct pico_ipv4_link *link);
+int pico_ipv4_nat_isenabled_in(struct pico_frame *f);
+
+#else
+
+static inline int pico_ipv4_nat_isenabled_out(struct pico_ipv4_link *link)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+static inline int pico_ipv4_nat_isenabled_in(struct pico_frame *f)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_nat(struct pico_frame* f, struct pico_ip4 pub_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;
+}
+
+#define pico_ipv4_nat_print_table() do{}while(0)
+
+static inline int pico_ipv4_nat_add(struct pico_ip4 pub_addr, uint16_t pub_port, struct pico_ip4 priv_addr, uint16_t priv_port, uint8_t proto)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_nat_del(uint16_t pub_port, uint8_t proto)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+
+static inline int pico_ipv4_nat_find(uint16_t pub_port, struct pico_ip4 priv_addr, uint16_t priv_port, uint8_t proto)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+
+static inline int pico_ipv4_port_forward(struct pico_ip4 pub_addr, uint16_t pub_port, struct pico_ip4 priv_addr, uint16_t priv_port, uint8_t proto, uint8_t persistant)
+{
+ pico_err = PICO_ERR_EPROTONOSUPPORT;
+ return -1;
+}
+#endif
+
+#endif /* _INCLUDE_PICO_NAT */
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_simple_http.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_simple_http.c Sat Aug 03 13:16: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
+
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_simple_http.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/modules/pico_simple_http.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_tcp.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_tcp.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,2217 @@
+/*********************************************************************
+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_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_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
+
+
+#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 = {};
+ 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 = {};
+static struct pico_queue tcp_out = {};
+
+/* 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;
+ hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+
+ 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;
+ int 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;
+ int 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->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);
+
+#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) > 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 == 0) || (in_frame_len == 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;
+ 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("\n\nTIMEOUT! 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", 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;
+ int 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\nTCP> 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;
+ 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)
+{
+ 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;
+ 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;
+ 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;
+
+ /* 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) {
+ 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;
+ tcp_dbg("\n\nSending 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);
+ }
+ }
+}
+
+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) {
+ /* 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");
+ }
+ }
+ 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;
+
+ 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 (f->payload_len > (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 {
+ tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue hold failed 2\n");
+ return 0;
+ }
+ }
+ }
+ }
+ /***************************************************************************/
+}
+#endif //PICO_SUPPORT_TCP
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_tcp.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_tcp.h Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,98 @@
+/*********************************************************************
+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);
+
+#endif
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_udp.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_udp.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,176 @@
+/*********************************************************************
+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 = {};
+static struct pico_queue udp_out = {};
+
+
+/* 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)
+{
+ 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;
+}
+
diff -r 759afa79ebe8 -r 540f6e142d59 modules/pico_udp.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_udp.h Sat Aug 03 13:16: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
diff -r 759afa79ebe8 -r 540f6e142d59 stack/pico_arp.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_arp.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,317 @@
+/*********************************************************************
+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);
+ 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;
+ 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;
+}
diff -r 759afa79ebe8 -r 540f6e142d59 stack/pico_device.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_device.c Sat Aug 03 13:16: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, 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(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;
+}
diff -r 759afa79ebe8 -r 540f6e142d59 stack/pico_frame.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_frame.c Sat Aug 03 13:16: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 - (int)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) ;
+}
+
+
diff -r 759afa79ebe8 -r 540f6e142d59 stack/pico_protocol.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_protocol.c Sat Aug 03 13:16: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);
+
+}
+
diff -r 759afa79ebe8 -r 540f6e142d59 stack/pico_socket.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_socket.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,2271 @@
+/*********************************************************************
+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)
+
+
+#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) == 0) {
+ 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;
+}
+
+
+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_garbage_collect(unsigned long now, void *arg)
+{
+ struct pico_socket *s = (struct pico_socket *) arg;
+ 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 {
+ uint8_t filter_mode = 0;
+ 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);
+ 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, 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, void *buf, int len, void *dst, uint16_t remote_port)
+{
+ struct pico_frame *f;
+ struct pico_remote_duple *remote_duple = NULL;
+ 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)) {
+ 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
+
+#ifdef PICO_SUPPORT_IPV6
+ if (IS_SOCK_IPV6(s)) {
+ 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 > PICO_SOCKET_MTU)
+ transport_len = PICO_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) > PICO_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, 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, 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)) {
+ struct pico_ip6 *ip = (struct pico_ip6 *) remote_addr;
+ memcpy(s->remote_addr.ip6.addr, ip, PICO_SIZE_IP6);
+ } else if (is_sock_ipv4(s)) {
+ struct pico_ip4 *local, *ip = (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) {
+ uint8_t filter_mode = 0;
+ 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) {
+ uint8_t 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) {
+ uint8_t 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) {
+ uint8_t 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) {
+ uint8_t 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) {
+ uint8_t 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, PICO_SOCKET_STATE_BOUND, 0, 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)
+{
+ 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;
+ pico_tree_foreach(index, &sp_tcp->socks){
+ 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;
+ }
+ }
+
+ /* check if RB_FOREACH ended, if not, break to keep the cur sp_tcp */
+ if (s != NULL)
+ 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
diff -r 759afa79ebe8 -r 540f6e142d59 stack/pico_stack.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_stack.c Sat Aug 03 13:16: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 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)
+{
+ 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 = (struct pico_eth *) 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();
+}
+
diff -r 759afa79ebe8 -r 540f6e142d59 stack/pico_tree.c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stack/pico_tree.c Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,486 @@
+/*********************************************************************
+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"
+
+#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);
+
+ // 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;
+ 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)
+ {
+ 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;
+}
