add LwIP network stack

This commit is contained in:
yudongdong 2021-05-29 13:43:31 +08:00
parent 302079ab81
commit a3cb3fd650
330 changed files with 138375 additions and 4 deletions

View File

@ -98,6 +98,11 @@ if BSP_USING_USB
source "$BSP_DIR/third_party_driver/usb/Kconfig"
endif
menuconfig BSP_USING_LWIP
bool "Using LwIP device"
default n
select RESOURCES_LWIP
menuconfig BSP_USING_WDT
bool "Using WATCHDOG device"
default y

View File

@ -50,4 +50,8 @@ ifeq ($(CONFIG_BSP_USING_WDT),y)
SRC_DIR += watchdog
endif
ifeq ($(CONFIG_BSP_USING_LWIP),y)
SRC_DIR += ethernet
endif
include $(KERNEL_ROOT)/compiler.mk

View File

@ -0,0 +1,12 @@
config CAN_BUS_NAME_1
string "can bus name"
default "can1"
config CAN_DRIVER_NAME
string "can driver name"
default "can1_drv"
config CAN_1_DEVICE_NAME_1
string "can bus 1 device 1 name"
default "can1_dev1"

View File

@ -0,0 +1,4 @@
SRC_FILES := hardware_ethernet.c connect_ethernet.c ethernetif.c
include $(KERNEL_ROOT)/compiler.mk

View File

@ -0,0 +1,363 @@
/**
******************************************************************************
* @file stm32f4x7_eth_bsp.c
* @author MCD Application Team
* @version V1.0.0
* @date 31-October-2011
* @brief STM32F4x7 Ethernet hardware configuration.
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
/*
* Copyright (c) 2020 AIIT XUOS Lab
* XiUOS is licensed under Mulan PSL v2.
* You can use this software according to the terms and conditions of the Mulan PSL v2.
* You may obtain a copy of Mulan PSL v2 at:
* http://license.coscl.org.cn/MulanPSL2
* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
* See the Mulan PSL v2 for more details.
*/
/**
* @file connect_ethernet.c
* @brief Adapted network software protocol stack and hardware operation functions
* @version 1.0
* @author AIIT XUOS Lab
* @date 2021-05-29
*/
/* Includes ------------------------------------------------------------------*/
#include "hardware_syscfg.h"
#include "hardware_gpio.h"
#include "hardware_rcc.h"
#include "misc.h"
#include "hardware_exti.h"
#include "hardware_eth.h"
#include "connect_ethernet.h"
#include <xs_base.h>
#include <xs_sem.h>
#include <xs_isr.h>
__IO uint32_t EthInitStatus = 0;
__IO uint8_t EthLinkStatus = 0;
/* Private function prototypes -----------------------------------------------*/
static void ETH_GPIO_Config(void);
static void ETH_MACDMA_Config(void);
extern int32 s_xSemaphore;
/* Private functions ---------------------------------------------------------*/
/**
* @brief ETH_BSP_Config
* @param None
* @retval None
*/
void ETH_BSP_Config(void)
{
RCC_ClocksTypeDef RCC_Clocks;
/* Configure the GPIO ports for ethernet pins */
ETH_GPIO_Config();
/* Configure the Ethernet MAC/DMA */
ETH_MACDMA_Config();
if (EthInitStatus == 0)
{
while(1);
}
/* Configure Systick clock source as HCLK */
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);
/* SystTick configuration: an interrupt every 10ms */
RCC_GetClocksFreq(&RCC_Clocks);
SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);
}
/**
* @brief Configures the Ethernet Interface
* @param None
* @retval None
*/
static void ETH_MACDMA_Config(void)
{
ETH_InitTypeDef ETH_InitStructure;
/* Enable ETHERNET clock */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_ETH_MAC | RCC_AHB1Periph_ETH_MAC_Tx |
RCC_AHB1Periph_ETH_MAC_Rx, ENABLE);
/* Reset ETHERNET on AHB Bus */
ETH_DeInit();
/* Software reset */
ETH_SoftwareReset();
/* Wait for software reset */
while (ETH_GetSoftwareResetStatus() == SET);
/* ETHERNET Configuration --------------------------------------------------*/
/* Call ETH_StructInit if you don't like to configure all ETH_InitStructure parameter */
ETH_StructInit(&ETH_InitStructure);
/* Fill ETH_InitStructure parametrs */
/*------------------------ MAC -----------------------------------*/
ETH_InitStructure.ETH_AutoNegotiation = ETH_AutoNegotiation_Enable;
ETH_InitStructure.ETH_LoopbackMode = ETH_LoopbackMode_Disable;
ETH_InitStructure.ETH_RetryTransmission = ETH_RetryTransmission_Disable;
ETH_InitStructure.ETH_AutomaticPadCRCStrip = ETH_AutomaticPadCRCStrip_Disable;
ETH_InitStructure.ETH_ReceiveAll = ETH_ReceiveAll_Disable;
ETH_InitStructure.ETH_BroadcastFramesReception = ETH_BroadcastFramesReception_Enable;
ETH_InitStructure.ETH_PromiscuousMode = ETH_PromiscuousMode_Disable;
ETH_InitStructure.ETH_MulticastFramesFilter = ETH_MulticastFramesFilter_Perfect;
ETH_InitStructure.ETH_UnicastFramesFilter = ETH_UnicastFramesFilter_Perfect;
#ifdef CHECKSUM_BY_HARDWARE
ETH_InitStructure.ETH_ChecksumOffload = ETH_ChecksumOffload_Enable;
#endif
/*------------------------ DMA -----------------------------------*/
/* When we use the Checksum offload feature, we need to enable the Store and Forward mode:
the store and forward guarantee that a whole frame is stored in the FIFO, so the MAC can insert/verify the checksum,
if the checksum is OK the DMA can handle the frame otherwise the frame is dropped */
ETH_InitStructure.ETH_DropTCPIPChecksumErrorFrame = ETH_DropTCPIPChecksumErrorFrame_Enable;
ETH_InitStructure.ETH_ReceiveStoreForward = ETH_ReceiveStoreForward_Enable;
ETH_InitStructure.ETH_TransmitStoreForward = ETH_TransmitStoreForward_Enable;
ETH_InitStructure.ETH_ForwardErrorFrames = ETH_ForwardErrorFrames_Disable;
ETH_InitStructure.ETH_ForwardUndersizedGoodFrames = ETH_ForwardUndersizedGoodFrames_Disable;
ETH_InitStructure.ETH_SecondFrameOperate = ETH_SecondFrameOperate_Enable;
ETH_InitStructure.ETH_AddressAlignedBeats = ETH_AddressAlignedBeats_Enable;
ETH_InitStructure.ETH_FixedBurst = ETH_FixedBurst_Enable;
ETH_InitStructure.ETH_RxDMABurstLength = ETH_RxDMABurstLength_32Beat;
ETH_InitStructure.ETH_TxDMABurstLength = ETH_TxDMABurstLength_32Beat;
ETH_InitStructure.ETH_DMAArbitration = ETH_DMAArbitration_RoundRobin_RxTx_2_1;
/* Configure Ethernet */
EthInitStatus = ETH_Init(&ETH_InitStructure, DP83848_PHY_ADDRESS);
}
/**
* @brief Configures the different GPIO ports.
* @param None
* @retval None
*/
void ETH_GPIO_Config(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
/* Enable GPIOs clocks */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA |
RCC_AHB1Periph_GPIOB |
RCC_AHB1Periph_GPIOC |
RCC_AHB1Periph_GPIOI |
RCC_AHB1Periph_GPIOG |
RCC_AHB1Periph_GPIOH |
RCC_AHB1Periph_GPIOF, ENABLE);
/* Enable SYSCFG clock */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
/* Configure MCO (PA8) */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/* MII/RMII Media interface selection --------------------------------------*/
#ifdef MII_MODE /* Mode MII with STM324xG-EVAL */
#ifdef PHY_CLOCK_MCO
/* Output HSE clock (25MHz) on MCO pin (PA8) to clock the PHY */
RCC_MCO1Config(RCC_MCO1Source_HSE, RCC_MCO1Div_1);
#endif /* PHY_CLOCK_MCO */
SYSCFG_ETH_MediaInterfaceConfig(SYSCFG_ETH_MediaInterface_MII);
#elif defined RMII_MODE /* Mode RMII with STM324xG-EVAL */
SYSCFG_ETH_MediaInterfaceConfig(SYSCFG_ETH_MediaInterface_RMII);
#endif
/* Ethernet pins configuration ************************************************/
/*
ETH_MDIO -------------------------> PA2
ETH_MDC --------------------------> PC1
ETH_PPS_OUT ----------------------> PB5
ETH_MII_CRS ----------------------> PH2
ETH_MII_COL ----------------------> PH3
ETH_MII_RX_ER --------------------> PI10
ETH_MII_RXD2 ---------------------> PH6
ETH_MII_RXD3 ---------------------> PH7
ETH_MII_TX_CLK -------------------> PC3
ETH_MII_TXD2 ---------------------> PC2
ETH_MII_TXD3 ---------------------> PB8
ETH_MII_RX_CLK/ETH_RMII_REF_CLK---> PA1
ETH_MII_RX_DV/ETH_RMII_CRS_DV ----> PA7
ETH_MII_RXD0/ETH_RMII_RXD0 -------> PC4
ETH_MII_RXD1/ETH_RMII_RXD1 -------> PC5
ETH_MII_TX_EN/ETH_RMII_TX_EN -----> PG11
ETH_MII_TXD0/ETH_RMII_TXD0 -------> PG13
ETH_MII_TXD1/ETH_RMII_TXD1 -------> PG14
*/
/* Configure PA1, PA2 and PA7 */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_7;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_ETH);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_ETH);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_ETH);
/* Configure PB11 and PB12 */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_13;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource11, GPIO_AF_ETH);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource12, GPIO_AF_ETH);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource13, GPIO_AF_ETH);
/* Configure PC1, PC2, PC3, PC4 and PC5 */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_4 | GPIO_Pin_5;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource1, GPIO_AF_ETH);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource4, GPIO_AF_ETH);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource5, GPIO_AF_ETH);
}
/**
* @brief Configure the PHY to generate an interrupt on change of link status.
* @param PHYAddress: external PHY address
* @retval None
*/
uint32_t Eth_Link_PHYITConfig(uint16_t PHYAddress)
{
uint32_t tmpreg = 0;
/* Read MICR register */
tmpreg = ETH_ReadPHYRegister(PHYAddress, PHY_MICR);
/* Enable output interrupt events to signal via the INT pin */
tmpreg |= (uint32_t)PHY_MICR_INT_EN | PHY_MICR_INT_OE;
if(!(ETH_WritePHYRegister(PHYAddress, PHY_MICR, tmpreg)))
{
/* Return ERROR in case of write timeout */
return ETH_ERROR;
}
/* Read MISR register */
tmpreg = ETH_ReadPHYRegister(PHYAddress, PHY_MISR);
/* Enable Interrupt on change of link status */
tmpreg |= (uint32_t)PHY_MISR_LINK_INT_EN;
if(!(ETH_WritePHYRegister(PHYAddress, PHY_MISR, tmpreg)))
{
/* Return ERROR in case of write timeout */
return ETH_ERROR;
}
/* Return SUCCESS */
return ETH_SUCCESS;
}
/**
* @brief EXTI configuration for Ethernet link status.
* @param PHYAddress: external PHY address
* @retval None
*/
void Eth_Link_EXTIConfig(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
/* Enable the INT (PB14) Clock */
RCC_AHB1PeriphClockCmd(ETH_LINK_GPIO_CLK, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
/* Configure INT pin as input */
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Pin = ETH_LINK_PIN;
GPIO_Init(ETH_LINK_GPIO_PORT, &GPIO_InitStructure);
/* Connect EXTI Line to INT Pin */
SYSCFG_EXTILineConfig(ETH_LINK_EXTI_PORT_SOURCE, ETH_LINK_EXTI_PIN_SOURCE);
/* Configure EXTI line */
EXTI_InitStructure.EXTI_Line = ETH_LINK_EXTI_LINE;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
/* Enable and set the EXTI interrupt to the highest priority */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
/**
* @brief This function handles Ethernet link status.
* @param None
* @retval None
*/
void Eth_Link_ITHandler(uint16_t PHYAddress)
{
/* Check whether the link interrupt has occurred or not */
if(((ETH_ReadPHYRegister(PHYAddress, PHY_MISR)) & PHY_LINK_STATUS) != 0)
{
EthLinkStatus = ~EthLinkStatus;
}
}
void assert_failed(uint8_t* file, uint32_t line)
{
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* Infinite loop */
while (1)
{}
}
#ifdef BSP_USING_LWIP
void ETHERNET_IRQHandler(int irq_num, void *arg)
{
KPrintf("ethernet irq comes one ...\n");
ETH_DMAClearITPendingBit(ETH_DMA_IT_R);
ETH_DMAClearITPendingBit(ETH_DMA_IT_NIS);
KSemaphoreAbandon(s_xSemaphore);
}
DECLARE_HW_IRQ(ETH_IRQn, ETHERNET_IRQHandler, NONE);
#endif

View File

@ -0,0 +1,445 @@
/**
* @file
* Ethernet Interface Skeleton
*
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
/*
* This file is a skeleton for developing Ethernet network interface
* drivers for lwIP. Add code to the low_level functions and do a
* search-and-replace for the word "ethernetif" to replace it with
* something that better describes your network interface.
*/
#include "lwip/opt.h"
#include "lwip/def.h"
#include "lwip/mem.h"
#include "lwip/pbuf.h"
#include "lwip/sys.h"
#include "netif/etharp.h"
#include "err.h"
#include "ethernetif.h"
#include "main.h"
#include <string.h>
#include <hardware_eth.h>
#include <xs_sem.h>
#include <xs_ktask.h>
#include <priv/tcp_priv.h>
#define netifMTU (1500)
#define netifINTERFACE_TASK_STACK_SIZE ( 2048 )
#define netifINTERFACE_TASK_PRIORITY ( configMAX_PRIORITIES - 1 )
#define netifGUARD_BLOCK_TIME ( 250 )
/* The time to block waiting for input. */
#define emacBLOCK_TIME_WAITING_FOR_INPUT ( ( portTickType ) 100 )
/* Define those to better describe your network interface. */
#define IFNAME0 's'
#define IFNAME1 't'
/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */
#define MAC_ADDR0 2
#define MAC_ADDR1 0
#define MAC_ADDR2 0
#define MAC_ADDR3 0
#define MAC_ADDR4 0
#define MAC_ADDR5 0
static struct netif *s_pxNetIf = NULL;
int32 s_xSemaphore = -1;
/* Ethernet Rx & Tx DMA Descriptors */
extern ETH_DMADESCTypeDef DMARxDscrTab[ETH_RXBUFNB], DMATxDscrTab[ETH_TXBUFNB];
/* Ethernet Receive buffers */
extern uint8_t Rx_Buff[ETH_RXBUFNB][ETH_RX_BUF_SIZE];
/* Ethernet Transmit buffers */
extern uint8_t Tx_Buff[ETH_TXBUFNB][ETH_TX_BUF_SIZE];
/* Global pointers to track current transmit and receive descriptors */
extern ETH_DMADESCTypeDef *DMATxDescToSet;
extern ETH_DMADESCTypeDef *DMARxDescToGet;
/* Global pointer for last received frame infos */
extern ETH_DMA_Rx_Frame_infos *DMA_RX_FRAME_infos;
void ethernetif_input( void * pvParameters );
void LoopGetMacPkg(void * pvParameters);
static void arp_timer(void *arg);
uint32_t TCPTimer = 0;
uint32_t ARPTimer = 0;
__IO uint32_t LocalTime = 0; /* this variable is used to create a time reference incremented by 10ms */
void LwIP_Periodic_Handle(__IO uint32_t localtime)
{
#if LWIP_TCP
/* TCP periodic process every 250 ms */
if (localtime - TCPTimer >= TCP_TMR_INTERVAL)
{
TCPTimer = localtime;
tcp_tmr();
}
#endif
/* ARP periodic process every 5s */
if ((localtime - ARPTimer) >= ARP_TMR_INTERVAL)
{
ARPTimer = localtime;
etharp_tmr();
}
}
void LwIP_Pkt_Handle(struct netif *netif)
{
/* Read a received packet from the Ethernet buffers and send it to the lwIP for handling */
ethernetif_input(netif);
}
void Time_Update_LwIP(void)
{
LocalTime += MS_PER_SYSTICK_F407;
}
/**
* In this function, the hardware should be initialized.
* Called from ethernetif_init().
*
* @param netif the already initialized lwip network interface structure
* for this ethernetif
*/
static void low_level_init(struct netif *netif)
{
uint32_t i;
/* set netif MAC hardware address length */
netif->hwaddr_len = ETHARP_HWADDR_LEN;
/* set netif MAC hardware address */
netif->hwaddr[0] = MAC_ADDR0;
netif->hwaddr[1] = MAC_ADDR1;
netif->hwaddr[2] = MAC_ADDR2;
netif->hwaddr[3] = MAC_ADDR3;
netif->hwaddr[4] = MAC_ADDR4;
netif->hwaddr[5] = MAC_ADDR5;
/* set netif maximum transfer unit */
netif->mtu = 1500;
/* Accept broadcast address and ARP traffic */
netif->flags = NETIF_FLAG_BROADCAST | NETIF_FLAG_ETHARP | NETIF_FLAG_LINK_UP;
s_pxNetIf =netif;
/* create binary semaphore used for informing ethernetif of frame reception */
if (s_xSemaphore < 0)
{
s_xSemaphore = KSemaphoreCreate(0);
}
/* initialize MAC address in ethernet MAC */
ETH_MACAddressConfig(ETH_MAC_Address0, netif->hwaddr);
/* Initialize Tx Descriptors list: Chain Mode */
ETH_DMATxDescChainInit(DMATxDscrTab, &Tx_Buff[0][0], ETH_TXBUFNB);
/* Initialize Rx Descriptors list: Chain Mode */
ETH_DMARxDescChainInit(DMARxDscrTab, &Rx_Buff[0][0], ETH_RXBUFNB);
/* Enable MAC and DMA transmission and reception */
ETH_Start();
/* Enable Ethernet Rx interrrupt */
{
for(i=0; i<ETH_RXBUFNB; i++)
{
ETH_DMARxDescReceiveITConfig(&DMARxDscrTab[i], ENABLE);
}
}
#ifdef CHECKSUM_BY_HARDWARE
/* Enable the checksum insertion for the Tx frames */
{
for(i=0; i<ETH_TXBUFNB; i++)
{
ETH_DMATxDescChecksumInsertionConfig(&DMATxDscrTab[i], ETH_DMATxDesc_ChecksumTCPUDPICMPFull);
}
}
#endif
/* create the task that handles the ETH_MAC */
uint32 thr_id = KTaskCreate((signed char*) "Eth_if",
ethernetif_input,
NULL,
netifINTERFACE_TASK_STACK_SIZE,
15);
if (thr_id >= 0)
{
StartupKTask(thr_id);
}
else
{
KPrintf("Eth create failed !");
}
}
/**
* This function should do the actual transmission of the packet. The packet is
* contained in the pbuf that is passed to the function. This pbuf
* might be chained.
*
* @param netif the lwip network interface structure for this ethernetif
* @param p the MAC packet to send (e.g. IP packet including MAC addresses and type)
* @return ERR_OK if the packet could be sent
* an err_t value if the packet couldn't be sent
*
* @note Returning ERR_MEM here if a DMA queue of your MAC is full can lead to
* strange results. You might consider waiting for space in the DMA queue
* to become availale since the stack doesn't retry to send a packet
* dropped because of memory failure (except for the TCP timers).
*/
static err_t low_level_output(struct netif *netif, struct pbuf *p)
{
static int32 sem = -1;
struct pbuf *q;
uint32_t l = 0;
u8 *buffer ;
if(sem < 0)
{
sem = KSemaphoreCreate(1);
}
KSemaphoreObtain(sem, WAITING_FOREVER);
buffer = (u8 *)(DMATxDescToSet->Buffer1Addr);
for(q = p; q != NULL; q = q->next)
{
memcpy((u8_t*)&buffer[l], q->payload, q->len);
l = l + q->len;
}
ETH_Prepare_Transmit_Descriptors(l);
KSemaphoreAbandon(sem);
return ERR_OK;
}
/**
* Should allocate a pbuf and transfer the bytes of the incoming
* packet from the interface into the pbuf.
*
* @param netif the lwip network interface structure for this ethernetif
* @return a pbuf filled with the received packet (including MAC header)
* NULL on memory error
*/
static struct pbuf * low_level_input(struct netif *netif)
{
struct pbuf *p, *q;
u16_t len;
uint32_t l=0,i =0;
FrameTypeDef frame;
u8 *buffer;
__IO ETH_DMADESCTypeDef *DMARxNextDesc;
p = NULL;
/* Get received frame */
frame = ETH_Get_Received_Frame_interrupt();
/* check that frame has no error */
if ((frame.descriptor->Status & ETH_DMARxDesc_ES) == (uint32_t)RESET)
{
/* Obtain the size of the packet and put it into the "len" variable. */
len = frame.length;
buffer = (u8 *)frame.buffer;
/* We allocate a pbuf chain of pbufs from the pool. */
p = pbuf_alloc(PBUF_RAW, len, PBUF_POOL);
/* Copy received frame from ethernet driver buffer to stack buffer */
if (p != NULL)
{
for (q = p; q != NULL; q = q->next)
{
memcpy((u8_t*)q->payload, (u8_t*)&buffer[l], q->len);
l = l + q->len;
}
}
}
/* Release descriptors to DMA */
/* Check if received frame with multiple DMA buffer segments */
if (DMA_RX_FRAME_infos->Seg_Count > 1)
{
DMARxNextDesc = DMA_RX_FRAME_infos->FS_Rx_Desc;
}
else
{
DMARxNextDesc = frame.descriptor;
}
/* Set Own bit in Rx descriptors: gives the buffers back to DMA */
for (i=0; i<DMA_RX_FRAME_infos->Seg_Count; i++)
{
DMARxNextDesc->Status = ETH_DMARxDesc_OWN;
DMARxNextDesc = (ETH_DMADESCTypeDef *)(DMARxNextDesc->Buffer2NextDescAddr);
}
/* Clear Segment_Count */
DMA_RX_FRAME_infos->Seg_Count =0;
/* When Rx Buffer unavailable flag is set: clear it and resume reception */
if ((ETH->DMASR & ETH_DMASR_RBUS) != (u32)RESET)
{
/* Clear RBUS ETHERNET DMA flag */
ETH->DMASR = ETH_DMASR_RBUS;
/* Resume DMA reception */
ETH->DMARPDR = 0;
}
return p;
}
/**
* This function is the ethernetif_input task, it is processed when a packet
* is ready to be read from the interface. It uses the function low_level_input()
* that should handle the actual reception of bytes from the network
* interface. Then the type of the received packet is determined and
* the appropriate input function is called.
*
* @param netif the lwip network interface structure for this ethernetif
*/
void ethernetif_input( void * pvParameters )
{
struct pbuf *p;
for( ;; )
{
if (KSemaphoreObtain( s_xSemaphore, WAITING_FOREVER)==EOK)
{
p = low_level_input( s_pxNetIf );
if (ERR_OK != s_pxNetIf->input( p, s_pxNetIf))
{
KPrintf("netif input return not OK ! \n");
pbuf_free(p);
p=NULL;
}
}
}
}
void LoopGetMacPkg(void * pvParameters)
{
// Loop Get MAX Pkg
struct netif *netif = (struct netif *)pvParameters;
while (1)
{
/* check if any packet received */
if (ETH_CheckFrameReceived())
{
KPrintf("ETH_CheckFrameReceived invoke !\n");
/* process received ethernet packet */
LwIP_Pkt_Handle(netif);
}
/* handle periodic timers for LwIP */
LwIP_Periodic_Handle(LocalTime);
}
}
/**
* Should be called at the beginning of the program to set up the
* network interface. It calls the function low_level_init() to do the
* actual setup of the hardware.
*
* This function should be passed as a parameter to netif_add().
*
* @param netif the lwip network interface structure for this ethernetif
* @return ERR_OK if the loopif is initialized
* ERR_MEM if private data couldn't be allocated
* any other err_t on error
*/
err_t ethernetif_init(struct netif *netif)
{
LWIP_ASSERT("netif != NULL", (netif != NULL));
#if LWIP_NETIF_HOSTNAME
/* Initialize interface hostname */
netif->hostname = "lwip";
#endif /* LWIP_NETIF_HOSTNAME */
netif->name[0] = IFNAME0;
netif->name[1] = IFNAME1;
netif->output = etharp_output;
netif->linkoutput = low_level_output;
/* initialize the hardware */
low_level_init(netif);
etharp_init();
sys_timeout(ARP_TMR_INTERVAL, arp_timer, NULL);
return ERR_OK;
}
static void arp_timer(void *arg)
{
etharp_tmr();
sys_timeout(ARP_TMR_INTERVAL, arp_timer, NULL);
}

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,112 @@
/**
******************************************************************************
* @file connect_ethernet.h
* @author MCD Application Team
* @version V1.0.0
* @date 31-October-2011
* @brief STM32F4x7 Ethernet hardware configuration.
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
/*
* Copyright (c) 2020 AIIT XUOS Lab
* XiUOS is licensed under Mulan PSL v2.
* You can use this software according to the terms and conditions of the Mulan PSL v2.
* You may obtain a copy of Mulan PSL v2 at:
* http://license.coscl.org.cn/MulanPSL2
* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
* See the Mulan PSL v2 for more details.
*/
/**
* @file connect_ethernet.h
* @brief Adapted network software protocol stack and hardware operation functions
* @version 1.0
* @author AIIT XUOS Lab
* @date 2021-05-29
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F4x7_ETH_BSP_H
#define __STM32F4x7_ETH_BSP_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
#define DP83848_PHY_ADDRESS 0x01 /* Relative to STM324xG-EVAL Board */
/* Specific defines for EXTI line, used to manage Ethernet link status */
#define ETH_LINK_EXTI_LINE EXTI_Line14
#define ETH_LINK_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB
#define ETH_LINK_EXTI_PIN_SOURCE EXTI_PinSource14
#define ETH_LINK_EXTI_IRQn EXTI15_10_IRQn
/* PB14 */
#define ETH_LINK_PIN GPIO_Pin_14
#define ETH_LINK_GPIO_PORT GPIOB
#define ETH_LINK_GPIO_CLK RCC_AHB1Periph_GPIOB
/* PHY registers */
#define PHY_MICR 0x11 /* MII Interrupt Control Register */
#define PHY_MICR_INT_EN ((unsigned short)0x0002) /* PHY Enable interrupts */
#define PHY_MICR_INT_OE ((unsigned short)0x0001) /* PHY Enable output interrupt events */
#define PHY_MISR 0x12 /* MII Interrupt Status and Misc. Control Register */
#define PHY_MISR_LINK_INT_EN ((unsigned short)0x0020) /* Enable Interrupt on change of link status */
#define PHY_LINK_STATUS ((unsigned short)0x2000) /* PHY link status interrupt mask */
#define RMII_MODE // User have to provide the 50 MHz clock by soldering a 50 MHz
// oscillator (ref SM7745HEV-50.0M or equivalent) on the U3
// footprint located under CN3 and also removing jumper on JP5.
// This oscillator is not provided with the board.
// For more details, please refer to STM3240G-EVAL evaluation
// board User manual (UM1461).
//#define MII_MODE
/* Uncomment the define below to clock the PHY from external 25MHz crystal (only for MII mode) */
#ifdef MII_MODE
#define PHY_CLOCK_MCO
#endif
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
void ETH_BSP_Config(void);
uint32_t Eth_Link_PHYITConfig(uint16_t PHYAddress);
void Eth_Link_EXTIConfig(void);
void Eth_Link_ITHandler(unsigned short PHYAddress);
#ifdef __cplusplus
}
#endif
#endif /* __STM32F4x7_ETH_BSP_H */
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,91 @@
/**
******************************************************************************
* @file hardware_eth_conf.h
* @author MCD Application Team
* @version V1.0.0
* @date 31-October-2011
* @brief Configuration file for the STM32F4x7 Ethernet driver.
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F4x7_ETH_CONF_H
#define __STM32F4x7_ETH_CONF_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include <stm32f4xx.h>
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Uncomment the line below when using time stamping and/or IPv4 checksum offload */
#define USE_ENHANCED_DMA_DESCRIPTORS
/* Uncomment the line below if you want to use user defined Delay function
(for precise timing), otherwise default _eth_delay_ function defined within
the Ethernet driver is used (less precise timing) */
//#define USE_Delay
#ifdef USE_Delay
#include "main.h" /* Header file where the Delay function prototype is exported */
#define _eth_delay_ Delay /* User can provide more timing precise _eth_delay_ function */
#else
#define _eth_delay_ ETH_Delay /* Default _eth_delay_ function with less precise timing */
#endif
/* Uncomment the line below to allow custom configuration of the Ethernet driver buffers */
#define CUSTOM_DRIVER_BUFFERS_CONFIG
#ifdef CUSTOM_DRIVER_BUFFERS_CONFIG
/* Redefinition of the Ethernet driver buffers size and count */
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
#define ETH_RXBUFNB 4 /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
#define ETH_TXBUFNB 2 /* 2 Tx buffers of size ETH_TX_BUF_SIZE */
#endif
/* PHY configuration section **************************************************/
/* PHY Reset delay */
#define PHY_RESET_DELAY ((uint32_t)0x000FFFFF)
/* PHY Configuration delay */
#define PHY_CONFIG_DELAY ((uint32_t)0x00FFFFFF)
/* The PHY status register value change from a PHY to another, so the user have
to update this value depending on the used external PHY */
#define PHY_SR ((uint16_t)16) /* Value for DP83848 PHY */
/* The Speed and Duplex mask values change from a PHY to another, so the user
have to update this value depending on the used external PHY */
#define PHY_SPEED_STATUS ((uint16_t)0x0002) /* Value for DP83848 PHY */
#define PHY_DUPLEX_STATUS ((uint16_t)0x0004) /* Value for DP83848 PHY */
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
#ifdef __cplusplus
}
#endif
#endif /* __STM32F4x7_ETH_CONF_H */
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/

View File

@ -236,12 +236,12 @@ x_err_t _MdelayKTask(KTaskDescriptorType task, uint32 ms)
*/
x_err_t _KTaskPrioSet(KTaskDescriptorType task, uint8 prio)
{
extern long ShowTask(void);
x_base lock = 0;
int ret = EOK;
uint8 task_stat = 0;
NULL_PARAM_CHECK(task);
lock = CriticalAreaLock();
if (0 == strncmp("ktaskidle",task->task_base_info.name, strlen("ktaskidle")))
@ -269,11 +269,11 @@ x_err_t _KTaskPrioSet(KTaskDescriptorType task, uint8 prio)
case KTASK_INIT:
case KTASK_SUSPEND:
case KTASK_RUNNING:
task->task_dync_sched_member.cur_prio = prio;
__BitmapSiteMask(task);
break;
task->task_dync_sched_member.cur_prio = prio; KTaskDescriptorType tid;
case KTASK_CLOSE:
ShowTask();
KPrintf("the close stat task is forbidden to change priority.\n");
ret = -ERROR;
break;
default:

View File

@ -23,6 +23,8 @@
#include <xs_spinlock.h>
#include <xs_delay.h>
extern void Time_Update_LwIP(void);
#ifdef ARCH_SMP
static x_ticks_t heartbeat_ticks[CPU_NUMBERS] = {0};
#else
@ -78,6 +80,10 @@ void TickAndTaskTimesliceUpdate(void)
#ifdef KERNEL_SOFTTIMER
CheckTimerList();
#endif
#ifdef BSP_USING_LWIP
Time_Update_LwIP();
#endif
}
/**

View File

@ -33,6 +33,15 @@ KERNELPATHS :=-I$(BSP_ROOT) \
-I$(BSP_ROOT)/third_party_driver/include \
-I$(BSP_ROOT)/third_party_driver/usb/STM32_USB_OTG_Driver/inc \
-I$(KERNEL_ROOT)/include \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP/include \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP/include/compat \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP/include/lwip \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP/include/netif \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP/include/lwip/apps \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP/include/lwip/priv \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP/include/lwip/prot \
-I$(KERNEL_ROOT)/resources/ethernet/LwIP/arch \
-I$(BSP_ROOT)/include #
endif

View File

@ -141,6 +141,12 @@ if BSP_USING_USB
endif
endif
if BSP_USING_LWIP
config RESOURCES_LWIP
bool "Using LwIP bus drivers"
default n
endif
if BSP_USING_WDT
config RESOURCES_WDT
bool "Using Watch Dog bus drivers"

View File

@ -49,6 +49,10 @@ ifeq ($(CONFIG_RESOURCES_USB),y)
SRC_DIR += usb
endif
ifeq ($(CONFIG_RESOURCES_LWIP),y)
SRC_DIR += ethernet
endif
ifeq ($(CONFIG_RESOURCES_WDT),y)
SRC_DIR += watchdog
endif

View File

@ -0,0 +1,15 @@
api/ - The code for the high-level wrapper API. Not needed if
you use the lowel-level call-back/raw API.
apps/ - Higher layer applications that are specifically programmed
with the lwIP low-level raw API.
core/ - The core of the TPC/IP stack; protocol implementations,
memory and buffer management, and the low-level raw API.
include/ - lwIP include files.
netif/ - Generic network interface device drivers are kept here.
For more information on the various subdirectories, check the FILES
file in each directory.

View File

@ -0,0 +1,279 @@
# This file is indended to be included in end-user CMakeLists.txt
# include(/path/to/Filelists.cmake)
# It assumes the variable LWIP_DIR is defined pointing to the
# root path of lwIP sources.
#
# This file is NOT designed (on purpose) to be used as cmake
# subdir via add_subdirectory()
# The intention is to provide greater flexibility to users to
# create their own targets using the *_SRCS variables.
set(LWIP_VERSION_MAJOR "2")
set(LWIP_VERSION_MINOR "1")
set(LWIP_VERSION_REVISION "2")
# LWIP_VERSION_RC is set to LWIP_RC_RELEASE for official releases
# LWIP_VERSION_RC is set to LWIP_RC_DEVELOPMENT for Git versions
# Numbers 1..31 are reserved for release candidates
set(LWIP_VERSION_RC "LWIP_RC_RELEASE")
if ("${LWIP_VERSION_RC}" STREQUAL "LWIP_RC_RELEASE")
set(LWIP_VERSION_STRING
"${LWIP_VERSION_MAJOR}.${LWIP_VERSION_MINOR}.${LWIP_VERSION_REVISION}"
)
elseif ("${LWIP_VERSION_RC}" STREQUAL "LWIP_RC_DEVELOPMENT")
set(LWIP_VERSION_STRING
"${LWIP_VERSION_MAJOR}.${LWIP_VERSION_MINOR}.${LWIP_VERSION_REVISION}.dev"
)
else ("${LWIP_VERSION_RC}" STREQUAL "LWIP_RC_RELEASE")
set(LWIP_VERSION_STRING
"${LWIP_VERSION_MAJOR}.${LWIP_VERSION_MINOR}.${LWIP_VERSION_REVISION}.rc${LWIP_VERSION_RC}"
)
endif ("${LWIP_VERSION_RC}" STREQUAL "LWIP_RC_RELEASE")
# The minimum set of files needed for lwIP.
set(lwipcore_SRCS
${LWIP_DIR}/src/core/init.c
${LWIP_DIR}/src/core/def.c
${LWIP_DIR}/src/core/dns.c
${LWIP_DIR}/src/core/inet_chksum.c
${LWIP_DIR}/src/core/ip.c
${LWIP_DIR}/src/core/mem.c
${LWIP_DIR}/src/core/memp.c
${LWIP_DIR}/src/core/netif.c
${LWIP_DIR}/src/core/pbuf.c
${LWIP_DIR}/src/core/raw.c
${LWIP_DIR}/src/core/stats.c
${LWIP_DIR}/src/core/sys.c
${LWIP_DIR}/src/core/altcp.c
${LWIP_DIR}/src/core/altcp_alloc.c
${LWIP_DIR}/src/core/altcp_tcp.c
${LWIP_DIR}/src/core/tcp.c
${LWIP_DIR}/src/core/tcp_in.c
${LWIP_DIR}/src/core/tcp_out.c
${LWIP_DIR}/src/core/timeouts.c
${LWIP_DIR}/src/core/udp.c
)
set(lwipcore4_SRCS
${LWIP_DIR}/src/core/ipv4/autoip.c
${LWIP_DIR}/src/core/ipv4/dhcp.c
${LWIP_DIR}/src/core/ipv4/etharp.c
${LWIP_DIR}/src/core/ipv4/icmp.c
${LWIP_DIR}/src/core/ipv4/igmp.c
${LWIP_DIR}/src/core/ipv4/ip4_frag.c
${LWIP_DIR}/src/core/ipv4/ip4.c
${LWIP_DIR}/src/core/ipv4/ip4_addr.c
)
set(lwipcore6_SRCS
${LWIP_DIR}/src/core/ipv6/dhcp6.c
${LWIP_DIR}/src/core/ipv6/ethip6.c
${LWIP_DIR}/src/core/ipv6/icmp6.c
${LWIP_DIR}/src/core/ipv6/inet6.c
${LWIP_DIR}/src/core/ipv6/ip6.c
${LWIP_DIR}/src/core/ipv6/ip6_addr.c
${LWIP_DIR}/src/core/ipv6/ip6_frag.c
${LWIP_DIR}/src/core/ipv6/mld6.c
${LWIP_DIR}/src/core/ipv6/nd6.c
)
# APIFILES: The files which implement the sequential and socket APIs.
set(lwipapi_SRCS
${LWIP_DIR}/src/api/api_lib.c
${LWIP_DIR}/src/api/api_msg.c
${LWIP_DIR}/src/api/err.c
${LWIP_DIR}/src/api/if_api.c
${LWIP_DIR}/src/api/netbuf.c
${LWIP_DIR}/src/api/netdb.c
${LWIP_DIR}/src/api/netifapi.c
${LWIP_DIR}/src/api/sockets.c
${LWIP_DIR}/src/api/tcpip.c
)
# Files implementing various generic network interface functions
set(lwipnetif_SRCS
${LWIP_DIR}/src/netif/ethernet.c
${LWIP_DIR}/src/netif/bridgeif.c
${LWIP_DIR}/src/netif/bridgeif_fdb.c
${LWIP_DIR}/src/netif/slipif.c
)
# 6LoWPAN
set(lwipsixlowpan_SRCS
${LWIP_DIR}/src/netif/lowpan6_common.c
${LWIP_DIR}/src/netif/lowpan6.c
${LWIP_DIR}/src/netif/lowpan6_ble.c
${LWIP_DIR}/src/netif/zepif.c
)
# PPP
set(lwipppp_SRCS
${LWIP_DIR}/src/netif/ppp/auth.c
${LWIP_DIR}/src/netif/ppp/ccp.c
${LWIP_DIR}/src/netif/ppp/chap-md5.c
${LWIP_DIR}/src/netif/ppp/chap_ms.c
${LWIP_DIR}/src/netif/ppp/chap-new.c
${LWIP_DIR}/src/netif/ppp/demand.c
${LWIP_DIR}/src/netif/ppp/eap.c
${LWIP_DIR}/src/netif/ppp/ecp.c
${LWIP_DIR}/src/netif/ppp/eui64.c
${LWIP_DIR}/src/netif/ppp/fsm.c
${LWIP_DIR}/src/netif/ppp/ipcp.c
${LWIP_DIR}/src/netif/ppp/ipv6cp.c
${LWIP_DIR}/src/netif/ppp/lcp.c
${LWIP_DIR}/src/netif/ppp/magic.c
${LWIP_DIR}/src/netif/ppp/mppe.c
${LWIP_DIR}/src/netif/ppp/multilink.c
${LWIP_DIR}/src/netif/ppp/ppp.c
${LWIP_DIR}/src/netif/ppp/pppapi.c
${LWIP_DIR}/src/netif/ppp/pppcrypt.c
${LWIP_DIR}/src/netif/ppp/pppoe.c
${LWIP_DIR}/src/netif/ppp/pppol2tp.c
${LWIP_DIR}/src/netif/ppp/pppos.c
${LWIP_DIR}/src/netif/ppp/upap.c
${LWIP_DIR}/src/netif/ppp/utils.c
${LWIP_DIR}/src/netif/ppp/vj.c
${LWIP_DIR}/src/netif/ppp/polarssl/arc4.c
${LWIP_DIR}/src/netif/ppp/polarssl/des.c
${LWIP_DIR}/src/netif/ppp/polarssl/md4.c
${LWIP_DIR}/src/netif/ppp/polarssl/md5.c
${LWIP_DIR}/src/netif/ppp/polarssl/sha1.c
)
# SNMPv3 agent
set(lwipsnmp_SRCS
${LWIP_DIR}/src/apps/snmp/snmp_asn1.c
${LWIP_DIR}/src/apps/snmp/snmp_core.c
${LWIP_DIR}/src/apps/snmp/snmp_mib2.c
${LWIP_DIR}/src/apps/snmp/snmp_mib2_icmp.c
${LWIP_DIR}/src/apps/snmp/snmp_mib2_interfaces.c
${LWIP_DIR}/src/apps/snmp/snmp_mib2_ip.c
${LWIP_DIR}/src/apps/snmp/snmp_mib2_snmp.c
${LWIP_DIR}/src/apps/snmp/snmp_mib2_system.c
${LWIP_DIR}/src/apps/snmp/snmp_mib2_tcp.c
${LWIP_DIR}/src/apps/snmp/snmp_mib2_udp.c
${LWIP_DIR}/src/apps/snmp/snmp_snmpv2_framework.c
${LWIP_DIR}/src/apps/snmp/snmp_snmpv2_usm.c
${LWIP_DIR}/src/apps/snmp/snmp_msg.c
${LWIP_DIR}/src/apps/snmp/snmpv3.c
${LWIP_DIR}/src/apps/snmp/snmp_netconn.c
${LWIP_DIR}/src/apps/snmp/snmp_pbuf_stream.c
${LWIP_DIR}/src/apps/snmp/snmp_raw.c
${LWIP_DIR}/src/apps/snmp/snmp_scalar.c
${LWIP_DIR}/src/apps/snmp/snmp_table.c
${LWIP_DIR}/src/apps/snmp/snmp_threadsync.c
${LWIP_DIR}/src/apps/snmp/snmp_traps.c
)
# HTTP server + client
set(lwiphttp_SRCS
${LWIP_DIR}/src/apps/http/altcp_proxyconnect.c
${LWIP_DIR}/src/apps/http/fs.c
${LWIP_DIR}/src/apps/http/http_client.c
${LWIP_DIR}/src/apps/http/httpd.c
)
# MAKEFSDATA HTTP server host utility
set(lwipmakefsdata_SRCS
${LWIP_DIR}/src/apps/http/makefsdata/makefsdata.c
)
# IPERF server
set(lwipiperf_SRCS
${LWIP_DIR}/src/apps/lwiperf/lwiperf.c
)
# SMTP client
set(lwipsmtp_SRCS
${LWIP_DIR}/src/apps/smtp/smtp.c
)
# SNTP client
set(lwipsntp_SRCS
${LWIP_DIR}/src/apps/sntp/sntp.c
)
# MDNS responder
set(lwipmdns_SRCS
${LWIP_DIR}/src/apps/mdns/mdns.c
)
# NetBIOS name server
set(lwipnetbios_SRCS
${LWIP_DIR}/src/apps/netbiosns/netbiosns.c
)
# TFTP server files
set(lwiptftp_SRCS
${LWIP_DIR}/src/apps/tftp/tftp_server.c
)
# MQTT client files
set(lwipmqtt_SRCS
${LWIP_DIR}/src/apps/mqtt/mqtt.c
)
# ARM MBEDTLS related files of lwIP rep
set(lwipmbedtls_SRCS
${LWIP_DIR}/src/apps/altcp_tls/altcp_tls_mbedtls.c
${LWIP_DIR}/src/apps/altcp_tls/altcp_tls_mbedtls_mem.c
${LWIP_DIR}/src/apps/snmp/snmpv3_mbedtls.c
)
# All LWIP files without apps
set(lwipnoapps_SRCS
${lwipcore_SRCS}
${lwipcore4_SRCS}
${lwipcore6_SRCS}
${lwipapi_SRCS}
${lwipnetif_SRCS}
${lwipsixlowpan_SRCS}
${lwipppp_SRCS}
)
# LWIPAPPFILES: All LWIP APPs
set(lwipallapps_SRCS
${lwipsnmp_SRCS}
${lwiphttp_SRCS}
${lwipiperf_SRCS}
${lwipsmtp_SRCS}
${lwipsntp_SRCS}
${lwipmdns_SRCS}
${lwipnetbios_SRCS}
${lwiptftp_SRCS}
${lwipmqtt_SRCS}
${lwipmbedtls_SRCS}
)
# Generate lwip/init.h (version info)
configure_file(${LWIP_DIR}/src/include/lwip/init.h.cmake.in ${LWIP_DIR}/src/include/lwip/init.h)
# Documentation
set(DOXYGEN_DIR ${LWIP_DIR}/doc/doxygen)
set(DOXYGEN_OUTPUT_DIR output)
set(DOXYGEN_IN ${LWIP_DIR}/doc/doxygen/lwip.Doxyfile.cmake.in)
set(DOXYGEN_OUT ${LWIP_DIR}/doc/doxygen/lwip.Doxyfile)
configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT})
find_package(Doxygen)
if (DOXYGEN_FOUND)
message("Doxygen build started")
add_custom_target(lwipdocs
COMMAND ${CMAKE_COMMAND} -E remove_directory ${DOXYGEN_DIR}/${DOXYGEN_OUTPUT_DIR}/html
COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT}
WORKING_DIRECTORY ${DOXYGEN_DIR}
COMMENT "Generating API documentation with Doxygen"
VERBATIM)
else (DOXYGEN_FOUND)
message("Doxygen needs to be installed to generate the doxygen documentation")
endif (DOXYGEN_FOUND)
# lwIP libraries
add_library(lwipcore EXCLUDE_FROM_ALL ${lwipnoapps_SRCS})
target_compile_options(lwipcore PRIVATE ${LWIP_COMPILER_FLAGS})
target_compile_definitions(lwipcore PRIVATE ${LWIP_DEFINITIONS} ${LWIP_MBEDTLS_DEFINITIONS})
target_include_directories(lwipcore PRIVATE ${LWIP_INCLUDE_DIRS} ${LWIP_MBEDTLS_INCLUDE_DIRS})
add_library(lwipallapps EXCLUDE_FROM_ALL ${lwipallapps_SRCS})
target_compile_options(lwipallapps PRIVATE ${LWIP_COMPILER_FLAGS})
target_compile_definitions(lwipallapps PRIVATE ${LWIP_DEFINITIONS} ${LWIP_MBEDTLS_DEFINITIONS})
target_include_directories(lwipallapps PRIVATE ${LWIP_INCLUDE_DIRS} ${LWIP_MBEDTLS_INCLUDE_DIRS})

View File

@ -0,0 +1,205 @@
#
# Copyright (c) 2001, 2002 Swedish Institute of Computer Science.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification,
# are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
# 3. The name of the author may not be used to endorse or promote products
# derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
# WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
# SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
# EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
# OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
# IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
# OF SUCH DAMAGE.
#
# This file is part of the lwIP TCP/IP stack.
#
# Author: Adam Dunkels <adam@sics.se>
#
# COREFILES, CORE4FILES: The minimum set of files needed for lwIP.
COREFILES=$(LWIPDIR)/core/init.c \
$(LWIPDIR)/core/def.c \
$(LWIPDIR)/core/dns.c \
$(LWIPDIR)/core/inet_chksum.c \
$(LWIPDIR)/core/ip.c \
$(LWIPDIR)/core/mem.c \
$(LWIPDIR)/core/memp.c \
$(LWIPDIR)/core/netif.c \
$(LWIPDIR)/core/pbuf.c \
$(LWIPDIR)/core/raw.c \
$(LWIPDIR)/core/stats.c \
$(LWIPDIR)/core/sys.c \
$(LWIPDIR)/core/altcp.c \
$(LWIPDIR)/core/altcp_alloc.c \
$(LWIPDIR)/core/altcp_tcp.c \
$(LWIPDIR)/core/tcp.c \
$(LWIPDIR)/core/tcp_in.c \
$(LWIPDIR)/core/tcp_out.c \
$(LWIPDIR)/core/timeouts.c \
$(LWIPDIR)/core/udp.c
CORE4FILES=$(LWIPDIR)/core/ipv4/autoip.c \
$(LWIPDIR)/core/ipv4/dhcp.c \
$(LWIPDIR)/core/ipv4/etharp.c \
$(LWIPDIR)/core/ipv4/icmp.c \
$(LWIPDIR)/core/ipv4/igmp.c \
$(LWIPDIR)/core/ipv4/ip4_frag.c \
$(LWIPDIR)/core/ipv4/ip4.c \
$(LWIPDIR)/core/ipv4/ip4_addr.c
CORE6FILES=$(LWIPDIR)/core/ipv6/dhcp6.c \
$(LWIPDIR)/core/ipv6/ethip6.c \
$(LWIPDIR)/core/ipv6/icmp6.c \
$(LWIPDIR)/core/ipv6/inet6.c \
$(LWIPDIR)/core/ipv6/ip6.c \
$(LWIPDIR)/core/ipv6/ip6_addr.c \
$(LWIPDIR)/core/ipv6/ip6_frag.c \
$(LWIPDIR)/core/ipv6/mld6.c \
$(LWIPDIR)/core/ipv6/nd6.c
# APIFILES: The files which implement the sequential and socket APIs.
APIFILES=$(LWIPDIR)/api/api_lib.c \
$(LWIPDIR)/api/api_msg.c \
$(LWIPDIR)/api/err.c \
$(LWIPDIR)/api/if_api.c \
$(LWIPDIR)/api/netbuf.c \
$(LWIPDIR)/api/netdb.c \
$(LWIPDIR)/api/netifapi.c \
$(LWIPDIR)/api/sockets.c \
$(LWIPDIR)/api/tcpip.c
# NETIFFILES: Files implementing various generic network interface functions
NETIFFILES=$(LWIPDIR)/netif/ethernet.c \
$(LWIPDIR)/netif/bridgeif.c \
$(LWIPDIR)/netif/bridgeif_fdb.c \
$(LWIPDIR)/netif/slipif.c
# SIXLOWPAN: 6LoWPAN
SIXLOWPAN=$(LWIPDIR)/netif/lowpan6_common.c \
$(LWIPDIR)/netif/lowpan6.c \
$(LWIPDIR)/netif/lowpan6_ble.c \
$(LWIPDIR)/netif/zepif.c
# PPPFILES: PPP
PPPFILES=$(LWIPDIR)/netif/ppp/auth.c \
$(LWIPDIR)/netif/ppp/ccp.c \
$(LWIPDIR)/netif/ppp/chap-md5.c \
$(LWIPDIR)/netif/ppp/chap_ms.c \
$(LWIPDIR)/netif/ppp/chap-new.c \
$(LWIPDIR)/netif/ppp/demand.c \
$(LWIPDIR)/netif/ppp/eap.c \
$(LWIPDIR)/netif/ppp/ecp.c \
$(LWIPDIR)/netif/ppp/eui64.c \
$(LWIPDIR)/netif/ppp/fsm.c \
$(LWIPDIR)/netif/ppp/ipcp.c \
$(LWIPDIR)/netif/ppp/ipv6cp.c \
$(LWIPDIR)/netif/ppp/lcp.c \
$(LWIPDIR)/netif/ppp/magic.c \
$(LWIPDIR)/netif/ppp/mppe.c \
$(LWIPDIR)/netif/ppp/multilink.c \
$(LWIPDIR)/netif/ppp/ppp.c \
$(LWIPDIR)/netif/ppp/pppapi.c \
$(LWIPDIR)/netif/ppp/pppcrypt.c \
$(LWIPDIR)/netif/ppp/pppoe.c \
$(LWIPDIR)/netif/ppp/pppol2tp.c \
$(LWIPDIR)/netif/ppp/pppos.c \
$(LWIPDIR)/netif/ppp/upap.c \
$(LWIPDIR)/netif/ppp/utils.c \
$(LWIPDIR)/netif/ppp/vj.c \
$(LWIPDIR)/netif/ppp/polarssl/arc4.c \
$(LWIPDIR)/netif/ppp/polarssl/des.c \
$(LWIPDIR)/netif/ppp/polarssl/md4.c \
$(LWIPDIR)/netif/ppp/polarssl/md5.c \
$(LWIPDIR)/netif/ppp/polarssl/sha1.c
# LWIPNOAPPSFILES: All LWIP files without apps
LWIPNOAPPSFILES=$(COREFILES) \
$(CORE4FILES) \
$(CORE6FILES) \
$(APIFILES) \
$(NETIFFILES) \
$(PPPFILES) \
$(SIXLOWPAN)
# SNMPFILES: SNMPv2c agent
SNMPFILES=$(LWIPDIR)/apps/snmp/snmp_asn1.c \
$(LWIPDIR)/apps/snmp/snmp_core.c \
$(LWIPDIR)/apps/snmp/snmp_mib2.c \
$(LWIPDIR)/apps/snmp/snmp_mib2_icmp.c \
$(LWIPDIR)/apps/snmp/snmp_mib2_interfaces.c \
$(LWIPDIR)/apps/snmp/snmp_mib2_ip.c \
$(LWIPDIR)/apps/snmp/snmp_mib2_snmp.c \
$(LWIPDIR)/apps/snmp/snmp_mib2_system.c \
$(LWIPDIR)/apps/snmp/snmp_mib2_tcp.c \
$(LWIPDIR)/apps/snmp/snmp_mib2_udp.c \
$(LWIPDIR)/apps/snmp/snmp_snmpv2_framework.c \
$(LWIPDIR)/apps/snmp/snmp_snmpv2_usm.c \
$(LWIPDIR)/apps/snmp/snmp_msg.c \
$(LWIPDIR)/apps/snmp/snmpv3.c \
$(LWIPDIR)/apps/snmp/snmp_netconn.c \
$(LWIPDIR)/apps/snmp/snmp_pbuf_stream.c \
$(LWIPDIR)/apps/snmp/snmp_raw.c \
$(LWIPDIR)/apps/snmp/snmp_scalar.c \
$(LWIPDIR)/apps/snmp/snmp_table.c \
$(LWIPDIR)/apps/snmp/snmp_threadsync.c \
$(LWIPDIR)/apps/snmp/snmp_traps.c
# HTTPFILES: HTTP server + client
HTTPFILES=$(LWIPDIR)/apps/http/altcp_proxyconnect.c \
$(LWIPDIR)/apps/http/fs.c \
$(LWIPDIR)/apps/http/http_client.c \
$(LWIPDIR)/apps/http/httpd.c
# MAKEFSDATA: MAKEFSDATA HTTP server host utility
MAKEFSDATAFILES=$(LWIPDIR)/apps/http/makefsdata/makefsdata.c
# LWIPERFFILES: IPERF server
LWIPERFFILES=$(LWIPDIR)/apps/lwiperf/lwiperf.c
# SMTPFILES: SMTP client
SMTPFILES=$(LWIPDIR)/apps/smtp/smtp.c
# SNTPFILES: SNTP client
SNTPFILES=$(LWIPDIR)/apps/sntp/sntp.c
# MDNSFILES: MDNS responder
MDNSFILES=$(LWIPDIR)/apps/mdns/mdns.c
# NETBIOSNSFILES: NetBIOS name server
NETBIOSNSFILES=$(LWIPDIR)/apps/netbiosns/netbiosns.c
# TFTPFILES: TFTP server files
TFTPFILES=$(LWIPDIR)/apps/tftp/tftp_server.c
# MQTTFILES: MQTT client files
MQTTFILES=$(LWIPDIR)/apps/mqtt/mqtt.c
# MBEDTLS_FILES: MBEDTLS related files of lwIP rep
MBEDTLS_FILES=$(LWIPDIR)/apps/altcp_tls/altcp_tls_mbedtls.c \
$(LWIPDIR)/apps/altcp_tls/altcp_tls_mbedtls_mem.c \
$(LWIPDIR)/apps/snmp/snmpv3_mbedtls.c
# LWIPAPPFILES: All LWIP APPs
LWIPAPPFILES=$(SNMPFILES) \
$(HTTPFILES) \
$(LWIPERFFILES) \
$(SMTPFILES) \
$(SNTPFILES) \
$(MDNSFILES) \
$(NETBIOSNSFILES) \
$(TFTPFILES) \
$(MQTTFILES) \
$(MBEDTLS_FILES)

View File

@ -0,0 +1,6 @@
SRC_DIR += api
SRC_DIR += arch
SRC_DIR += core
SRC_DIR += netif
include $(KERNEL_ROOT)/compiler.mk

View File

@ -0,0 +1,12 @@
SRC_FILES += api_lib.c \
api_msg.c \
err.c \
if_api.c \
netbuf.c \
netdb.c \
netifapi.c \
sockets.c \
tcpip.c
include $(KERNEL_ROOT)/compiler.mk

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,127 @@
/**
* @file
* Error Management module
*
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#include "lwip/err.h"
#include "lwip/def.h"
#include "lwip/sys.h"
#include "lwip/errno.h"
#if !NO_SYS
//tmp
#define EHOSTUNREACH 113 /* No route to host */
#define EINPROGRESS 115 /* Operation now in progress */
#define EADDRINUSE 98 /* Address already in use */
#define EALREADY 114 /* Operation already in progress */
#define EISCONN 106 /* Transport endpoint is already connected */
#define ENOTCONN 107 /* Transport endpoint is not connected */
#define ECONNABORTED 103 /* Software caused connection abort */
/** Table to quickly map an lwIP error (err_t) to a socket error
* by using -err as an index */
static const int err_to_errno_table[] = {
0, /* ERR_OK 0 No error, everything OK. */
ENOMEM, /* ERR_MEM -1 Out of memory error. */
ENOBUFS, /* ERR_BUF -2 Buffer error. */
EWOULDBLOCK, /* ERR_TIMEOUT -3 Timeout */
EHOSTUNREACH, /* ERR_RTE -4 Routing problem. */
EINPROGRESS, /* ERR_INPROGRESS -5 Operation in progress */
EINVAL, /* ERR_VAL -6 Illegal value. */
EWOULDBLOCK, /* ERR_WOULDBLOCK -7 Operation would block. */
EADDRINUSE, /* ERR_USE -8 Address in use. */
EALREADY, /* ERR_ALREADY -9 Already connecting. */
EISCONN, /* ERR_ISCONN -10 Conn already established.*/
ENOTCONN, /* ERR_CONN -11 Not connected. */
-1, /* ERR_IF -12 Low-level netif error */
ECONNABORTED, /* ERR_ABRT -13 Connection aborted. */
ECONNRESET, /* ERR_RST -14 Connection reset. */
ENOTCONN, /* ERR_CLSD -15 Connection closed. */
EIO /* ERR_ARG -16 Illegal argument. */
};
int
err_to_errno(err_t err)
{
if ((err > 0) || (-err >= (err_t)LWIP_ARRAYSIZE(err_to_errno_table))) {
return EIO;
}
return err_to_errno_table[-err];
}
#endif /* !NO_SYS */
#ifdef LWIP_DEBUG
static const char *err_strerr[] = {
"Ok.", /* ERR_OK 0 */
"Out of memory error.", /* ERR_MEM -1 */
"Buffer error.", /* ERR_BUF -2 */
"Timeout.", /* ERR_TIMEOUT -3 */
"Routing problem.", /* ERR_RTE -4 */
"Operation in progress.", /* ERR_INPROGRESS -5 */
"Illegal value.", /* ERR_VAL -6 */
"Operation would block.", /* ERR_WOULDBLOCK -7 */
"Address in use.", /* ERR_USE -8 */
"Already connecting.", /* ERR_ALREADY -9 */
"Already connected.", /* ERR_ISCONN -10 */
"Not connected.", /* ERR_CONN -11 */
"Low-level netif error.", /* ERR_IF -12 */
"Connection aborted.", /* ERR_ABRT -13 */
"Connection reset.", /* ERR_RST -14 */
"Connection closed.", /* ERR_CLSD -15 */
"Illegal argument." /* ERR_ARG -16 */
};
/**
* Convert an lwip internal error to a string representation.
*
* @param err an lwip internal err_t
* @return a string representation for err
*/
const char *
lwip_strerr(err_t err)
{
if ((err > 0) || (-err >= (err_t)LWIP_ARRAYSIZE(err_strerr))) {
return "Unknown error.";
}
return err_strerr[-err];
}
#endif /* LWIP_DEBUG */

View File

@ -0,0 +1,102 @@
/**
* @file
* Interface Identification APIs from:
* RFC 3493: Basic Socket Interface Extensions for IPv6
* Section 4: Interface Identification
*
* @defgroup if_api Interface Identification API
* @ingroup socket
*/
/*
* Copyright (c) 2017 Joel Cunningham, Garmin International, Inc. <joel.cunningham@garmin.com>
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Joel Cunningham <joel.cunningham@me.com>
*
*/
#include "lwip/opt.h"
#if LWIP_SOCKET
#include "lwip/errno.h"
#include "lwip/if_api.h"
#include "lwip/netifapi.h"
#include "lwip/priv/sockets_priv.h"
/**
* @ingroup if_api
* Maps an interface index to its corresponding name.
* @param ifindex interface index
* @param ifname shall point to a buffer of at least {IF_NAMESIZE} bytes
* @return If ifindex is an interface index, then the function shall return the
* value supplied in ifname, which points to a buffer now containing the interface name.
* Otherwise, the function shall return a NULL pointer.
*/
char *
lwip_if_indextoname(unsigned int ifindex, char *ifname)
{
#if LWIP_NETIF_API
if (ifindex <= 0xff) {
err_t err = netifapi_netif_index_to_name((u8_t)ifindex, ifname);
if (!err && ifname[0] != '\0') {
return ifname;
}
}
#else /* LWIP_NETIF_API */
LWIP_UNUSED_ARG(ifindex);
LWIP_UNUSED_ARG(ifname);
#endif /* LWIP_NETIF_API */
set_errno(ENXIO);
return NULL;
}
/**
* @ingroup if_api
* Returs the interface index corresponding to name ifname.
* @param ifname Interface name
* @return The corresponding index if ifname is the name of an interface;
* otherwise, zero.
*/
unsigned int
lwip_if_nametoindex(const char *ifname)
{
#if LWIP_NETIF_API
err_t err;
u8_t idx;
err = netifapi_netif_name_to_index(ifname, &idx);
if (!err) {
return idx;
}
#else /* LWIP_NETIF_API */
LWIP_UNUSED_ARG(ifname);
#endif /* LWIP_NETIF_API */
return 0; /* invalid index */
}
#endif /* LWIP_SOCKET */

View File

@ -0,0 +1,250 @@
/**
* @file
* Network buffer management
*
* @defgroup netbuf Network buffers
* @ingroup netconn
* Network buffer descriptor for @ref netconn. Based on @ref pbuf internally
* to avoid copying data around.\n
* Buffers must not be shared accross multiple threads, all functions except
* netbuf_new() and netbuf_delete() are not thread-safe.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#include "lwip/opt.h"
#if LWIP_NETCONN /* don't build if not configured for use in lwipopts.h */
#include "lwip/netbuf.h"
#include "lwip/memp.h"
#include <string.h>
/**
* @ingroup netbuf
* Create (allocate) and initialize a new netbuf.
* The netbuf doesn't yet contain a packet buffer!
*
* @return a pointer to a new netbuf
* NULL on lack of memory
*/
struct
netbuf *netbuf_new(void)
{
struct netbuf *buf;
buf = (struct netbuf *)memp_malloc(MEMP_NETBUF);
if (buf != NULL) {
memset(buf, 0, sizeof(struct netbuf));
}
return buf;
}
/**
* @ingroup netbuf
* Deallocate a netbuf allocated by netbuf_new().
*
* @param buf pointer to a netbuf allocated by netbuf_new()
*/
void
netbuf_delete(struct netbuf *buf)
{
if (buf != NULL) {
if (buf->p != NULL) {
pbuf_free(buf->p);
buf->p = buf->ptr = NULL;
}
memp_free(MEMP_NETBUF, buf);
}
}
/**
* @ingroup netbuf
* Allocate memory for a packet buffer for a given netbuf.
*
* @param buf the netbuf for which to allocate a packet buffer
* @param size the size of the packet buffer to allocate
* @return pointer to the allocated memory
* NULL if no memory could be allocated
*/
void *
netbuf_alloc(struct netbuf *buf, u16_t size)
{
LWIP_ERROR("netbuf_alloc: invalid buf", (buf != NULL), return NULL;);
/* Deallocate any previously allocated memory. */
if (buf->p != NULL) {
pbuf_free(buf->p);
}
buf->p = pbuf_alloc(PBUF_TRANSPORT, size, PBUF_RAM);
if (buf->p == NULL) {
return NULL;
}
LWIP_ASSERT("check that first pbuf can hold size",
(buf->p->len >= size));
buf->ptr = buf->p;
return buf->p->payload;
}
/**
* @ingroup netbuf
* Free the packet buffer included in a netbuf
*
* @param buf pointer to the netbuf which contains the packet buffer to free
*/
void
netbuf_free(struct netbuf *buf)
{
LWIP_ERROR("netbuf_free: invalid buf", (buf != NULL), return;);
if (buf->p != NULL) {
pbuf_free(buf->p);
}
buf->p = buf->ptr = NULL;
#if LWIP_CHECKSUM_ON_COPY
buf->flags = 0;
buf->toport_chksum = 0;
#endif /* LWIP_CHECKSUM_ON_COPY */
}
/**
* @ingroup netbuf
* Let a netbuf reference existing (non-volatile) data.
*
* @param buf netbuf which should reference the data
* @param dataptr pointer to the data to reference
* @param size size of the data
* @return ERR_OK if data is referenced
* ERR_MEM if data couldn't be referenced due to lack of memory
*/
err_t
netbuf_ref(struct netbuf *buf, const void *dataptr, u16_t size)
{
LWIP_ERROR("netbuf_ref: invalid buf", (buf != NULL), return ERR_ARG;);
if (buf->p != NULL) {
pbuf_free(buf->p);
}
buf->p = pbuf_alloc(PBUF_TRANSPORT, 0, PBUF_REF);
if (buf->p == NULL) {
buf->ptr = NULL;
return ERR_MEM;
}
((struct pbuf_rom *)buf->p)->payload = dataptr;
buf->p->len = buf->p->tot_len = size;
buf->ptr = buf->p;
return ERR_OK;
}
/**
* @ingroup netbuf
* Chain one netbuf to another (@see pbuf_chain)
*
* @param head the first netbuf
* @param tail netbuf to chain after head, freed by this function, may not be reference after returning
*/
void
netbuf_chain(struct netbuf *head, struct netbuf *tail)
{
LWIP_ERROR("netbuf_chain: invalid head", (head != NULL), return;);
LWIP_ERROR("netbuf_chain: invalid tail", (tail != NULL), return;);
pbuf_cat(head->p, tail->p);
head->ptr = head->p;
memp_free(MEMP_NETBUF, tail);
}
/**
* @ingroup netbuf
* Get the data pointer and length of the data inside a netbuf.
*
* @param buf netbuf to get the data from
* @param dataptr pointer to a void pointer where to store the data pointer
* @param len pointer to an u16_t where the length of the data is stored
* @return ERR_OK if the information was retrieved,
* ERR_BUF on error.
*/
err_t
netbuf_data(struct netbuf *buf, void **dataptr, u16_t *len)
{
LWIP_ERROR("netbuf_data: invalid buf", (buf != NULL), return ERR_ARG;);
LWIP_ERROR("netbuf_data: invalid dataptr", (dataptr != NULL), return ERR_ARG;);
LWIP_ERROR("netbuf_data: invalid len", (len != NULL), return ERR_ARG;);
if (buf->ptr == NULL) {
return ERR_BUF;
}
*dataptr = buf->ptr->payload;
*len = buf->ptr->len;
return ERR_OK;
}
/**
* @ingroup netbuf
* Move the current data pointer of a packet buffer contained in a netbuf
* to the next part.
* The packet buffer itself is not modified.
*
* @param buf the netbuf to modify
* @return -1 if there is no next part
* 1 if moved to the next part but now there is no next part
* 0 if moved to the next part and there are still more parts
*/
s8_t
netbuf_next(struct netbuf *buf)
{
LWIP_ERROR("netbuf_next: invalid buf", (buf != NULL), return -1;);
if (buf->ptr->next == NULL) {
return -1;
}
buf->ptr = buf->ptr->next;
if (buf->ptr->next == NULL) {
return 1;
}
return 0;
}
/**
* @ingroup netbuf
* Move the current data pointer of a packet buffer contained in a netbuf
* to the beginning of the packet.
* The packet buffer itself is not modified.
*
* @param buf the netbuf to modify
*/
void
netbuf_first(struct netbuf *buf)
{
LWIP_ERROR("netbuf_first: invalid buf", (buf != NULL), return;);
buf->ptr = buf->p;
}
#endif /* LWIP_NETCONN */

View File

@ -0,0 +1,414 @@
/**
* @file
* API functions for name resolving
*
* @defgroup netdbapi NETDB API
* @ingroup socket
*/
/*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt
*
*/
#include "lwip/netdb.h"
#if LWIP_DNS && LWIP_SOCKET
#include "lwip/err.h"
#include "lwip/mem.h"
#include "lwip/memp.h"
#include "lwip/ip_addr.h"
#include "lwip/api.h"
#include "lwip/dns.h"
#include <string.h> /* memset */
#include <stdlib.h> /* atoi */
/** helper struct for gethostbyname_r to access the char* buffer */
struct gethostbyname_r_helper {
ip_addr_t *addr_list[2];
ip_addr_t addr;
char *aliases;
};
/** h_errno is exported in netdb.h for access by applications. */
#if LWIP_DNS_API_DECLARE_H_ERRNO
int h_errno;
#endif /* LWIP_DNS_API_DECLARE_H_ERRNO */
/** define "hostent" variables storage: 0 if we use a static (but unprotected)
* set of variables for lwip_gethostbyname, 1 if we use a local storage */
#ifndef LWIP_DNS_API_HOSTENT_STORAGE
#define LWIP_DNS_API_HOSTENT_STORAGE 0
#endif
/** define "hostent" variables storage */
#if LWIP_DNS_API_HOSTENT_STORAGE
#define HOSTENT_STORAGE
#else
#define HOSTENT_STORAGE static
#endif /* LWIP_DNS_API_STATIC_HOSTENT */
/**
* Returns an entry containing addresses of address family AF_INET
* for the host with name name.
* Due to dns_gethostbyname limitations, only one address is returned.
*
* @param name the hostname to resolve
* @return an entry containing addresses of address family AF_INET
* for the host with name name
*/
struct hostent *
lwip_gethostbyname(const char *name)
{
err_t err;
ip_addr_t addr;
/* buffer variables for lwip_gethostbyname() */
HOSTENT_STORAGE struct hostent s_hostent;
HOSTENT_STORAGE char *s_aliases;
HOSTENT_STORAGE ip_addr_t s_hostent_addr;
HOSTENT_STORAGE ip_addr_t *s_phostent_addr[2];
HOSTENT_STORAGE char s_hostname[DNS_MAX_NAME_LENGTH + 1];
/* query host IP address */
err = netconn_gethostbyname(name, &addr);
if (err != ERR_OK) {
LWIP_DEBUGF(DNS_DEBUG, ("lwip_gethostbyname(%s) failed, err=%d\n", name, err));
h_errno = HOST_NOT_FOUND;
return NULL;
}
/* fill hostent */
s_hostent_addr = addr;
s_phostent_addr[0] = &s_hostent_addr;
s_phostent_addr[1] = NULL;
strncpy(s_hostname, name, DNS_MAX_NAME_LENGTH);
s_hostname[DNS_MAX_NAME_LENGTH] = 0;
s_hostent.h_name = s_hostname;
s_aliases = NULL;
s_hostent.h_aliases = &s_aliases;
s_hostent.h_addrtype = AF_INET;
s_hostent.h_length = sizeof(ip_addr_t);
s_hostent.h_addr_list = (char **)&s_phostent_addr;
#if DNS_DEBUG
/* dump hostent */
LWIP_DEBUGF(DNS_DEBUG, ("hostent.h_name == %s\n", s_hostent.h_name));
LWIP_DEBUGF(DNS_DEBUG, ("hostent.h_aliases == %p\n", (void *)s_hostent.h_aliases));
/* h_aliases are always empty */
LWIP_DEBUGF(DNS_DEBUG, ("hostent.h_addrtype == %d\n", s_hostent.h_addrtype));
LWIP_DEBUGF(DNS_DEBUG, ("hostent.h_length == %d\n", s_hostent.h_length));
LWIP_DEBUGF(DNS_DEBUG, ("hostent.h_addr_list == %p\n", (void *)s_hostent.h_addr_list));
if (s_hostent.h_addr_list != NULL) {
u8_t idx;
for (idx = 0; s_hostent.h_addr_list[idx]; idx++) {
LWIP_DEBUGF(DNS_DEBUG, ("hostent.h_addr_list[%i] == %p\n", idx, s_hostent.h_addr_list[idx]));
LWIP_DEBUGF(DNS_DEBUG, ("hostent.h_addr_list[%i]-> == %s\n", idx, ipaddr_ntoa((ip_addr_t *)s_hostent.h_addr_list[idx])));
}
}
#endif /* DNS_DEBUG */
#if LWIP_DNS_API_HOSTENT_STORAGE
/* this function should return the "per-thread" hostent after copy from s_hostent */
return sys_thread_hostent(&s_hostent);
#else
return &s_hostent;
#endif /* LWIP_DNS_API_HOSTENT_STORAGE */
}
/**
* Thread-safe variant of lwip_gethostbyname: instead of using a static
* buffer, this function takes buffer and errno pointers as arguments
* and uses these for the result.
*
* @param name the hostname to resolve
* @param ret pre-allocated struct where to store the result
* @param buf pre-allocated buffer where to store additional data
* @param buflen the size of buf
* @param result pointer to a hostent pointer that is set to ret on success
* and set to zero on error
* @param h_errnop pointer to an int where to store errors (instead of modifying
* the global h_errno)
* @return 0 on success, non-zero on error, additional error information
* is stored in *h_errnop instead of h_errno to be thread-safe
*/
int
lwip_gethostbyname_r(const char *name, struct hostent *ret, char *buf,
size_t buflen, struct hostent **result, int *h_errnop)
{
err_t err;
struct gethostbyname_r_helper *h;
char *hostname;
size_t namelen;
int lh_errno;
if (h_errnop == NULL) {
/* ensure h_errnop is never NULL */
h_errnop = &lh_errno;
}
if (result == NULL) {
/* not all arguments given */
*h_errnop = EINVAL;
return -1;
}
/* first thing to do: set *result to nothing */
*result = NULL;
if ((name == NULL) || (ret == NULL) || (buf == NULL)) {
/* not all arguments given */
*h_errnop = EINVAL;
return -1;
}
namelen = strlen(name);
if (buflen < (sizeof(struct gethostbyname_r_helper) + LWIP_MEM_ALIGN_BUFFER(namelen + 1))) {
/* buf can't hold the data needed + a copy of name */
*h_errnop = ERANGE;
return -1;
}
h = (struct gethostbyname_r_helper *)LWIP_MEM_ALIGN(buf);
hostname = ((char *)h) + sizeof(struct gethostbyname_r_helper);
/* query host IP address */
err = netconn_gethostbyname(name, &h->addr);
if (err != ERR_OK) {
LWIP_DEBUGF(DNS_DEBUG, ("lwip_gethostbyname(%s) failed, err=%d\n", name, err));
*h_errnop = HOST_NOT_FOUND;
return -1;
}
/* copy the hostname into buf */
MEMCPY(hostname, name, namelen);
hostname[namelen] = 0;
/* fill hostent */
h->addr_list[0] = &h->addr;
h->addr_list[1] = NULL;
h->aliases = NULL;
ret->h_name = hostname;
ret->h_aliases = &h->aliases;
ret->h_addrtype = AF_INET;
ret->h_length = sizeof(ip_addr_t);
ret->h_addr_list = (char **)&h->addr_list;
/* set result != NULL */
*result = ret;
/* return success */
return 0;
}
/**
* Frees one or more addrinfo structures returned by getaddrinfo(), along with
* any additional storage associated with those structures. If the ai_next field
* of the structure is not null, the entire list of structures is freed.
*
* @param ai struct addrinfo to free
*/
void
lwip_freeaddrinfo(struct addrinfo *ai)
{
struct addrinfo *next;
while (ai != NULL) {
next = ai->ai_next;
memp_free(MEMP_NETDB, ai);
ai = next;
}
}
/**
* Translates the name of a service location (for example, a host name) and/or
* a service name and returns a set of socket addresses and associated
* information to be used in creating a socket with which to address the
* specified service.
* Memory for the result is allocated internally and must be freed by calling
* lwip_freeaddrinfo()!
*
* Due to a limitation in dns_gethostbyname, only the first address of a
* host is returned.
* Also, service names are not supported (only port numbers)!
*
* @param nodename descriptive name or address string of the host
* (may be NULL -> local address)
* @param servname port number as string of NULL
* @param hints structure containing input values that set socktype and protocol
* @param res pointer to a pointer where to store the result (set to NULL on failure)
* @return 0 on success, non-zero on failure
*
* @todo: implement AI_V4MAPPED, AI_ADDRCONFIG
*/
int
lwip_getaddrinfo(const char *nodename, const char *servname,
const struct addrinfo *hints, struct addrinfo **res)
{
err_t err;
ip_addr_t addr;
struct addrinfo *ai;
struct sockaddr_storage *sa = NULL;
int port_nr = 0;
size_t total_size;
size_t namelen = 0;
int ai_family;
if (res == NULL) {
return EAI_FAIL;
}
*res = NULL;
if ((nodename == NULL) && (servname == NULL)) {
return EAI_NONAME;
}
if (hints != NULL) {
ai_family = hints->ai_family;
if ((ai_family != AF_UNSPEC)
#if LWIP_IPV4
&& (ai_family != AF_INET)
#endif /* LWIP_IPV4 */
#if LWIP_IPV6
&& (ai_family != AF_INET6)
#endif /* LWIP_IPV6 */
) {
return EAI_FAMILY;
}
} else {
ai_family = AF_UNSPEC;
}
if (servname != NULL) {
/* service name specified: convert to port number
* @todo?: currently, only ASCII integers (port numbers) are supported (AI_NUMERICSERV)! */
port_nr = atoi(servname);
if ((port_nr <= 0) || (port_nr > 0xffff)) {
return EAI_SERVICE;
}
}
if (nodename != NULL) {
/* service location specified, try to resolve */
if ((hints != NULL) && (hints->ai_flags & AI_NUMERICHOST)) {
/* no DNS lookup, just parse for an address string */
if (!ipaddr_aton(nodename, &addr)) {
return EAI_NONAME;
}
#if LWIP_IPV4 && LWIP_IPV6
if ((IP_IS_V6_VAL(addr) && ai_family == AF_INET) ||
(IP_IS_V4_VAL(addr) && ai_family == AF_INET6)) {
return EAI_NONAME;
}
#endif /* LWIP_IPV4 && LWIP_IPV6 */
} else {
#if LWIP_IPV4 && LWIP_IPV6
/* AF_UNSPEC: prefer IPv4 */
u8_t type = NETCONN_DNS_IPV4_IPV6;
if (ai_family == AF_INET) {
type = NETCONN_DNS_IPV4;
} else if (ai_family == AF_INET6) {
type = NETCONN_DNS_IPV6;
}
#endif /* LWIP_IPV4 && LWIP_IPV6 */
err = netconn_gethostbyname_addrtype(nodename, &addr, type);
if (err != ERR_OK) {
return EAI_FAIL;
}
}
} else {
/* service location specified, use loopback address */
if ((hints != NULL) && (hints->ai_flags & AI_PASSIVE)) {
ip_addr_set_any_val(ai_family == AF_INET6, addr);
} else {
ip_addr_set_loopback_val(ai_family == AF_INET6, addr);
}
}
total_size = sizeof(struct addrinfo) + sizeof(struct sockaddr_storage);
if (nodename != NULL) {
namelen = strlen(nodename);
if (namelen > DNS_MAX_NAME_LENGTH) {
/* invalid name length */
return EAI_FAIL;
}
LWIP_ASSERT("namelen is too long", total_size + namelen + 1 > total_size);
total_size += namelen + 1;
}
/* If this fails, please report to lwip-devel! :-) */
LWIP_ASSERT("total_size <= NETDB_ELEM_SIZE: please report this!",
total_size <= NETDB_ELEM_SIZE);
ai = (struct addrinfo *)memp_malloc(MEMP_NETDB);
if (ai == NULL) {
return EAI_MEMORY;
}
memset(ai, 0, total_size);
/* cast through void* to get rid of alignment warnings */
sa = (struct sockaddr_storage *)(void *)((u8_t *)ai + sizeof(struct addrinfo));
if (IP_IS_V6_VAL(addr)) {
#if LWIP_IPV6
struct sockaddr_in6 *sa6 = (struct sockaddr_in6 *)sa;
/* set up sockaddr */
inet6_addr_from_ip6addr(&sa6->sin6_addr, ip_2_ip6(&addr));
sa6->sin6_family = AF_INET6;
sa6->sin6_len = sizeof(struct sockaddr_in6);
sa6->sin6_port = lwip_htons((u16_t)port_nr);
sa6->sin6_scope_id = ip6_addr_zone(ip_2_ip6(&addr));
ai->ai_family = AF_INET6;
#endif /* LWIP_IPV6 */
} else {
#if LWIP_IPV4
struct sockaddr_in *sa4 = (struct sockaddr_in *)sa;
/* set up sockaddr */
inet_addr_from_ip4addr(&sa4->sin_addr, ip_2_ip4(&addr));
sa4->sin_family = AF_INET;
sa4->sin_len = sizeof(struct sockaddr_in);
sa4->sin_port = lwip_htons((u16_t)port_nr);
ai->ai_family = AF_INET;
#endif /* LWIP_IPV4 */
}
/* set up addrinfo */
if (hints != NULL) {
/* copy socktype & protocol from hints if specified */
ai->ai_socktype = hints->ai_socktype;
ai->ai_protocol = hints->ai_protocol;
}
if (nodename != NULL) {
/* copy nodename to canonname if specified */
ai->ai_canonname = ((char *)ai + sizeof(struct addrinfo) + sizeof(struct sockaddr_storage));
MEMCPY(ai->ai_canonname, nodename, namelen);
ai->ai_canonname[namelen] = 0;
}
ai->ai_addrlen = sizeof(struct sockaddr_storage);
ai->ai_addr = (struct sockaddr *)sa;
*res = ai;
return 0;
}
#endif /* LWIP_DNS && LWIP_SOCKET */

View File

@ -0,0 +1,380 @@
/**
* @file
* Network Interface Sequential API module
*
* @defgroup netifapi NETIF API
* @ingroup sequential_api
* Thread-safe functions to be called from non-TCPIP threads
*
* @defgroup netifapi_netif NETIF related
* @ingroup netifapi
* To be called from non-TCPIP threads
*/
/*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
*/
#include "lwip/opt.h"
#if LWIP_NETIF_API /* don't build if not configured for use in lwipopts.h */
#include "lwip/etharp.h"
#include "lwip/netifapi.h"
#include "lwip/memp.h"
#include "lwip/priv/tcpip_priv.h"
#include <string.h> /* strncpy */
#define NETIFAPI_VAR_REF(name) API_VAR_REF(name)
#define NETIFAPI_VAR_DECLARE(name) API_VAR_DECLARE(struct netifapi_msg, name)
#define NETIFAPI_VAR_ALLOC(name) API_VAR_ALLOC(struct netifapi_msg, MEMP_NETIFAPI_MSG, name, ERR_MEM)
#define NETIFAPI_VAR_FREE(name) API_VAR_FREE(MEMP_NETIFAPI_MSG, name)
/**
* Call netif_add() inside the tcpip_thread context.
*/
static err_t
netifapi_do_netif_add(struct tcpip_api_call_data *m)
{
/* cast through void* to silence alignment warnings.
* We know it works because the structs have been instantiated as struct netifapi_msg */
struct netifapi_msg *msg = (struct netifapi_msg *)(void *)m;
if (!netif_add( msg->netif,
#if LWIP_IPV4
API_EXPR_REF(msg->msg.add.ipaddr),
API_EXPR_REF(msg->msg.add.netmask),
API_EXPR_REF(msg->msg.add.gw),
#endif /* LWIP_IPV4 */
msg->msg.add.state,
msg->msg.add.init,
msg->msg.add.input)) {
return ERR_IF;
} else {
return ERR_OK;
}
}
#if LWIP_IPV4
/**
* Call netif_set_addr() inside the tcpip_thread context.
*/
static err_t
netifapi_do_netif_set_addr(struct tcpip_api_call_data *m)
{
/* cast through void* to silence alignment warnings.
* We know it works because the structs have been instantiated as struct netifapi_msg */
struct netifapi_msg *msg = (struct netifapi_msg *)(void *)m;
netif_set_addr( msg->netif,
API_EXPR_REF(msg->msg.add.ipaddr),
API_EXPR_REF(msg->msg.add.netmask),
API_EXPR_REF(msg->msg.add.gw));
return ERR_OK;
}
#endif /* LWIP_IPV4 */
/**
* Call netif_name_to_index() inside the tcpip_thread context.
*/
static err_t
netifapi_do_name_to_index(struct tcpip_api_call_data *m)
{
/* cast through void* to silence alignment warnings.
* We know it works because the structs have been instantiated as struct netifapi_msg */
struct netifapi_msg *msg = (struct netifapi_msg *)(void *)m;
msg->msg.ifs.index = netif_name_to_index(msg->msg.ifs.name);
return ERR_OK;
}
/**
* Call netif_index_to_name() inside the tcpip_thread context.
*/
static err_t
netifapi_do_index_to_name(struct tcpip_api_call_data *m)
{
/* cast through void* to silence alignment warnings.
* We know it works because the structs have been instantiated as struct netifapi_msg */
struct netifapi_msg *msg = (struct netifapi_msg *)(void *)m;
if (!netif_index_to_name(msg->msg.ifs.index, msg->msg.ifs.name)) {
/* return failure via empty name */
msg->msg.ifs.name[0] = '\0';
}
return ERR_OK;
}
/**
* Call the "errtfunc" (or the "voidfunc" if "errtfunc" is NULL) inside the
* tcpip_thread context.
*/
static err_t
netifapi_do_netif_common(struct tcpip_api_call_data *m)
{
/* cast through void* to silence alignment warnings.
* We know it works because the structs have been instantiated as struct netifapi_msg */
struct netifapi_msg *msg = (struct netifapi_msg *)(void *)m;
if (msg->msg.common.errtfunc != NULL) {
return msg->msg.common.errtfunc(msg->netif);
} else {
msg->msg.common.voidfunc(msg->netif);
return ERR_OK;
}
}
#if LWIP_ARP && LWIP_IPV4
/**
* @ingroup netifapi_arp
* Add or update an entry in the ARP cache.
* For an update, ipaddr is used to find the cache entry.
*
* @param ipaddr IPv4 address of cache entry
* @param ethaddr hardware address mapped to ipaddr
* @param type type of ARP cache entry
* @return ERR_OK: entry added/updated, else error from err_t
*/
err_t
netifapi_arp_add(const ip4_addr_t *ipaddr, struct eth_addr *ethaddr, enum netifapi_arp_entry type)
{
err_t err;
/* We only support permanent entries currently */
LWIP_UNUSED_ARG(type);
#if ETHARP_SUPPORT_STATIC_ENTRIES && LWIP_TCPIP_CORE_LOCKING
LOCK_TCPIP_CORE();
err = etharp_add_static_entry(ipaddr, ethaddr);
UNLOCK_TCPIP_CORE();
#else
/* @todo add new vars to struct netifapi_msg and create a 'do' func */
LWIP_UNUSED_ARG(ipaddr);
LWIP_UNUSED_ARG(ethaddr);
err = ERR_VAL;
#endif /* ETHARP_SUPPORT_STATIC_ENTRIES && LWIP_TCPIP_CORE_LOCKING */
return err;
}
/**
* @ingroup netifapi_arp
* Remove an entry in the ARP cache identified by ipaddr
*
* @param ipaddr IPv4 address of cache entry
* @param type type of ARP cache entry
* @return ERR_OK: entry removed, else error from err_t
*/
err_t
netifapi_arp_remove(const ip4_addr_t *ipaddr, enum netifapi_arp_entry type)
{
err_t err;
/* We only support permanent entries currently */
LWIP_UNUSED_ARG(type);
#if ETHARP_SUPPORT_STATIC_ENTRIES && LWIP_TCPIP_CORE_LOCKING
LOCK_TCPIP_CORE();
err = etharp_remove_static_entry(ipaddr);
UNLOCK_TCPIP_CORE();
#else
/* @todo add new vars to struct netifapi_msg and create a 'do' func */
LWIP_UNUSED_ARG(ipaddr);
err = ERR_VAL;
#endif /* ETHARP_SUPPORT_STATIC_ENTRIES && LWIP_TCPIP_CORE_LOCKING */
return err;
}
#endif /* LWIP_ARP && LWIP_IPV4 */
/**
* @ingroup netifapi_netif
* Call netif_add() in a thread-safe way by running that function inside the
* tcpip_thread context.
*
* @note for params @see netif_add()
*/
err_t
netifapi_netif_add(struct netif *netif,
#if LWIP_IPV4
const ip4_addr_t *ipaddr, const ip4_addr_t *netmask, const ip4_addr_t *gw,
#endif /* LWIP_IPV4 */
void *state, netif_init_fn init, netif_input_fn input)
{
err_t err;
NETIFAPI_VAR_DECLARE(msg);
NETIFAPI_VAR_ALLOC(msg);
#if LWIP_IPV4
if (ipaddr == NULL) {
ipaddr = IP4_ADDR_ANY4;
}
if (netmask == NULL) {
netmask = IP4_ADDR_ANY4;
}
if (gw == NULL) {
gw = IP4_ADDR_ANY4;
}
#endif /* LWIP_IPV4 */
NETIFAPI_VAR_REF(msg).netif = netif;
#if LWIP_IPV4
NETIFAPI_VAR_REF(msg).msg.add.ipaddr = NETIFAPI_VAR_REF(ipaddr);
NETIFAPI_VAR_REF(msg).msg.add.netmask = NETIFAPI_VAR_REF(netmask);
NETIFAPI_VAR_REF(msg).msg.add.gw = NETIFAPI_VAR_REF(gw);
#endif /* LWIP_IPV4 */
NETIFAPI_VAR_REF(msg).msg.add.state = state;
NETIFAPI_VAR_REF(msg).msg.add.init = init;
NETIFAPI_VAR_REF(msg).msg.add.input = input;
err = tcpip_api_call(netifapi_do_netif_add, &API_VAR_REF(msg).call);
NETIFAPI_VAR_FREE(msg);
return err;
}
#if LWIP_IPV4
/**
* @ingroup netifapi_netif
* Call netif_set_addr() in a thread-safe way by running that function inside the
* tcpip_thread context.
*
* @note for params @see netif_set_addr()
*/
err_t
netifapi_netif_set_addr(struct netif *netif,
const ip4_addr_t *ipaddr,
const ip4_addr_t *netmask,
const ip4_addr_t *gw)
{
err_t err;
NETIFAPI_VAR_DECLARE(msg);
NETIFAPI_VAR_ALLOC(msg);
if (ipaddr == NULL) {
ipaddr = IP4_ADDR_ANY4;
}
if (netmask == NULL) {
netmask = IP4_ADDR_ANY4;
}
if (gw == NULL) {
gw = IP4_ADDR_ANY4;
}
NETIFAPI_VAR_REF(msg).netif = netif;
NETIFAPI_VAR_REF(msg).msg.add.ipaddr = NETIFAPI_VAR_REF(ipaddr);
NETIFAPI_VAR_REF(msg).msg.add.netmask = NETIFAPI_VAR_REF(netmask);
NETIFAPI_VAR_REF(msg).msg.add.gw = NETIFAPI_VAR_REF(gw);
err = tcpip_api_call(netifapi_do_netif_set_addr, &API_VAR_REF(msg).call);
NETIFAPI_VAR_FREE(msg);
return err;
}
#endif /* LWIP_IPV4 */
/**
* call the "errtfunc" (or the "voidfunc" if "errtfunc" is NULL) in a thread-safe
* way by running that function inside the tcpip_thread context.
*
* @note use only for functions where there is only "netif" parameter.
*/
err_t
netifapi_netif_common(struct netif *netif, netifapi_void_fn voidfunc,
netifapi_errt_fn errtfunc)
{
err_t err;
NETIFAPI_VAR_DECLARE(msg);
NETIFAPI_VAR_ALLOC(msg);
NETIFAPI_VAR_REF(msg).netif = netif;
NETIFAPI_VAR_REF(msg).msg.common.voidfunc = voidfunc;
NETIFAPI_VAR_REF(msg).msg.common.errtfunc = errtfunc;
err = tcpip_api_call(netifapi_do_netif_common, &API_VAR_REF(msg).call);
NETIFAPI_VAR_FREE(msg);
return err;
}
/**
* @ingroup netifapi_netif
* Call netif_name_to_index() in a thread-safe way by running that function inside the
* tcpip_thread context.
*
* @param name the interface name of the netif
* @param idx output index of the found netif
*/
err_t
netifapi_netif_name_to_index(const char *name, u8_t *idx)
{
err_t err;
NETIFAPI_VAR_DECLARE(msg);
NETIFAPI_VAR_ALLOC(msg);
*idx = 0;
#if LWIP_MPU_COMPATIBLE
strncpy(NETIFAPI_VAR_REF(msg).msg.ifs.name, name, NETIF_NAMESIZE - 1);
NETIFAPI_VAR_REF(msg).msg.ifs.name[NETIF_NAMESIZE - 1] = '\0';
#else
NETIFAPI_VAR_REF(msg).msg.ifs.name = LWIP_CONST_CAST(char *, name);
#endif /* LWIP_MPU_COMPATIBLE */
err = tcpip_api_call(netifapi_do_name_to_index, &API_VAR_REF(msg).call);
if (!err) {
*idx = NETIFAPI_VAR_REF(msg).msg.ifs.index;
}
NETIFAPI_VAR_FREE(msg);
return err;
}
/**
* @ingroup netifapi_netif
* Call netif_index_to_name() in a thread-safe way by running that function inside the
* tcpip_thread context.
*
* @param idx the interface index of the netif
* @param name output name of the found netif, empty '\0' string if netif not found.
* name should be of at least NETIF_NAMESIZE bytes
*/
err_t
netifapi_netif_index_to_name(u8_t idx, char *name)
{
err_t err;
NETIFAPI_VAR_DECLARE(msg);
NETIFAPI_VAR_ALLOC(msg);
NETIFAPI_VAR_REF(msg).msg.ifs.index = idx;
#if !LWIP_MPU_COMPATIBLE
NETIFAPI_VAR_REF(msg).msg.ifs.name = name;
#endif /* LWIP_MPU_COMPATIBLE */
err = tcpip_api_call(netifapi_do_index_to_name, &API_VAR_REF(msg).call);
#if LWIP_MPU_COMPATIBLE
if (!err) {
strncpy(name, NETIFAPI_VAR_REF(msg).msg.ifs.name, NETIF_NAMESIZE - 1);
name[NETIF_NAMESIZE - 1] = '\0';
}
#endif /* LWIP_MPU_COMPATIBLE */
NETIFAPI_VAR_FREE(msg);
return err;
}
#endif /* LWIP_NETIF_API */

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,658 @@
/**
* @file
* Sequential API Main thread module
*
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#include "lwip/opt.h"
#if !NO_SYS /* don't build if not configured for use in lwipopts.h */
#include "lwip/priv/tcpip_priv.h"
#include "lwip/sys.h"
#include "lwip/memp.h"
#include "lwip/mem.h"
#include "lwip/init.h"
#include "lwip/ip.h"
#include "lwip/pbuf.h"
#include "lwip/etharp.h"
#include "netif/ethernet.h"
#define TCPIP_MSG_VAR_REF(name) API_VAR_REF(name)
#define TCPIP_MSG_VAR_DECLARE(name) API_VAR_DECLARE(struct tcpip_msg, name)
#define TCPIP_MSG_VAR_ALLOC(name) API_VAR_ALLOC(struct tcpip_msg, MEMP_TCPIP_MSG_API, name, ERR_MEM)
#define TCPIP_MSG_VAR_FREE(name) API_VAR_FREE(MEMP_TCPIP_MSG_API, name)
/* global variables */
static tcpip_init_done_fn tcpip_init_done;
static void *tcpip_init_done_arg;
static sys_mbox_t tcpip_mbox;
#if LWIP_TCPIP_CORE_LOCKING
/** The global semaphore to lock the stack. */
sys_mutex_t lock_tcpip_core;
#endif /* LWIP_TCPIP_CORE_LOCKING */
static void tcpip_thread_handle_msg(struct tcpip_msg *msg);
#if !LWIP_TIMERS
/* wait for a message with timers disabled (e.g. pass a timer-check trigger into tcpip_thread) */
#define TCPIP_MBOX_FETCH(mbox, msg) sys_mbox_fetch(mbox, msg)
#else /* !LWIP_TIMERS */
/* wait for a message, timeouts are processed while waiting */
#define TCPIP_MBOX_FETCH(mbox, msg) tcpip_timeouts_mbox_fetch(mbox, msg)
/**
* Wait (forever) for a message to arrive in an mbox.
* While waiting, timeouts are processed.
*
* @param mbox the mbox to fetch the message from
* @param msg the place to store the message
*/
static void
tcpip_timeouts_mbox_fetch(sys_mbox_t *mbox, void **msg)
{
u32_t sleeptime, res;
again:
LWIP_ASSERT_CORE_LOCKED();
sleeptime = sys_timeouts_sleeptime();
if (sleeptime == SYS_TIMEOUTS_SLEEPTIME_INFINITE) {
UNLOCK_TCPIP_CORE();
sys_arch_mbox_fetch(mbox, msg, 0);
LOCK_TCPIP_CORE();
return;
} else if (sleeptime == 0) {
sys_check_timeouts();
/* We try again to fetch a message from the mbox. */
goto again;
}
UNLOCK_TCPIP_CORE();
res = sys_arch_mbox_fetch(mbox, msg, sleeptime);
LOCK_TCPIP_CORE();
if (res == SYS_ARCH_TIMEOUT) {
/* If a SYS_ARCH_TIMEOUT value is returned, a timeout occurred
before a message could be fetched. */
sys_check_timeouts();
/* We try again to fetch a message from the mbox. */
goto again;
}
}
#endif /* !LWIP_TIMERS */
/**
* The main lwIP thread. This thread has exclusive access to lwIP core functions
* (unless access to them is not locked). Other threads communicate with this
* thread using message boxes.
*
* It also starts all the timers to make sure they are running in the right
* thread context.
*
* @param arg unused argument
*/
static void
tcpip_thread(void *arg)
{
struct tcpip_msg *msg;
LWIP_UNUSED_ARG(arg);
LWIP_MARK_TCPIP_THREAD();
LOCK_TCPIP_CORE();
if (tcpip_init_done != NULL) {
tcpip_init_done(tcpip_init_done_arg);
}
while (1) { /* MAIN Loop */
LWIP_TCPIP_THREAD_ALIVE();
/* wait for a message, timeouts are processed while waiting */
TCPIP_MBOX_FETCH(&tcpip_mbox, (void **)&msg);
if (msg == NULL) {
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: invalid message: NULL\n"));
LWIP_ASSERT("tcpip_thread: invalid message", 0);
continue;
}
tcpip_thread_handle_msg(msg);
}
}
/* Handle a single tcpip_msg
* This is in its own function for access by tests only.
*/
static void
tcpip_thread_handle_msg(struct tcpip_msg *msg)
{
switch (msg->type) {
#if !LWIP_TCPIP_CORE_LOCKING
case TCPIP_MSG_API:
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: API message %p\n", (void *)msg));
msg->msg.api_msg.function(msg->msg.api_msg.msg);
break;
case TCPIP_MSG_API_CALL:
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: API CALL message %p\n", (void *)msg));
msg->msg.api_call.arg->err = msg->msg.api_call.function(msg->msg.api_call.arg);
sys_sem_signal(msg->msg.api_call.sem);
break;
#endif /* !LWIP_TCPIP_CORE_LOCKING */
#if !LWIP_TCPIP_CORE_LOCKING_INPUT
case TCPIP_MSG_INPKT:
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: PACKET %p\n", (void *)msg));
if (msg->msg.inp.input_fn(msg->msg.inp.p, msg->msg.inp.netif) != ERR_OK) {
pbuf_free(msg->msg.inp.p);
}
memp_free(MEMP_TCPIP_MSG_INPKT, msg);
break;
#endif /* !LWIP_TCPIP_CORE_LOCKING_INPUT */
#if LWIP_TCPIP_TIMEOUT && LWIP_TIMERS
case TCPIP_MSG_TIMEOUT:
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: TIMEOUT %p\n", (void *)msg));
sys_timeout(msg->msg.tmo.msecs, msg->msg.tmo.h, msg->msg.tmo.arg);
memp_free(MEMP_TCPIP_MSG_API, msg);
break;
case TCPIP_MSG_UNTIMEOUT:
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: UNTIMEOUT %p\n", (void *)msg));
sys_untimeout(msg->msg.tmo.h, msg->msg.tmo.arg);
memp_free(MEMP_TCPIP_MSG_API, msg);
break;
#endif /* LWIP_TCPIP_TIMEOUT && LWIP_TIMERS */
case TCPIP_MSG_CALLBACK:
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: CALLBACK %p\n", (void *)msg));
msg->msg.cb.function(msg->msg.cb.ctx);
memp_free(MEMP_TCPIP_MSG_API, msg);
break;
case TCPIP_MSG_CALLBACK_STATIC:
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: CALLBACK_STATIC %p\n", (void *)msg));
msg->msg.cb.function(msg->msg.cb.ctx);
break;
default:
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_thread: invalid message: %d\n", msg->type));
LWIP_ASSERT("tcpip_thread: invalid message", 0);
break;
}
}
#ifdef TCPIP_THREAD_TEST
/** Work on queued items in single-threaded test mode */
int
tcpip_thread_poll_one(void)
{
int ret = 0;
struct tcpip_msg *msg;
if (sys_arch_mbox_tryfetch(&tcpip_mbox, (void **)&msg) != SYS_ARCH_TIMEOUT) {
LOCK_TCPIP_CORE();
if (msg != NULL) {
tcpip_thread_handle_msg(msg);
ret = 1;
}
UNLOCK_TCPIP_CORE();
}
return ret;
}
#endif
/**
* Pass a received packet to tcpip_thread for input processing
*
* @param p the received packet
* @param inp the network interface on which the packet was received
* @param input_fn input function to call
*/
err_t
tcpip_inpkt(struct pbuf *p, struct netif *inp, netif_input_fn input_fn)
{
#if LWIP_TCPIP_CORE_LOCKING_INPUT
err_t ret;
LWIP_DEBUGF(TCPIP_DEBUG, ("tcpip_inpkt: PACKET %p/%p\n", (void *)p, (void *)inp));
LOCK_TCPIP_CORE();
ret = input_fn(p, inp);
UNLOCK_TCPIP_CORE();
return ret;
#else /* LWIP_TCPIP_CORE_LOCKING_INPUT */
struct tcpip_msg *msg;
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
msg = (struct tcpip_msg *)memp_malloc(MEMP_TCPIP_MSG_INPKT);
if (msg == NULL) {
return ERR_MEM;
}
msg->type = TCPIP_MSG_INPKT;
msg->msg.inp.p = p;
msg->msg.inp.netif = inp;
msg->msg.inp.input_fn = input_fn;
if (sys_mbox_trypost(&tcpip_mbox, msg) != ERR_OK) {
memp_free(MEMP_TCPIP_MSG_INPKT, msg);
return ERR_MEM;
}
return ERR_OK;
#endif /* LWIP_TCPIP_CORE_LOCKING_INPUT */
}
/**
* @ingroup lwip_os
* Pass a received packet to tcpip_thread for input processing with
* ethernet_input or ip_input. Don't call directly, pass to netif_add()
* and call netif->input().
*
* @param p the received packet, p->payload pointing to the Ethernet header or
* to an IP header (if inp doesn't have NETIF_FLAG_ETHARP or
* NETIF_FLAG_ETHERNET flags)
* @param inp the network interface on which the packet was received
*/
err_t
tcpip_input(struct pbuf *p, struct netif *inp)
{
#if LWIP_ETHERNET
if (inp->flags & (NETIF_FLAG_ETHARP | NETIF_FLAG_ETHERNET)) {
return tcpip_inpkt(p, inp, ethernet_input);
} else
#endif /* LWIP_ETHERNET */
return tcpip_inpkt(p, inp, ip_input);
}
/**
* @ingroup lwip_os
* Call a specific function in the thread context of
* tcpip_thread for easy access synchronization.
* A function called in that way may access lwIP core code
* without fearing concurrent access.
* Blocks until the request is posted.
* Must not be called from interrupt context!
*
* @param function the function to call
* @param ctx parameter passed to f
* @return ERR_OK if the function was called, another err_t if not
*
* @see tcpip_try_callback
*/
err_t
tcpip_callback(tcpip_callback_fn function, void *ctx)
{
struct tcpip_msg *msg;
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
msg = (struct tcpip_msg *)memp_malloc(MEMP_TCPIP_MSG_API);
if (msg == NULL) {
return ERR_MEM;
}
msg->type = TCPIP_MSG_CALLBACK;
msg->msg.cb.function = function;
msg->msg.cb.ctx = ctx;
sys_mbox_post(&tcpip_mbox, msg);
return ERR_OK;
}
/**
* @ingroup lwip_os
* Call a specific function in the thread context of
* tcpip_thread for easy access synchronization.
* A function called in that way may access lwIP core code
* without fearing concurrent access.
* Does NOT block when the request cannot be posted because the
* tcpip_mbox is full, but returns ERR_MEM instead.
* Can be called from interrupt context.
*
* @param function the function to call
* @param ctx parameter passed to f
* @return ERR_OK if the function was called, another err_t if not
*
* @see tcpip_callback
*/
err_t
tcpip_try_callback(tcpip_callback_fn function, void *ctx)
{
struct tcpip_msg *msg;
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
msg = (struct tcpip_msg *)memp_malloc(MEMP_TCPIP_MSG_API);
if (msg == NULL) {
return ERR_MEM;
}
msg->type = TCPIP_MSG_CALLBACK;
msg->msg.cb.function = function;
msg->msg.cb.ctx = ctx;
if (sys_mbox_trypost(&tcpip_mbox, msg) != ERR_OK) {
memp_free(MEMP_TCPIP_MSG_API, msg);
return ERR_MEM;
}
return ERR_OK;
}
#if LWIP_TCPIP_TIMEOUT && LWIP_TIMERS
/**
* call sys_timeout in tcpip_thread
*
* @param msecs time in milliseconds for timeout
* @param h function to be called on timeout
* @param arg argument to pass to timeout function h
* @return ERR_MEM on memory error, ERR_OK otherwise
*/
err_t
tcpip_timeout(u32_t msecs, sys_timeout_handler h, void *arg)
{
struct tcpip_msg *msg;
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
msg = (struct tcpip_msg *)memp_malloc(MEMP_TCPIP_MSG_API);
if (msg == NULL) {
return ERR_MEM;
}
msg->type = TCPIP_MSG_TIMEOUT;
msg->msg.tmo.msecs = msecs;
msg->msg.tmo.h = h;
msg->msg.tmo.arg = arg;
sys_mbox_post(&tcpip_mbox, msg);
return ERR_OK;
}
/**
* call sys_untimeout in tcpip_thread
*
* @param h function to be called on timeout
* @param arg argument to pass to timeout function h
* @return ERR_MEM on memory error, ERR_OK otherwise
*/
err_t
tcpip_untimeout(sys_timeout_handler h, void *arg)
{
struct tcpip_msg *msg;
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
msg = (struct tcpip_msg *)memp_malloc(MEMP_TCPIP_MSG_API);
if (msg == NULL) {
return ERR_MEM;
}
msg->type = TCPIP_MSG_UNTIMEOUT;
msg->msg.tmo.h = h;
msg->msg.tmo.arg = arg;
sys_mbox_post(&tcpip_mbox, msg);
return ERR_OK;
}
#endif /* LWIP_TCPIP_TIMEOUT && LWIP_TIMERS */
/**
* Sends a message to TCPIP thread to call a function. Caller thread blocks on
* on a provided semaphore, which ist NOT automatically signalled by TCPIP thread,
* this has to be done by the user.
* It is recommended to use LWIP_TCPIP_CORE_LOCKING since this is the way
* with least runtime overhead.
*
* @param fn function to be called from TCPIP thread
* @param apimsg argument to API function
* @param sem semaphore to wait on
* @return ERR_OK if the function was called, another err_t if not
*/
err_t
tcpip_send_msg_wait_sem(tcpip_callback_fn fn, void *apimsg, sys_sem_t *sem)
{
#if LWIP_TCPIP_CORE_LOCKING
LWIP_UNUSED_ARG(sem);
LOCK_TCPIP_CORE();
fn(apimsg);
UNLOCK_TCPIP_CORE();
return ERR_OK;
#else /* LWIP_TCPIP_CORE_LOCKING */
TCPIP_MSG_VAR_DECLARE(msg);
LWIP_ASSERT("semaphore not initialized", sys_sem_valid(sem));
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
TCPIP_MSG_VAR_ALLOC(msg);
TCPIP_MSG_VAR_REF(msg).type = TCPIP_MSG_API;
TCPIP_MSG_VAR_REF(msg).msg.api_msg.function = fn;
TCPIP_MSG_VAR_REF(msg).msg.api_msg.msg = apimsg;
sys_mbox_post(&tcpip_mbox, &TCPIP_MSG_VAR_REF(msg));
sys_arch_sem_wait(sem, 0);
TCPIP_MSG_VAR_FREE(msg);
return ERR_OK;
#endif /* LWIP_TCPIP_CORE_LOCKING */
}
/**
* Synchronously calls function in TCPIP thread and waits for its completion.
* It is recommended to use LWIP_TCPIP_CORE_LOCKING (preferred) or
* LWIP_NETCONN_SEM_PER_THREAD.
* If not, a semaphore is created and destroyed on every call which is usually
* an expensive/slow operation.
* @param fn Function to call
* @param call Call parameters
* @return Return value from tcpip_api_call_fn
*/
err_t
tcpip_api_call(tcpip_api_call_fn fn, struct tcpip_api_call_data *call)
{
#if LWIP_TCPIP_CORE_LOCKING
err_t err;
LOCK_TCPIP_CORE();
err = fn(call);
UNLOCK_TCPIP_CORE();
return err;
#else /* LWIP_TCPIP_CORE_LOCKING */
TCPIP_MSG_VAR_DECLARE(msg);
#if !LWIP_NETCONN_SEM_PER_THREAD
err_t err = sys_sem_new(&call->sem, 0);
if (err != ERR_OK) {
return err;
}
#endif /* LWIP_NETCONN_SEM_PER_THREAD */
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
TCPIP_MSG_VAR_ALLOC(msg);
TCPIP_MSG_VAR_REF(msg).type = TCPIP_MSG_API_CALL;
TCPIP_MSG_VAR_REF(msg).msg.api_call.arg = call;
TCPIP_MSG_VAR_REF(msg).msg.api_call.function = fn;
#if LWIP_NETCONN_SEM_PER_THREAD
TCPIP_MSG_VAR_REF(msg).msg.api_call.sem = LWIP_NETCONN_THREAD_SEM_GET();
#else /* LWIP_NETCONN_SEM_PER_THREAD */
TCPIP_MSG_VAR_REF(msg).msg.api_call.sem = &call->sem;
#endif /* LWIP_NETCONN_SEM_PER_THREAD */
sys_mbox_post(&tcpip_mbox, &TCPIP_MSG_VAR_REF(msg));
sys_arch_sem_wait(TCPIP_MSG_VAR_REF(msg).msg.api_call.sem, 0);
TCPIP_MSG_VAR_FREE(msg);
#if !LWIP_NETCONN_SEM_PER_THREAD
sys_sem_free(&call->sem);
#endif /* LWIP_NETCONN_SEM_PER_THREAD */
return call->err;
#endif /* LWIP_TCPIP_CORE_LOCKING */
}
/**
* @ingroup lwip_os
* Allocate a structure for a static callback message and initialize it.
* The message has a special type such that lwIP never frees it.
* This is intended to be used to send "static" messages from interrupt context,
* e.g. the message is allocated once and posted several times from an IRQ
* using tcpip_callbackmsg_trycallback().
* Example usage: Trigger execution of an ethernet IRQ DPC routine in lwIP thread context.
*
* @param function the function to call
* @param ctx parameter passed to function
* @return a struct pointer to pass to tcpip_callbackmsg_trycallback().
*
* @see tcpip_callbackmsg_trycallback()
* @see tcpip_callbackmsg_delete()
*/
struct tcpip_callback_msg *
tcpip_callbackmsg_new(tcpip_callback_fn function, void *ctx)
{
struct tcpip_msg *msg = (struct tcpip_msg *)memp_malloc(MEMP_TCPIP_MSG_API);
if (msg == NULL) {
return NULL;
}
msg->type = TCPIP_MSG_CALLBACK_STATIC;
msg->msg.cb.function = function;
msg->msg.cb.ctx = ctx;
return (struct tcpip_callback_msg *)msg;
}
/**
* @ingroup lwip_os
* Free a callback message allocated by tcpip_callbackmsg_new().
*
* @param msg the message to free
*
* @see tcpip_callbackmsg_new()
*/
void
tcpip_callbackmsg_delete(struct tcpip_callback_msg *msg)
{
memp_free(MEMP_TCPIP_MSG_API, msg);
}
/**
* @ingroup lwip_os
* Try to post a callback-message to the tcpip_thread tcpip_mbox.
*
* @param msg pointer to the message to post
* @return sys_mbox_trypost() return code
*
* @see tcpip_callbackmsg_new()
*/
err_t
tcpip_callbackmsg_trycallback(struct tcpip_callback_msg *msg)
{
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
return sys_mbox_trypost(&tcpip_mbox, msg);
}
/**
* @ingroup lwip_os
* Try to post a callback-message to the tcpip_thread mbox.
* Same as @ref tcpip_callbackmsg_trycallback but calls sys_mbox_trypost_fromisr(),
* mainly to help FreeRTOS, where calls differ between task level and ISR level.
*
* @param msg pointer to the message to post
* @return sys_mbox_trypost_fromisr() return code (without change, so this
* knowledge can be used to e.g. propagate "bool needs_scheduling")
*
* @see tcpip_callbackmsg_new()
*/
err_t
tcpip_callbackmsg_trycallback_fromisr(struct tcpip_callback_msg *msg)
{
LWIP_ASSERT("Invalid mbox", sys_mbox_valid_val(tcpip_mbox));
return sys_mbox_trypost_fromisr(&tcpip_mbox, msg);
}
/**
* @ingroup lwip_os
* Initialize this module:
* - initialize all sub modules
* - start the tcpip_thread
*
* @param initfunc a function to call when tcpip_thread is running and finished initializing
* @param arg argument to pass to initfunc
*/
void
tcpip_init(tcpip_init_done_fn initfunc, void *arg)
{
lwip_init();
tcpip_init_done = initfunc;
tcpip_init_done_arg = arg;
if (sys_mbox_new(&tcpip_mbox, TCPIP_MBOX_SIZE) != ERR_OK) {
LWIP_ASSERT("failed to create tcpip_thread mbox", 0);
}
#if LWIP_TCPIP_CORE_LOCKING
if (sys_mutex_new(&lock_tcpip_core) != ERR_OK) {
LWIP_ASSERT("failed to create lock_tcpip_core", 0);
}
#endif /* LWIP_TCPIP_CORE_LOCKING */
sys_thread_new(TCPIP_THREAD_NAME, tcpip_thread, NULL, TCPIP_THREAD_STACKSIZE, TCPIP_THREAD_PRIO);
}
/**
* Simple callback function used with tcpip_callback to free a pbuf
* (pbuf_free has a wrong signature for tcpip_callback)
*
* @param p The pbuf (chain) to be dereferenced.
*/
static void
pbuf_free_int(void *p)
{
struct pbuf *q = (struct pbuf *)p;
pbuf_free(q);
}
/**
* A simple wrapper function that allows you to free a pbuf from interrupt context.
*
* @param p The pbuf (chain) to be dereferenced.
* @return ERR_OK if callback could be enqueued, an err_t if not
*/
err_t
pbuf_free_callback(struct pbuf *p)
{
return tcpip_try_callback(pbuf_free_int, p);
}
/**
* A simple wrapper function that allows you to free heap memory from
* interrupt context.
*
* @param m the heap memory to free
* @return ERR_OK if callback could be enqueued, an err_t if not
*/
err_t
mem_free_callback(void *m)
{
return tcpip_try_callback(mem_free, m);
}
#endif /* !NO_SYS */

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,210 @@
/**
* @file
* Application layered TCP connection API (to be used from TCPIP thread)
*
* This file contains memory management functions for a TLS layer using mbedTLS.
*
* ATTENTION: For production usage, you might want to override this file with
* your own implementation since this implementation simply uses the
* lwIP heap without caring for fragmentation or leaving heap for
* other parts of lwIP!
*/
/*
* Copyright (c) 2017 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt <goldsimon@gmx.de>
*
* Missing things / @todo:
* - RX data is acknowledged after receiving (tcp_recved is called when enqueueing
* the pbuf for mbedTLS receive, not when processed by mbedTLS or the inner
* connection; altcp_recved() from inner connection does nothing)
* - TX data is marked as 'sent' (i.e. acknowledged; sent callback is called) right
* after enqueueing for transmission, not when actually ACKed be the remote host.
*/
#include "lwip/opt.h"
#if LWIP_ALTCP /* don't build if not configured for use in lwipopts.h */
#include "lwip/apps/altcp_tls_mbedtls_opts.h"
#if LWIP_ALTCP_TLS && LWIP_ALTCP_TLS_MBEDTLS
#include "altcp_tls_mbedtls_mem.h"
#include "altcp_tls_mbedtls_structs.h"
#include "lwip/mem.h"
#include "mbedtls/platform.h"
#include <string.h>
#ifndef ALTCP_MBEDTLS_MEM_DEBUG
#define ALTCP_MBEDTLS_MEM_DEBUG LWIP_DBG_OFF
#endif
#if defined(MBEDTLS_PLATFORM_MEMORY) && \
(!defined(MBEDTLS_PLATFORM_FREE_MACRO) || \
defined(MBEDTLS_PLATFORM_CALLOC_MACRO))
#define ALTCP_MBEDTLS_PLATFORM_ALLOC 1
#else
#define ALTCP_MBEDTLS_PLATFORM_ALLOC 0
#endif
#if ALTCP_MBEDTLS_PLATFORM_ALLOC
#ifndef ALTCP_MBEDTLS_PLATFORM_ALLOC_STATS
#define ALTCP_MBEDTLS_PLATFORM_ALLOC_STATS 0
#endif
/* This is an example/debug implementation of alloc/free functions only */
typedef struct altcp_mbedtls_malloc_helper_s {
size_t c;
size_t len;
} altcp_mbedtls_malloc_helper_t;
#if ALTCP_MBEDTLS_PLATFORM_ALLOC_STATS
typedef struct altcp_mbedtls_malloc_stats_s {
size_t allocedBytes;
size_t allocCnt;
size_t maxBytes;
size_t totalBytes;
} altcp_mbedtls_malloc_stats_t;
altcp_mbedtls_malloc_stats_t altcp_mbedtls_malloc_stats;
volatile int altcp_mbedtls_malloc_clear_stats;
#endif
static void *
tls_malloc(size_t c, size_t len)
{
altcp_mbedtls_malloc_helper_t *hlpr;
void *ret;
size_t alloc_size;
#if ALTCP_MBEDTLS_PLATFORM_ALLOC_STATS
if (altcp_mbedtls_malloc_clear_stats) {
altcp_mbedtls_malloc_clear_stats = 0;
memset(&altcp_mbedtls_malloc_stats, 0, sizeof(altcp_mbedtls_malloc_stats));
}
#endif
alloc_size = sizeof(altcp_mbedtls_malloc_helper_t) + (c * len);
/* check for maximum allocation size, mainly to prevent mem_size_t overflow */
if (alloc_size > MEM_SIZE) {
LWIP_DEBUGF(ALTCP_MBEDTLS_MEM_DEBUG, ("mbedtls allocation too big: %c * %d bytes vs MEM_SIZE=%d",
(int)c, (int)len, (int)MEM_SIZE));
return NULL;
}
hlpr = (altcp_mbedtls_malloc_helper_t *)mem_malloc((mem_size_t)alloc_size);
if (hlpr == NULL) {
LWIP_DEBUGF(ALTCP_MBEDTLS_MEM_DEBUG, ("mbedtls alloc callback failed for %c * %d bytes", (int)c, (int)len));
return NULL;
}
#if ALTCP_MBEDTLS_PLATFORM_ALLOC_STATS
altcp_mbedtls_malloc_stats.allocCnt++;
altcp_mbedtls_malloc_stats.allocedBytes += c * len;
if (altcp_mbedtls_malloc_stats.allocedBytes > altcp_mbedtls_malloc_stats.maxBytes) {
altcp_mbedtls_malloc_stats.maxBytes = altcp_mbedtls_malloc_stats.allocedBytes;
}
altcp_mbedtls_malloc_stats.totalBytes += c * len;
#endif
hlpr->c = c;
hlpr->len = len;
ret = hlpr + 1;
/* zeroing the allocated chunk is required by mbedTLS! */
memset(ret, 0, c * len);
return ret;
}
static void
tls_free(void *ptr)
{
altcp_mbedtls_malloc_helper_t *hlpr;
if (ptr == NULL) {
/* this obviously happened in mbedtls... */
return;
}
hlpr = ((altcp_mbedtls_malloc_helper_t *)ptr) - 1;
#if ALTCP_MBEDTLS_PLATFORM_ALLOC_STATS
if (!altcp_mbedtls_malloc_clear_stats) {
altcp_mbedtls_malloc_stats.allocedBytes -= hlpr->c * hlpr->len;
}
#endif
mem_free(hlpr);
}
#endif /* ALTCP_MBEDTLS_PLATFORM_ALLOC*/
void
altcp_mbedtls_mem_init(void)
{
/* not much to do here when using the heap */
#if ALTCP_MBEDTLS_PLATFORM_ALLOC
/* set mbedtls allocation methods */
mbedtls_platform_set_calloc_free(&tls_malloc, &tls_free);
#endif
}
altcp_mbedtls_state_t *
altcp_mbedtls_alloc(void *conf)
{
altcp_mbedtls_state_t *ret = (altcp_mbedtls_state_t *)mem_calloc(1, sizeof(altcp_mbedtls_state_t));
if (ret != NULL) {
ret->conf = conf;
}
return ret;
}
void
altcp_mbedtls_free(void *conf, altcp_mbedtls_state_t *state)
{
LWIP_UNUSED_ARG(conf);
LWIP_ASSERT("state != NULL", state != NULL);
mem_free(state);
}
void *
altcp_mbedtls_alloc_config(size_t size)
{
void *ret;
size_t checked_size = (mem_size_t)size;
if (size != checked_size) {
/* allocation too big (mem_size_t overflow) */
return NULL;
}
ret = (altcp_mbedtls_state_t *)mem_calloc(1, (mem_size_t)size);
return ret;
}
void
altcp_mbedtls_free_config(void *item)
{
LWIP_ASSERT("item != NULL", item != NULL);
mem_free(item);
}
#endif /* LWIP_ALTCP_TLS && LWIP_ALTCP_TLS_MBEDTLS */
#endif /* LWIP_ALTCP */

View File

@ -0,0 +1,72 @@
/**
* @file
* Application layered TCP/TLS connection API (to be used from TCPIP thread)
*
* This file contains memory management function prototypes for a TLS layer using mbedTLS.
*
* Memory management contains:
* - allocating/freeing altcp_mbedtls_state_t
* - allocating/freeing memory used in the mbedTLS library
*/
/*
* Copyright (c) 2017 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt <goldsimon@gmx.de>
*
*/
#ifndef LWIP_HDR_ALTCP_MBEDTLS_MEM_H
#define LWIP_HDR_ALTCP_MBEDTLS_MEM_H
#include "lwip/opt.h"
#if LWIP_ALTCP /* don't build if not configured for use in lwipopts.h */
#include "lwip/apps/altcp_tls_mbedtls_opts.h"
#if LWIP_ALTCP_TLS && LWIP_ALTCP_TLS_MBEDTLS
#include "altcp_tls_mbedtls_structs.h"
#ifdef __cplusplus
extern "C" {
#endif
void altcp_mbedtls_mem_init(void);
altcp_mbedtls_state_t *altcp_mbedtls_alloc(void *conf);
void altcp_mbedtls_free(void *conf, altcp_mbedtls_state_t *state);
void *altcp_mbedtls_alloc_config(size_t size);
void altcp_mbedtls_free_config(void *item);
#ifdef __cplusplus
}
#endif
#endif /* LWIP_ALTCP_TLS && LWIP_ALTCP_TLS_MBEDTLS */
#endif /* LWIP_ALTCP */
#endif /* LWIP_HDR_ALTCP_MBEDTLS_MEM_H */

View File

@ -0,0 +1,83 @@
/**
* @file
* Application layered TCP/TLS connection API (to be used from TCPIP thread)
*
* This file contains structure definitions for a TLS layer using mbedTLS.
*/
/*
* Copyright (c) 2017 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt <goldsimon@gmx.de>
*
*/
#ifndef LWIP_HDR_ALTCP_MBEDTLS_STRUCTS_H
#define LWIP_HDR_ALTCP_MBEDTLS_STRUCTS_H
#include "lwip/opt.h"
#if LWIP_ALTCP /* don't build if not configured for use in lwipopts.h */
#include "lwip/apps/altcp_tls_mbedtls_opts.h"
#if LWIP_ALTCP_TLS && LWIP_ALTCP_TLS_MBEDTLS
#include "lwip/altcp.h"
#include "lwip/pbuf.h"
#include "mbedtls/ssl.h"
#ifdef __cplusplus
extern "C" {
#endif
#define ALTCP_MBEDTLS_FLAGS_HANDSHAKE_DONE 0x01
#define ALTCP_MBEDTLS_FLAGS_UPPER_CALLED 0x02
#define ALTCP_MBEDTLS_FLAGS_RX_CLOSE_QUEUED 0x04
#define ALTCP_MBEDTLS_FLAGS_RX_CLOSED 0x08
#define ALTCP_MBEDTLS_FLAGS_APPLDATA_SENT 0x10
typedef struct altcp_mbedtls_state_s {
void *conf;
mbedtls_ssl_context ssl_context;
/* chain of rx pbufs (before decryption) */
struct pbuf *rx;
struct pbuf *rx_app;
u8_t flags;
int rx_passed_unrecved;
int bio_bytes_read;
int bio_bytes_appl;
} altcp_mbedtls_state_t;
#ifdef __cplusplus
}
#endif
#endif /* LWIP_ALTCP_TLS && LWIP_ALTCP_TLS_MBEDTLS */
#endif /* LWIP_ALTCP */
#endif /* LWIP_HDR_ALTCP_MBEDTLS_STRUCTS_H */

View File

@ -0,0 +1,584 @@
/**
* @file
* Application layered TCP connection API that executes a proxy-connect.
*
* This file provides a starting layer that executes a proxy-connect e.g. to
* set up TLS connections through a http proxy.
*/
/*
* Copyright (c) 2018 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt <goldsimon@gmx.de>
*
*/
#include "lwip/apps/altcp_proxyconnect.h"
#if LWIP_ALTCP /* don't build if not configured for use in lwipopts.h */
#include "lwip/altcp.h"
#include "lwip/priv/altcp_priv.h"
#include "lwip/altcp_tcp.h"
#include "lwip/altcp_tls.h"
#include "lwip/mem.h"
#include "lwip/init.h"
#include <stdio.h>
/** This string is passed in the HTTP header as "User-Agent: " */
#ifndef ALTCP_PROXYCONNECT_CLIENT_AGENT
#define ALTCP_PROXYCONNECT_CLIENT_AGENT "lwIP/" LWIP_VERSION_STRING " (http://savannah.nongnu.org/projects/lwip)"
#endif
#define ALTCP_PROXYCONNECT_FLAGS_CONNECT_STARTED 0x01
#define ALTCP_PROXYCONNECT_FLAGS_HANDSHAKE_DONE 0x02
typedef struct altcp_proxyconnect_state_s
{
ip_addr_t outer_addr;
u16_t outer_port;
struct altcp_proxyconnect_config *conf;
u8_t flags;
} altcp_proxyconnect_state_t;
/* Variable prototype, the actual declaration is at the end of this file
since it contains pointers to static functions declared here */
extern const struct altcp_functions altcp_proxyconnect_functions;
/* memory management functions: */
static altcp_proxyconnect_state_t *
altcp_proxyconnect_state_alloc(void)
{
altcp_proxyconnect_state_t *ret = (altcp_proxyconnect_state_t *)mem_calloc(1, sizeof(altcp_proxyconnect_state_t));
return ret;
}
static void
altcp_proxyconnect_state_free(altcp_proxyconnect_state_t *state)
{
LWIP_ASSERT("state != NULL", state != NULL);
mem_free(state);
}
/* helper functions */
#define PROXY_CONNECT "CONNECT %s:%d HTTP/1.1\r\n" /* HOST, PORT */ \
"User-Agent: %s\r\n" /* User-Agent */\
"Proxy-Connection: keep-alive\r\n" \
"Connection: keep-alive\r\n" \
"\r\n"
#define PROXY_CONNECT_FORMAT(host, port) PROXY_CONNECT, host, port, ALTCP_PROXYCONNECT_CLIENT_AGENT
/* Format the http proxy connect request via snprintf */
static int
altcp_proxyconnect_format_request(char *buffer, size_t bufsize, const char *host, int port)
{
return snprintf(buffer, bufsize, PROXY_CONNECT_FORMAT(host, port));
}
/* Create and send the http proxy connect request */
static err_t
altcp_proxyconnect_send_request(struct altcp_pcb *conn)
{
int len, len2;
mem_size_t alloc_len;
char *buffer, *host;
altcp_proxyconnect_state_t *state = (altcp_proxyconnect_state_t *)conn->state;
if (!state) {
return ERR_VAL;
}
/* Use printf with zero length to get the required allocation size */
len = altcp_proxyconnect_format_request(NULL, 0, "", state->outer_port);
if (len < 0) {
return ERR_VAL;
}
/* add allocation size for IP address strings */
#if LWIP_IPV6
len += 40; /* worst-case IPv6 address length */
#else
len += 16; /* worst-case IPv4 address length */
#endif
alloc_len = (mem_size_t)len;
if ((len < 0) || (int)alloc_len != len) {
/* overflow */
return ERR_MEM;
}
/* Allocate a bufer for the request string */
buffer = (char *)mem_malloc(alloc_len);
if (buffer == NULL) {
return ERR_MEM;
}
host = ipaddr_ntoa(&state->outer_addr);
len2 = altcp_proxyconnect_format_request(buffer, alloc_len, host, state->outer_port);
if ((len2 > 0) && (len2 <= len) && (len2 <= 0xFFFF)) {
err_t err = altcp_write(conn->inner_conn, buffer, (u16_t)len2, TCP_WRITE_FLAG_COPY);
if (err != ERR_OK) {
/* @todo: abort? */
mem_free(buffer);
return err;
}
}
mem_free(buffer);
return ERR_OK;
}
/* callback functions from inner/lower connection: */
/** Connected callback from lower connection (i.e. TCP).
* Not really implemented/tested yet...
*/
static err_t
altcp_proxyconnect_lower_connected(void *arg, struct altcp_pcb *inner_conn, err_t err)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
if (conn && conn->state) {
LWIP_ASSERT("pcb mismatch", conn->inner_conn == inner_conn);
LWIP_UNUSED_ARG(inner_conn); /* for LWIP_NOASSERT */
/* upper connected is called when handshake is done */
if (err != ERR_OK) {
if (conn->connected) {
if (conn->connected(conn->arg, conn, err) == ERR_ABRT) {
return ERR_ABRT;
}
return ERR_OK;
}
}
/* send proxy connect request here */
return altcp_proxyconnect_send_request(conn);
}
return ERR_VAL;
}
/** Recv callback from lower connection (i.e. TCP)
* This one mainly differs between connection setup (wait for proxy OK string)
* and application phase (data is passed on to the application).
*/
static err_t
altcp_proxyconnect_lower_recv(void *arg, struct altcp_pcb *inner_conn, struct pbuf *p, err_t err)
{
altcp_proxyconnect_state_t *state;
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
LWIP_ASSERT("no err expected", err == ERR_OK);
LWIP_UNUSED_ARG(err);
if (!conn) {
/* no connection given as arg? should not happen, but prevent pbuf/conn leaks */
if (p != NULL) {
pbuf_free(p);
}
altcp_close(inner_conn);
return ERR_CLSD;
}
state = (altcp_proxyconnect_state_t *)conn->state;
LWIP_ASSERT("pcb mismatch", conn->inner_conn == inner_conn);
if (!state) {
/* already closed */
if (p != NULL) {
pbuf_free(p);
}
altcp_close(inner_conn);
return ERR_CLSD;
}
if (state->flags & ALTCP_PROXYCONNECT_FLAGS_HANDSHAKE_DONE) {
/* application phase, just pass this through */
if (conn->recv) {
return conn->recv(conn->arg, conn, p, err);
}
pbuf_free(p);
return ERR_OK;
} else {
/* setup phase */
/* handle NULL pbuf (inner connection closed) */
if (p == NULL) {
if (altcp_close(conn) != ERR_OK) {
altcp_abort(conn);
return ERR_ABRT;
}
return ERR_OK;
} else {
/* @todo: parse setup phase rx data
for now, we just wait for the end of the header... */
u16_t idx = pbuf_memfind(p, "\r\n\r\n", 4, 0);
altcp_recved(inner_conn, p->tot_len);
pbuf_free(p);
if (idx != 0xFFFF) {
state->flags |= ALTCP_PROXYCONNECT_FLAGS_HANDSHAKE_DONE;
if (conn->connected) {
return conn->connected(conn->arg, conn, ERR_OK);
}
}
return ERR_OK;
}
}
}
/** Sent callback from lower connection (i.e. TCP)
* This only informs the upper layer to try to send more, not about
* the number of ACKed bytes.
*/
static err_t
altcp_proxyconnect_lower_sent(void *arg, struct altcp_pcb *inner_conn, u16_t len)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
LWIP_UNUSED_ARG(len);
if (conn) {
altcp_proxyconnect_state_t *state = (altcp_proxyconnect_state_t *)conn->state;
LWIP_ASSERT("pcb mismatch", conn->inner_conn == inner_conn);
LWIP_UNUSED_ARG(inner_conn); /* for LWIP_NOASSERT */
if (!state || !(state->flags & ALTCP_PROXYCONNECT_FLAGS_HANDSHAKE_DONE)) {
/* @todo: do something here? */
return ERR_OK;
}
/* pass this on to upper sent */
if (conn->sent) {
return conn->sent(conn->arg, conn, len);
}
}
return ERR_OK;
}
/** Poll callback from lower connection (i.e. TCP)
* Just pass this on to the application.
* @todo: retry sending?
*/
static err_t
altcp_proxyconnect_lower_poll(void *arg, struct altcp_pcb *inner_conn)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
if (conn) {
LWIP_ASSERT("pcb mismatch", conn->inner_conn == inner_conn);
LWIP_UNUSED_ARG(inner_conn); /* for LWIP_NOASSERT */
if (conn->poll) {
return conn->poll(conn->arg, conn);
}
}
return ERR_OK;
}
static void
altcp_proxyconnect_lower_err(void *arg, err_t err)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
if (conn) {
conn->inner_conn = NULL; /* already freed */
if (conn->err) {
conn->err(conn->arg, err);
}
altcp_free(conn);
}
}
/* setup functions */
static void
altcp_proxyconnect_setup_callbacks(struct altcp_pcb *conn, struct altcp_pcb *inner_conn)
{
altcp_arg(inner_conn, conn);
altcp_recv(inner_conn, altcp_proxyconnect_lower_recv);
altcp_sent(inner_conn, altcp_proxyconnect_lower_sent);
altcp_err(inner_conn, altcp_proxyconnect_lower_err);
/* tcp_poll is set when interval is set by application */
/* listen is set totally different :-) */
}
static err_t
altcp_proxyconnect_setup(struct altcp_proxyconnect_config *config, struct altcp_pcb *conn, struct altcp_pcb *inner_conn)
{
altcp_proxyconnect_state_t *state;
if (!config) {
return ERR_ARG;
}
LWIP_ASSERT("invalid inner_conn", conn != inner_conn);
/* allocate proxyconnect context */
state = altcp_proxyconnect_state_alloc();
if (state == NULL) {
return ERR_MEM;
}
state->flags = 0;
state->conf = config;
altcp_proxyconnect_setup_callbacks(conn, inner_conn);
conn->inner_conn = inner_conn;
conn->fns = &altcp_proxyconnect_functions;
conn->state = state;
return ERR_OK;
}
/** Allocate a new altcp layer connecting through a proxy.
* This function gets the inner pcb passed.
*
* @param config struct altcp_proxyconnect_config that contains the proxy settings
* @param inner_pcb pcb that makes the connection to the proxy (i.e. tcp pcb)
*/
struct altcp_pcb *
altcp_proxyconnect_new(struct altcp_proxyconnect_config *config, struct altcp_pcb *inner_pcb)
{
struct altcp_pcb *ret;
if (inner_pcb == NULL) {
return NULL;
}
ret = altcp_alloc();
if (ret != NULL) {
if (altcp_proxyconnect_setup(config, ret, inner_pcb) != ERR_OK) {
altcp_free(ret);
return NULL;
}
}
return ret;
}
/** Allocate a new altcp layer connecting through a proxy.
* This function allocates the inner pcb as tcp pcb, resulting in a direct tcp
* connection to the proxy.
*
* @param config struct altcp_proxyconnect_config that contains the proxy settings
* @param ip_type IP type of the connection (@ref lwip_ip_addr_type)
*/
struct altcp_pcb *
altcp_proxyconnect_new_tcp(struct altcp_proxyconnect_config *config, u8_t ip_type)
{
struct altcp_pcb *inner_pcb, *ret;
/* inner pcb is tcp */
inner_pcb = altcp_tcp_new_ip_type(ip_type);
if (inner_pcb == NULL) {
return NULL;
}
ret = altcp_proxyconnect_new(config, inner_pcb);
if (ret == NULL) {
altcp_close(inner_pcb);
}
return ret;
}
/** Allocator function to allocate a proxy connect altcp pcb connecting directly
* via tcp to the proxy.
*
* The returned pcb is a chain: altcp_proxyconnect - altcp_tcp - tcp pcb
*
* This function is meant for use with @ref altcp_new.
*
* @param arg struct altcp_proxyconnect_config that contains the proxy settings
* @param ip_type IP type of the connection (@ref lwip_ip_addr_type)
*/
struct altcp_pcb *
altcp_proxyconnect_alloc(void *arg, u8_t ip_type)
{
return altcp_proxyconnect_new_tcp((struct altcp_proxyconnect_config *)arg, ip_type);
}
#if LWIP_ALTCP_TLS
/** Allocator function to allocate a TLS connection through a proxy.
*
* The returned pcb is a chain: altcp_tls - altcp_proxyconnect - altcp_tcp - tcp pcb
*
* This function is meant for use with @ref altcp_new.
*
* @param arg struct altcp_proxyconnect_tls_config that contains the proxy settings
* and tls settings
* @param ip_type IP type of the connection (@ref lwip_ip_addr_type)
*/
struct altcp_pcb *
altcp_proxyconnect_tls_alloc(void *arg, u8_t ip_type)
{
struct altcp_proxyconnect_tls_config *cfg = (struct altcp_proxyconnect_tls_config *)arg;
struct altcp_pcb *proxy_pcb;
struct altcp_pcb *tls_pcb;
proxy_pcb = altcp_proxyconnect_new_tcp(&cfg->proxy, ip_type);
tls_pcb = altcp_tls_wrap(cfg->tls_config, proxy_pcb);
if (tls_pcb == NULL) {
altcp_close(proxy_pcb);
}
return tls_pcb;
}
#endif /* LWIP_ALTCP_TLS */
/* "virtual" functions */
static void
altcp_proxyconnect_set_poll(struct altcp_pcb *conn, u8_t interval)
{
if (conn != NULL) {
altcp_poll(conn->inner_conn, altcp_proxyconnect_lower_poll, interval);
}
}
static void
altcp_proxyconnect_recved(struct altcp_pcb *conn, u16_t len)
{
altcp_proxyconnect_state_t *state;
if (conn == NULL) {
return;
}
state = (altcp_proxyconnect_state_t *)conn->state;
if (state == NULL) {
return;
}
if (!(state->flags & ALTCP_PROXYCONNECT_FLAGS_HANDSHAKE_DONE)) {
return;
}
altcp_recved(conn->inner_conn, len);
}
static err_t
altcp_proxyconnect_connect(struct altcp_pcb *conn, const ip_addr_t *ipaddr, u16_t port, altcp_connected_fn connected)
{
altcp_proxyconnect_state_t *state;
if ((conn == NULL) || (ipaddr == NULL)) {
return ERR_VAL;
}
state = (altcp_proxyconnect_state_t *)conn->state;
if (state == NULL) {
return ERR_VAL;
}
if (state->flags & ALTCP_PROXYCONNECT_FLAGS_CONNECT_STARTED) {
return ERR_VAL;
}
state->flags |= ALTCP_PROXYCONNECT_FLAGS_CONNECT_STARTED;
conn->connected = connected;
/* connect to our proxy instead, but store the requested address and port */
ip_addr_copy(state->outer_addr, *ipaddr);
state->outer_port = port;
return altcp_connect(conn->inner_conn, &state->conf->proxy_addr, state->conf->proxy_port, altcp_proxyconnect_lower_connected);
}
static struct altcp_pcb *
altcp_proxyconnect_listen(struct altcp_pcb *conn, u8_t backlog, err_t *err)
{
LWIP_UNUSED_ARG(conn);
LWIP_UNUSED_ARG(backlog);
LWIP_UNUSED_ARG(err);
/* listen not supported! */
return NULL;
}
static void
altcp_proxyconnect_abort(struct altcp_pcb *conn)
{
if (conn != NULL) {
if (conn->inner_conn != NULL) {
altcp_abort(conn->inner_conn);
}
altcp_free(conn);
}
}
static err_t
altcp_proxyconnect_close(struct altcp_pcb *conn)
{
if (conn == NULL) {
return ERR_VAL;
}
if (conn->inner_conn != NULL) {
err_t err = altcp_close(conn->inner_conn);
if (err != ERR_OK) {
/* closing inner conn failed, return the error */
return err;
}
}
/* no inner conn or closing it succeeded, deallocate myself */
altcp_free(conn);
return ERR_OK;
}
static err_t
altcp_proxyconnect_write(struct altcp_pcb *conn, const void *dataptr, u16_t len, u8_t apiflags)
{
altcp_proxyconnect_state_t *state;
LWIP_UNUSED_ARG(apiflags);
if (conn == NULL) {
return ERR_VAL;
}
state = (altcp_proxyconnect_state_t *)conn->state;
if (state == NULL) {
/* @todo: which error? */
return ERR_CLSD;
}
if (!(state->flags & ALTCP_PROXYCONNECT_FLAGS_HANDSHAKE_DONE)) {
/* @todo: which error? */
return ERR_VAL;
}
return altcp_write(conn->inner_conn, dataptr, len, apiflags);
}
static void
altcp_proxyconnect_dealloc(struct altcp_pcb *conn)
{
/* clean up and free tls state */
if (conn) {
altcp_proxyconnect_state_t *state = (altcp_proxyconnect_state_t *)conn->state;
if (state) {
altcp_proxyconnect_state_free(state);
conn->state = NULL;
}
}
}
const struct altcp_functions altcp_proxyconnect_functions = {
altcp_proxyconnect_set_poll,
altcp_proxyconnect_recved,
altcp_default_bind,
altcp_proxyconnect_connect,
altcp_proxyconnect_listen,
altcp_proxyconnect_abort,
altcp_proxyconnect_close,
altcp_default_shutdown,
altcp_proxyconnect_write,
altcp_default_output,
altcp_default_mss,
altcp_default_sndbuf,
altcp_default_sndqueuelen,
altcp_default_nagle_disable,
altcp_default_nagle_enable,
altcp_default_nagle_disabled,
altcp_default_setprio,
altcp_proxyconnect_dealloc,
altcp_default_get_tcp_addrinfo,
altcp_default_get_ip,
altcp_default_get_port
#ifdef LWIP_DEBUG
, altcp_default_dbg_get_tcp_state
#endif
};
#endif /* LWIP_ALTCP */

View File

@ -0,0 +1,174 @@
/*
* Copyright (c) 2001-2003 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#include "lwip/apps/httpd_opts.h"
#include "lwip/def.h"
#include "lwip/apps/fs.h"
#include <string.h>
#include HTTPD_FSDATA_FILE
/*-----------------------------------------------------------------------------------*/
#if LWIP_HTTPD_CUSTOM_FILES
int fs_open_custom(struct fs_file *file, const char *name);
void fs_close_custom(struct fs_file *file);
#if LWIP_HTTPD_FS_ASYNC_READ
u8_t fs_canread_custom(struct fs_file *file);
u8_t fs_wait_read_custom(struct fs_file *file, fs_wait_cb callback_fn, void *callback_arg);
int fs_read_async_custom(struct fs_file *file, char *buffer, int count, fs_wait_cb callback_fn, void *callback_arg);
#else /* LWIP_HTTPD_FS_ASYNC_READ */
int fs_read_custom(struct fs_file *file, char *buffer, int count);
#endif /* LWIP_HTTPD_FS_ASYNC_READ */
#endif /* LWIP_HTTPD_CUSTOM_FILES */
/*-----------------------------------------------------------------------------------*/
err_t
fs_open(struct fs_file *file, const char *name)
{
const struct fsdata_file *f;
if ((file == NULL) || (name == NULL)) {
return ERR_ARG;
}
#if LWIP_HTTPD_CUSTOM_FILES
if (fs_open_custom(file, name)) {
file->is_custom_file = 1;
return ERR_OK;
}
file->is_custom_file = 0;
#endif /* LWIP_HTTPD_CUSTOM_FILES */
for (f = FS_ROOT; f != NULL; f = f->next) {
if (!strcmp(name, (const char *)f->name)) {
file->data = (const char *)f->data;
file->len = f->len;
file->index = f->len;
file->pextension = NULL;
file->flags = f->flags;
#if HTTPD_PRECALCULATED_CHECKSUM
file->chksum_count = f->chksum_count;
file->chksum = f->chksum;
#endif /* HTTPD_PRECALCULATED_CHECKSUM */
#if LWIP_HTTPD_FILE_STATE
file->state = fs_state_init(file, name);
#endif /* #if LWIP_HTTPD_FILE_STATE */
return ERR_OK;
}
}
/* file not found */
return ERR_VAL;
}
/*-----------------------------------------------------------------------------------*/
void
fs_close(struct fs_file *file)
{
#if LWIP_HTTPD_CUSTOM_FILES
if (file->is_custom_file) {
fs_close_custom(file);
}
#endif /* LWIP_HTTPD_CUSTOM_FILES */
#if LWIP_HTTPD_FILE_STATE
fs_state_free(file, file->state);
#endif /* #if LWIP_HTTPD_FILE_STATE */
LWIP_UNUSED_ARG(file);
}
/*-----------------------------------------------------------------------------------*/
#if LWIP_HTTPD_DYNAMIC_FILE_READ
#if LWIP_HTTPD_FS_ASYNC_READ
int
fs_read_async(struct fs_file *file, char *buffer, int count, fs_wait_cb callback_fn, void *callback_arg)
#else /* LWIP_HTTPD_FS_ASYNC_READ */
int
fs_read(struct fs_file *file, char *buffer, int count)
#endif /* LWIP_HTTPD_FS_ASYNC_READ */
{
int read;
if (file->index == file->len) {
return FS_READ_EOF;
}
#if LWIP_HTTPD_FS_ASYNC_READ
LWIP_UNUSED_ARG(callback_fn);
LWIP_UNUSED_ARG(callback_arg);
#endif /* LWIP_HTTPD_FS_ASYNC_READ */
#if LWIP_HTTPD_CUSTOM_FILES
if (file->is_custom_file) {
#if LWIP_HTTPD_FS_ASYNC_READ
return fs_read_async_custom(file, buffer, count, callback_fn, callback_arg);
#else /* LWIP_HTTPD_FS_ASYNC_READ */
return fs_read_custom(file, buffer, count);
#endif /* LWIP_HTTPD_FS_ASYNC_READ */
}
#endif /* LWIP_HTTPD_CUSTOM_FILES */
read = file->len - file->index;
if (read > count) {
read = count;
}
MEMCPY(buffer, (file->data + file->index), read);
file->index += read;
return (read);
}
#endif /* LWIP_HTTPD_DYNAMIC_FILE_READ */
/*-----------------------------------------------------------------------------------*/
#if LWIP_HTTPD_FS_ASYNC_READ
int
fs_is_file_ready(struct fs_file *file, fs_wait_cb callback_fn, void *callback_arg)
{
if (file != NULL) {
#if LWIP_HTTPD_FS_ASYNC_READ
#if LWIP_HTTPD_CUSTOM_FILES
if (!fs_canread_custom(file)) {
if (fs_wait_read_custom(file, callback_fn, callback_arg)) {
return 0;
}
}
#else /* LWIP_HTTPD_CUSTOM_FILES */
LWIP_UNUSED_ARG(callback_fn);
LWIP_UNUSED_ARG(callback_arg);
#endif /* LWIP_HTTPD_CUSTOM_FILES */
#endif /* LWIP_HTTPD_FS_ASYNC_READ */
}
return 1;
}
#endif /* LWIP_HTTPD_FS_ASYNC_READ */
/*-----------------------------------------------------------------------------------*/
int
fs_bytes_left(struct fs_file *file)
{
return file->len - file->index;
}

Binary file not shown.

After

Width:  |  Height:  |  Size: 724 B

View File

@ -0,0 +1,337 @@
#include "lwip/apps/fs.h"
#include "lwip/def.h"
#define file_NULL (struct fsdata_file *) NULL
#ifndef FS_FILE_FLAGS_HEADER_INCLUDED
#define FS_FILE_FLAGS_HEADER_INCLUDED 1
#endif
#ifndef FS_FILE_FLAGS_HEADER_PERSISTENT
#define FS_FILE_FLAGS_HEADER_PERSISTENT 0
#endif
/* FSDATA_FILE_ALIGNMENT: 0=off, 1=by variable, 2=by include */
#ifndef FSDATA_FILE_ALIGNMENT
#define FSDATA_FILE_ALIGNMENT 0
#endif
#ifndef FSDATA_ALIGN_PRE
#define FSDATA_ALIGN_PRE
#endif
#ifndef FSDATA_ALIGN_POST
#define FSDATA_ALIGN_POST
#endif
#if FSDATA_FILE_ALIGNMENT==2
#include "fsdata_alignment.h"
#endif
#if FSDATA_FILE_ALIGNMENT==1
static const unsigned int dummy_align__img_sics_gif = 0;
#endif
static const unsigned char FSDATA_ALIGN_PRE data__img_sics_gif[] FSDATA_ALIGN_POST = {
/* /img/sics.gif (14 chars) */
0x2f,0x69,0x6d,0x67,0x2f,0x73,0x69,0x63,0x73,0x2e,0x67,0x69,0x66,0x00,0x00,0x00,
/* HTTP header */
/* "HTTP/1.0 200 OK
" (17 bytes) */
0x48,0x54,0x54,0x50,0x2f,0x31,0x2e,0x30,0x20,0x32,0x30,0x30,0x20,0x4f,0x4b,0x0d,
0x0a,
/* "Server: lwIP/2.0.3d (http://savannah.nongnu.org/projects/lwip)
" (64 bytes) */
0x53,0x65,0x72,0x76,0x65,0x72,0x3a,0x20,0x6c,0x77,0x49,0x50,0x2f,0x32,0x2e,0x30,
0x2e,0x33,0x64,0x20,0x28,0x68,0x74,0x74,0x70,0x3a,0x2f,0x2f,0x73,0x61,0x76,0x61,
0x6e,0x6e,0x61,0x68,0x2e,0x6e,0x6f,0x6e,0x67,0x6e,0x75,0x2e,0x6f,0x72,0x67,0x2f,
0x70,0x72,0x6f,0x6a,0x65,0x63,0x74,0x73,0x2f,0x6c,0x77,0x69,0x70,0x29,0x0d,0x0a,
/* "Content-Length: 724
" (18+ bytes) */
0x43,0x6f,0x6e,0x74,0x65,0x6e,0x74,0x2d,0x4c,0x65,0x6e,0x67,0x74,0x68,0x3a,0x20,
0x37,0x32,0x34,0x0d,0x0a,
/* "Content-Type: image/gif
" (27 bytes) */
0x43,0x6f,0x6e,0x74,0x65,0x6e,0x74,0x2d,0x54,0x79,0x70,0x65,0x3a,0x20,0x69,0x6d,
0x61,0x67,0x65,0x2f,0x67,0x69,0x66,0x0d,0x0a,0x0d,0x0a,
/* raw file data (724 bytes) */
0x47,0x49,0x46,0x38,0x39,0x61,0x46,0x00,0x22,0x00,0xa5,0x00,0x00,0xd9,0x2b,0x39,
0x6a,0x6a,0x6a,0xbf,0xbf,0xbf,0x93,0x93,0x93,0x0f,0x0f,0x0f,0xb0,0xb0,0xb0,0xa6,
0xa6,0xa6,0x80,0x80,0x80,0x76,0x76,0x76,0x1e,0x1e,0x1e,0x9d,0x9d,0x9d,0x2e,0x2e,
0x2e,0x49,0x49,0x49,0x54,0x54,0x54,0x8a,0x8a,0x8a,0x60,0x60,0x60,0xc6,0xa6,0x99,
0xbd,0xb5,0xb2,0xc2,0xab,0xa1,0xd9,0x41,0x40,0xd5,0x67,0x55,0xc0,0xb0,0xaa,0xd5,
0x5e,0x4e,0xd6,0x50,0x45,0xcc,0x93,0x7d,0xc8,0xa1,0x90,0xce,0x8b,0x76,0xd2,0x7b,
0x65,0xd1,0x84,0x6d,0xc9,0x99,0x86,0x3a,0x3a,0x3a,0x00,0x00,0x00,0xb8,0xb8,0xb8,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0x2c,0x00,0x00,
0x00,0x00,0x46,0x00,0x22,0x00,0x00,0x06,0xfe,0x40,0x90,0x70,0x48,0x2c,0x1a,0x8f,
0xc8,0xa4,0x72,0xc9,0x6c,0x3a,0x9f,0xd0,0xa8,0x74,0x4a,0xad,0x5a,0xaf,0xd8,0xac,
0x76,0xa9,0x40,0x04,0xbe,0x83,0xe2,0x60,0x3c,0x50,0x20,0x0d,0x8e,0x6f,0x00,0x31,
0x28,0x1c,0x0d,0x07,0xb5,0xc3,0x60,0x75,0x24,0x3e,0xf8,0xfc,0x87,0x11,0x06,0xe9,
0x3d,0x46,0x07,0x0b,0x7a,0x7a,0x7c,0x43,0x06,0x1e,0x84,0x78,0x0b,0x07,0x6e,0x51,
0x01,0x8a,0x84,0x08,0x7e,0x79,0x80,0x87,0x89,0x91,0x7a,0x93,0x0a,0x04,0x99,0x78,
0x96,0x4f,0x03,0x9e,0x79,0x01,0x94,0x9f,0x43,0x9c,0xa3,0xa4,0x05,0x77,0xa3,0xa0,
0x4e,0x98,0x79,0x0b,0x1e,0x83,0xa4,0xa6,0x1f,0x96,0x05,0x9d,0xaa,0x78,0x01,0x07,
0x84,0x04,0x1e,0x1e,0xbb,0xb8,0x51,0x84,0x0e,0x43,0x05,0x07,0x77,0xa5,0x7f,0x42,
0xb1,0xb2,0x01,0x63,0x08,0x0d,0xbb,0x01,0x0c,0x7a,0x0d,0x44,0x0e,0xd8,0xaf,0x4c,
0x05,0x7a,0x04,0x47,0x07,0x07,0xb7,0x80,0xa2,0xe1,0x7d,0x44,0x05,0x01,0x04,0x01,
0xd0,0xea,0x87,0x93,0x4f,0xe0,0x9a,0x49,0xce,0xd8,0x79,0x04,0x66,0x20,0x15,0x10,
0x10,0x11,0x92,0x29,0x80,0xb6,0xc0,0x91,0x15,0x45,0x1e,0x90,0x19,0x71,0x46,0xa8,
0x5c,0x04,0x0e,0x00,0x22,0x4e,0xe8,0x40,0x24,0x9f,0x3e,0x04,0x06,0xa7,0x58,0xd4,
0x93,0xa0,0x1c,0x91,0x3f,0xe8,0xf0,0x88,0x03,0xb1,0x21,0xa2,0x49,0x00,0x19,0x86,
0xfc,0x52,0x44,0xe0,0x01,0x9d,0x29,0x21,0x15,0x25,0x50,0xf7,0x67,0x25,0x1e,0x06,
0xfd,0x4e,0x9a,0xb4,0x90,0xac,0x15,0xfa,0xcb,0x52,0x53,0x1e,0x8c,0xf2,0xf8,0x07,
0x92,0x2d,0x08,0x3a,0x4d,0x12,0x49,0x95,0x49,0xdb,0x14,0x04,0xc4,0x14,0x85,0x29,
0xaa,0xe7,0x01,0x08,0xa4,0x49,0x01,0x14,0x51,0xe0,0x53,0x91,0xd5,0x29,0x06,0x1a,
0x64,0x02,0xf4,0xc7,0x81,0x9e,0x05,0x20,0x22,0x64,0xa5,0x30,0xae,0xab,0x9e,0x97,
0x53,0xd8,0xb9,0xfd,0x50,0xef,0x93,0x02,0x42,0x74,0x34,0xe8,0x9c,0x20,0x21,0xc9,
0x01,0x68,0x78,0xe6,0x55,0x29,0x20,0x56,0x4f,0x4c,0x40,0x51,0x71,0x82,0xc0,0x70,
0x21,0x22,0x85,0xbe,0x4b,0x1c,0x44,0x05,0xea,0xa4,0x01,0xbf,0x22,0xb5,0xf0,0x1c,
0x06,0x51,0x38,0x8f,0xe0,0x22,0xec,0x18,0xac,0x39,0x22,0xd4,0xd6,0x93,0x44,0x01,
0x32,0x82,0xc8,0xfc,0x61,0xb3,0x01,0x45,0x0c,0x2e,0x83,0x30,0xd0,0x0e,0x17,0x24,
0x0f,0x70,0x85,0x94,0xee,0x05,0x05,0x53,0x4b,0x32,0x1b,0x3f,0x98,0xd3,0x1d,0x29,
0x81,0xb0,0xae,0x1e,0x8c,0x7e,0x68,0xe0,0x60,0x5a,0x54,0x8f,0xb0,0x78,0x69,0x73,
0x06,0xa2,0x00,0x6b,0x57,0xca,0x3d,0x11,0x50,0xbd,0x04,0x30,0x4b,0x3a,0xd4,0xab,
0x5f,0x1f,0x9b,0x3d,0x13,0x74,0x27,0x88,0x3c,0x25,0xe0,0x17,0xbe,0x7a,0x79,0x45,
0x0d,0x0c,0xb0,0x8b,0xda,0x90,0xca,0x80,0x06,0x5d,0x17,0x60,0x1c,0x22,0x4c,0xd8,
0x57,0x22,0x06,0x20,0x00,0x98,0x07,0x08,0xe4,0x56,0x80,0x80,0x1c,0xc5,0xb7,0xc5,
0x82,0x0c,0x36,0xe8,0xe0,0x83,0x10,0x46,0x28,0xe1,0x84,0x14,0x56,0x68,0xa1,0x10,
0x41,0x00,0x00,0x3b,};
#if FSDATA_FILE_ALIGNMENT==1
static const unsigned int dummy_align__404_html = 1;
#endif
static const unsigned char FSDATA_ALIGN_PRE data__404_html[] FSDATA_ALIGN_POST = {
/* /404.html (10 chars) */
0x2f,0x34,0x30,0x34,0x2e,0x68,0x74,0x6d,0x6c,0x00,0x00,0x00,
/* HTTP header */
/* "HTTP/1.0 404 File not found
" (29 bytes) */
0x48,0x54,0x54,0x50,0x2f,0x31,0x2e,0x30,0x20,0x34,0x30,0x34,0x20,0x46,0x69,0x6c,
0x65,0x20,0x6e,0x6f,0x74,0x20,0x66,0x6f,0x75,0x6e,0x64,0x0d,0x0a,
/* "Server: lwIP/2.0.3d (http://savannah.nongnu.org/projects/lwip)
" (64 bytes) */
0x53,0x65,0x72,0x76,0x65,0x72,0x3a,0x20,0x6c,0x77,0x49,0x50,0x2f,0x32,0x2e,0x30,
0x2e,0x33,0x64,0x20,0x28,0x68,0x74,0x74,0x70,0x3a,0x2f,0x2f,0x73,0x61,0x76,0x61,
0x6e,0x6e,0x61,0x68,0x2e,0x6e,0x6f,0x6e,0x67,0x6e,0x75,0x2e,0x6f,0x72,0x67,0x2f,
0x70,0x72,0x6f,0x6a,0x65,0x63,0x74,0x73,0x2f,0x6c,0x77,0x69,0x70,0x29,0x0d,0x0a,
/* "Content-Length: 565
" (18+ bytes) */
0x43,0x6f,0x6e,0x74,0x65,0x6e,0x74,0x2d,0x4c,0x65,0x6e,0x67,0x74,0x68,0x3a,0x20,
0x35,0x36,0x35,0x0d,0x0a,
/* "Content-Type: text/html
" (27 bytes) */
0x43,0x6f,0x6e,0x74,0x65,0x6e,0x74,0x2d,0x54,0x79,0x70,0x65,0x3a,0x20,0x74,0x65,
0x78,0x74,0x2f,0x68,0x74,0x6d,0x6c,0x0d,0x0a,0x0d,0x0a,
/* raw file data (565 bytes) */
0x3c,0x68,0x74,0x6d,0x6c,0x3e,0x0d,0x0a,0x3c,0x68,0x65,0x61,0x64,0x3e,0x3c,0x74,
0x69,0x74,0x6c,0x65,0x3e,0x6c,0x77,0x49,0x50,0x20,0x2d,0x20,0x41,0x20,0x4c,0x69,
0x67,0x68,0x74,0x77,0x65,0x69,0x67,0x68,0x74,0x20,0x54,0x43,0x50,0x2f,0x49,0x50,
0x20,0x53,0x74,0x61,0x63,0x6b,0x3c,0x2f,0x74,0x69,0x74,0x6c,0x65,0x3e,0x3c,0x2f,
0x68,0x65,0x61,0x64,0x3e,0x0d,0x0a,0x3c,0x62,0x6f,0x64,0x79,0x20,0x62,0x67,0x63,
0x6f,0x6c,0x6f,0x72,0x3d,0x22,0x77,0x68,0x69,0x74,0x65,0x22,0x20,0x74,0x65,0x78,
0x74,0x3d,0x22,0x62,0x6c,0x61,0x63,0x6b,0x22,0x3e,0x0d,0x0a,0x0d,0x0a,0x20,0x20,
0x20,0x20,0x3c,0x74,0x61,0x62,0x6c,0x65,0x20,0x77,0x69,0x64,0x74,0x68,0x3d,0x22,
0x31,0x30,0x30,0x25,0x22,0x3e,0x0d,0x0a,0x20,0x20,0x20,0x20,0x20,0x20,0x3c,0x74,
0x72,0x20,0x76,0x61,0x6c,0x69,0x67,0x6e,0x3d,0x22,0x74,0x6f,0x70,0x22,0x3e,0x3c,
0x74,0x64,0x20,0x77,0x69,0x64,0x74,0x68,0x3d,0x22,0x38,0x30,0x22,0x3e,0x09,0x20,
0x20,0x0d,0x0a,0x09,0x20,0x20,0x3c,0x61,0x20,0x68,0x72,0x65,0x66,0x3d,0x22,0x68,
0x74,0x74,0x70,0x3a,0x2f,0x2f,0x77,0x77,0x77,0x2e,0x73,0x69,0x63,0x73,0x2e,0x73,
0x65,0x2f,0x22,0x3e,0x3c,0x69,0x6d,0x67,0x20,0x73,0x72,0x63,0x3d,0x22,0x2f,0x69,
0x6d,0x67,0x2f,0x73,0x69,0x63,0x73,0x2e,0x67,0x69,0x66,0x22,0x0d,0x0a,0x09,0x20,
0x20,0x62,0x6f,0x72,0x64,0x65,0x72,0x3d,0x22,0x30,0x22,0x20,0x61,0x6c,0x74,0x3d,
0x22,0x53,0x49,0x43,0x53,0x20,0x6c,0x6f,0x67,0x6f,0x22,0x20,0x74,0x69,0x74,0x6c,
0x65,0x3d,0x22,0x53,0x49,0x43,0x53,0x20,0x6c,0x6f,0x67,0x6f,0x22,0x3e,0x3c,0x2f,
0x61,0x3e,0x0d,0x0a,0x09,0x3c,0x2f,0x74,0x64,0x3e,0x3c,0x74,0x64,0x20,0x77,0x69,
0x64,0x74,0x68,0x3d,0x22,0x35,0x30,0x30,0x22,0x3e,0x09,0x20,0x20,0x0d,0x0a,0x09,
0x20,0x20,0x3c,0x68,0x31,0x3e,0x6c,0x77,0x49,0x50,0x20,0x2d,0x20,0x41,0x20,0x4c,
0x69,0x67,0x68,0x74,0x77,0x65,0x69,0x67,0x68,0x74,0x20,0x54,0x43,0x50,0x2f,0x49,
0x50,0x20,0x53,0x74,0x61,0x63,0x6b,0x3c,0x2f,0x68,0x31,0x3e,0x0d,0x0a,0x09,0x20,
0x20,0x3c,0x68,0x32,0x3e,0x34,0x30,0x34,0x20,0x2d,0x20,0x50,0x61,0x67,0x65,0x20,
0x6e,0x6f,0x74,0x20,0x66,0x6f,0x75,0x6e,0x64,0x3c,0x2f,0x68,0x32,0x3e,0x0d,0x0a,
0x09,0x20,0x20,0x3c,0x70,0x3e,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x53,0x6f,0x72,
0x72,0x79,0x2c,0x20,0x74,0x68,0x65,0x20,0x70,0x61,0x67,0x65,0x20,0x79,0x6f,0x75,
0x20,0x61,0x72,0x65,0x20,0x72,0x65,0x71,0x75,0x65,0x73,0x74,0x69,0x6e,0x67,0x20,
0x77,0x61,0x73,0x20,0x6e,0x6f,0x74,0x20,0x66,0x6f,0x75,0x6e,0x64,0x20,0x6f,0x6e,
0x20,0x74,0x68,0x69,0x73,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x73,0x65,0x72,0x76,
0x65,0x72,0x2e,0x20,0x0d,0x0a,0x09,0x20,0x20,0x3c,0x2f,0x70,0x3e,0x0d,0x0a,0x09,
0x3c,0x2f,0x74,0x64,0x3e,0x3c,0x74,0x64,0x3e,0x0d,0x0a,0x09,0x20,0x20,0x26,0x6e,
0x62,0x73,0x70,0x3b,0x0d,0x0a,0x09,0x3c,0x2f,0x74,0x64,0x3e,0x3c,0x2f,0x74,0x72,
0x3e,0x0d,0x0a,0x20,0x20,0x20,0x20,0x20,0x20,0x3c,0x2f,0x74,0x61,0x62,0x6c,0x65,
0x3e,0x0d,0x0a,0x3c,0x2f,0x62,0x6f,0x64,0x79,0x3e,0x0d,0x0a,0x3c,0x2f,0x68,0x74,
0x6d,0x6c,0x3e,0x0d,0x0a,};
#if FSDATA_FILE_ALIGNMENT==1
static const unsigned int dummy_align__index_html = 2;
#endif
static const unsigned char FSDATA_ALIGN_PRE data__index_html[] FSDATA_ALIGN_POST = {
/* /index.html (12 chars) */
0x2f,0x69,0x6e,0x64,0x65,0x78,0x2e,0x68,0x74,0x6d,0x6c,0x00,
/* HTTP header */
/* "HTTP/1.0 200 OK
" (17 bytes) */
0x48,0x54,0x54,0x50,0x2f,0x31,0x2e,0x30,0x20,0x32,0x30,0x30,0x20,0x4f,0x4b,0x0d,
0x0a,
/* "Server: lwIP/2.0.3d (http://savannah.nongnu.org/projects/lwip)
" (64 bytes) */
0x53,0x65,0x72,0x76,0x65,0x72,0x3a,0x20,0x6c,0x77,0x49,0x50,0x2f,0x32,0x2e,0x30,
0x2e,0x33,0x64,0x20,0x28,0x68,0x74,0x74,0x70,0x3a,0x2f,0x2f,0x73,0x61,0x76,0x61,
0x6e,0x6e,0x61,0x68,0x2e,0x6e,0x6f,0x6e,0x67,0x6e,0x75,0x2e,0x6f,0x72,0x67,0x2f,
0x70,0x72,0x6f,0x6a,0x65,0x63,0x74,0x73,0x2f,0x6c,0x77,0x69,0x70,0x29,0x0d,0x0a,
/* "Content-Length: 1751
" (18+ bytes) */
0x43,0x6f,0x6e,0x74,0x65,0x6e,0x74,0x2d,0x4c,0x65,0x6e,0x67,0x74,0x68,0x3a,0x20,
0x31,0x37,0x35,0x31,0x0d,0x0a,
/* "Content-Type: text/html
" (27 bytes) */
0x43,0x6f,0x6e,0x74,0x65,0x6e,0x74,0x2d,0x54,0x79,0x70,0x65,0x3a,0x20,0x74,0x65,
0x78,0x74,0x2f,0x68,0x74,0x6d,0x6c,0x0d,0x0a,0x0d,0x0a,
/* raw file data (1751 bytes) */
0x3c,0x68,0x74,0x6d,0x6c,0x3e,0x0d,0x0a,0x3c,0x68,0x65,0x61,0x64,0x3e,0x3c,0x74,
0x69,0x74,0x6c,0x65,0x3e,0x6c,0x77,0x49,0x50,0x20,0x2d,0x20,0x41,0x20,0x4c,0x69,
0x67,0x68,0x74,0x77,0x65,0x69,0x67,0x68,0x74,0x20,0x54,0x43,0x50,0x2f,0x49,0x50,
0x20,0x53,0x74,0x61,0x63,0x6b,0x3c,0x2f,0x74,0x69,0x74,0x6c,0x65,0x3e,0x3c,0x2f,
0x68,0x65,0x61,0x64,0x3e,0x0d,0x0a,0x3c,0x62,0x6f,0x64,0x79,0x20,0x62,0x67,0x63,
0x6f,0x6c,0x6f,0x72,0x3d,0x22,0x77,0x68,0x69,0x74,0x65,0x22,0x20,0x74,0x65,0x78,
0x74,0x3d,0x22,0x62,0x6c,0x61,0x63,0x6b,0x22,0x3e,0x0d,0x0a,0x0d,0x0a,0x20,0x20,
0x20,0x20,0x3c,0x74,0x61,0x62,0x6c,0x65,0x20,0x77,0x69,0x64,0x74,0x68,0x3d,0x22,
0x31,0x30,0x30,0x25,0x22,0x3e,0x0d,0x0a,0x20,0x20,0x20,0x20,0x20,0x20,0x3c,0x74,
0x72,0x20,0x76,0x61,0x6c,0x69,0x67,0x6e,0x3d,0x22,0x74,0x6f,0x70,0x22,0x3e,0x3c,
0x74,0x64,0x20,0x77,0x69,0x64,0x74,0x68,0x3d,0x22,0x38,0x30,0x22,0x3e,0x09,0x20,
0x20,0x0d,0x0a,0x09,0x20,0x20,0x3c,0x61,0x20,0x68,0x72,0x65,0x66,0x3d,0x22,0x68,
0x74,0x74,0x70,0x3a,0x2f,0x2f,0x77,0x77,0x77,0x2e,0x73,0x69,0x63,0x73,0x2e,0x73,
0x65,0x2f,0x22,0x3e,0x3c,0x69,0x6d,0x67,0x20,0x73,0x72,0x63,0x3d,0x22,0x2f,0x69,
0x6d,0x67,0x2f,0x73,0x69,0x63,0x73,0x2e,0x67,0x69,0x66,0x22,0x0d,0x0a,0x09,0x20,
0x20,0x62,0x6f,0x72,0x64,0x65,0x72,0x3d,0x22,0x30,0x22,0x20,0x61,0x6c,0x74,0x3d,
0x22,0x53,0x49,0x43,0x53,0x20,0x6c,0x6f,0x67,0x6f,0x22,0x20,0x74,0x69,0x74,0x6c,
0x65,0x3d,0x22,0x53,0x49,0x43,0x53,0x20,0x6c,0x6f,0x67,0x6f,0x22,0x3e,0x3c,0x2f,
0x61,0x3e,0x0d,0x0a,0x09,0x3c,0x2f,0x74,0x64,0x3e,0x3c,0x74,0x64,0x20,0x77,0x69,
0x64,0x74,0x68,0x3d,0x22,0x35,0x30,0x30,0x22,0x3e,0x09,0x20,0x20,0x0d,0x0a,0x09,
0x20,0x20,0x3c,0x68,0x31,0x3e,0x6c,0x77,0x49,0x50,0x20,0x2d,0x20,0x41,0x20,0x4c,
0x69,0x67,0x68,0x74,0x77,0x65,0x69,0x67,0x68,0x74,0x20,0x54,0x43,0x50,0x2f,0x49,
0x50,0x20,0x53,0x74,0x61,0x63,0x6b,0x3c,0x2f,0x68,0x31,0x3e,0x0d,0x0a,0x09,0x20,
0x20,0x3c,0x70,0x3e,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x54,0x68,0x65,0x20,0x77,
0x65,0x62,0x20,0x70,0x61,0x67,0x65,0x20,0x79,0x6f,0x75,0x20,0x61,0x72,0x65,0x20,
0x77,0x61,0x74,0x63,0x68,0x69,0x6e,0x67,0x20,0x77,0x61,0x73,0x20,0x73,0x65,0x72,
0x76,0x65,0x64,0x20,0x62,0x79,0x20,0x61,0x20,0x73,0x69,0x6d,0x70,0x6c,0x65,0x20,
0x77,0x65,0x62,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x73,0x65,0x72,0x76,0x65,0x72,
0x20,0x72,0x75,0x6e,0x6e,0x69,0x6e,0x67,0x20,0x6f,0x6e,0x20,0x74,0x6f,0x70,0x20,
0x6f,0x66,0x20,0x74,0x68,0x65,0x20,0x6c,0x69,0x67,0x68,0x74,0x77,0x65,0x69,0x67,
0x68,0x74,0x20,0x54,0x43,0x50,0x2f,0x49,0x50,0x20,0x73,0x74,0x61,0x63,0x6b,0x20,
0x3c,0x61,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x68,0x72,0x65,0x66,0x3d,0x22,0x68,
0x74,0x74,0x70,0x3a,0x2f,0x2f,0x77,0x77,0x77,0x2e,0x73,0x69,0x63,0x73,0x2e,0x73,
0x65,0x2f,0x7e,0x61,0x64,0x61,0x6d,0x2f,0x6c,0x77,0x69,0x70,0x2f,0x22,0x3e,0x6c,
0x77,0x49,0x50,0x3c,0x2f,0x61,0x3e,0x2e,0x0d,0x0a,0x09,0x20,0x20,0x3c,0x2f,0x70,
0x3e,0x0d,0x0a,0x09,0x20,0x20,0x3c,0x70,0x3e,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,
0x6c,0x77,0x49,0x50,0x20,0x69,0x73,0x20,0x61,0x6e,0x20,0x6f,0x70,0x65,0x6e,0x20,
0x73,0x6f,0x75,0x72,0x63,0x65,0x20,0x69,0x6d,0x70,0x6c,0x65,0x6d,0x65,0x6e,0x74,
0x61,0x74,0x69,0x6f,0x6e,0x20,0x6f,0x66,0x20,0x74,0x68,0x65,0x20,0x54,0x43,0x50,
0x2f,0x49,0x50,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x70,0x72,0x6f,0x74,0x6f,0x63,
0x6f,0x6c,0x20,0x73,0x75,0x69,0x74,0x65,0x20,0x74,0x68,0x61,0x74,0x20,0x77,0x61,
0x73,0x20,0x6f,0x72,0x69,0x67,0x69,0x6e,0x61,0x6c,0x6c,0x79,0x20,0x77,0x72,0x69,
0x74,0x74,0x65,0x6e,0x20,0x62,0x79,0x20,0x3c,0x61,0x0d,0x0a,0x09,0x20,0x20,0x20,
0x20,0x68,0x72,0x65,0x66,0x3d,0x22,0x68,0x74,0x74,0x70,0x3a,0x2f,0x2f,0x77,0x77,
0x77,0x2e,0x73,0x69,0x63,0x73,0x2e,0x73,0x65,0x2f,0x7e,0x61,0x64,0x61,0x6d,0x2f,
0x6c,0x77,0x69,0x70,0x2f,0x22,0x3e,0x41,0x64,0x61,0x6d,0x20,0x44,0x75,0x6e,0x6b,
0x65,0x6c,0x73,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x6f,0x66,0x20,0x74,0x68,0x65,
0x20,0x53,0x77,0x65,0x64,0x69,0x73,0x68,0x20,0x49,0x6e,0x73,0x74,0x69,0x74,0x75,
0x74,0x65,0x20,0x6f,0x66,0x20,0x43,0x6f,0x6d,0x70,0x75,0x74,0x65,0x72,0x20,0x53,
0x63,0x69,0x65,0x6e,0x63,0x65,0x3c,0x2f,0x61,0x3e,0x20,0x62,0x75,0x74,0x20,0x6e,
0x6f,0x77,0x20,0x69,0x73,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x62,0x65,0x69,0x6e,
0x67,0x20,0x61,0x63,0x74,0x69,0x76,0x65,0x6c,0x79,0x20,0x64,0x65,0x76,0x65,0x6c,
0x6f,0x70,0x65,0x64,0x20,0x62,0x79,0x20,0x61,0x20,0x74,0x65,0x61,0x6d,0x20,0x6f,
0x66,0x20,0x64,0x65,0x76,0x65,0x6c,0x6f,0x70,0x65,0x72,0x73,0x0d,0x0a,0x09,0x20,
0x20,0x20,0x20,0x64,0x69,0x73,0x74,0x72,0x69,0x62,0x75,0x74,0x65,0x64,0x20,0x77,
0x6f,0x72,0x6c,0x64,0x2d,0x77,0x69,0x64,0x65,0x2e,0x20,0x53,0x69,0x6e,0x63,0x65,
0x20,0x69,0x74,0x27,0x73,0x20,0x72,0x65,0x6c,0x65,0x61,0x73,0x65,0x2c,0x20,0x6c,
0x77,0x49,0x50,0x20,0x68,0x61,0x73,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x73,0x70,
0x75,0x72,0x72,0x65,0x64,0x20,0x61,0x20,0x6c,0x6f,0x74,0x20,0x6f,0x66,0x20,0x69,
0x6e,0x74,0x65,0x72,0x65,0x73,0x74,0x20,0x61,0x6e,0x64,0x20,0x68,0x61,0x73,0x20,
0x62,0x65,0x65,0x6e,0x20,0x70,0x6f,0x72,0x74,0x65,0x64,0x20,0x74,0x6f,0x20,0x73,
0x65,0x76,0x65,0x72,0x61,0x6c,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x70,0x6c,0x61,
0x74,0x66,0x6f,0x72,0x6d,0x73,0x20,0x61,0x6e,0x64,0x20,0x6f,0x70,0x65,0x72,0x61,
0x74,0x69,0x6e,0x67,0x20,0x73,0x79,0x73,0x74,0x65,0x6d,0x73,0x2e,0x20,0x6c,0x77,
0x49,0x50,0x20,0x63,0x61,0x6e,0x20,0x62,0x65,0x20,0x75,0x73,0x65,0x64,0x20,0x65,
0x69,0x74,0x68,0x65,0x72,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x77,0x69,0x74,0x68,
0x20,0x6f,0x72,0x20,0x77,0x69,0x74,0x68,0x6f,0x75,0x74,0x20,0x61,0x6e,0x20,0x75,
0x6e,0x64,0x65,0x72,0x6c,0x79,0x69,0x6e,0x67,0x20,0x4f,0x53,0x2e,0x0d,0x0a,0x09,
0x20,0x20,0x3c,0x2f,0x70,0x3e,0x0d,0x0a,0x09,0x20,0x20,0x3c,0x70,0x3e,0x0d,0x0a,
0x09,0x20,0x20,0x20,0x20,0x54,0x68,0x65,0x20,0x66,0x6f,0x63,0x75,0x73,0x20,0x6f,
0x66,0x20,0x74,0x68,0x65,0x20,0x6c,0x77,0x49,0x50,0x20,0x54,0x43,0x50,0x2f,0x49,
0x50,0x20,0x69,0x6d,0x70,0x6c,0x65,0x6d,0x65,0x6e,0x74,0x61,0x74,0x69,0x6f,0x6e,
0x20,0x69,0x73,0x20,0x74,0x6f,0x20,0x72,0x65,0x64,0x75,0x63,0x65,0x0d,0x0a,0x09,
0x20,0x20,0x20,0x20,0x74,0x68,0x65,0x20,0x52,0x41,0x4d,0x20,0x75,0x73,0x61,0x67,
0x65,0x20,0x77,0x68,0x69,0x6c,0x65,0x20,0x73,0x74,0x69,0x6c,0x6c,0x20,0x68,0x61,
0x76,0x69,0x6e,0x67,0x20,0x61,0x20,0x66,0x75,0x6c,0x6c,0x20,0x73,0x63,0x61,0x6c,
0x65,0x20,0x54,0x43,0x50,0x2e,0x20,0x54,0x68,0x69,0x73,0x0d,0x0a,0x09,0x20,0x20,
0x20,0x20,0x6d,0x61,0x6b,0x65,0x73,0x20,0x6c,0x77,0x49,0x50,0x20,0x73,0x75,0x69,
0x74,0x61,0x62,0x6c,0x65,0x20,0x66,0x6f,0x72,0x20,0x75,0x73,0x65,0x20,0x69,0x6e,
0x20,0x65,0x6d,0x62,0x65,0x64,0x64,0x65,0x64,0x20,0x73,0x79,0x73,0x74,0x65,0x6d,
0x73,0x20,0x77,0x69,0x74,0x68,0x20,0x74,0x65,0x6e,0x73,0x0d,0x0a,0x09,0x20,0x20,
0x20,0x20,0x6f,0x66,0x20,0x6b,0x69,0x6c,0x6f,0x62,0x79,0x74,0x65,0x73,0x20,0x6f,
0x66,0x20,0x66,0x72,0x65,0x65,0x20,0x52,0x41,0x4d,0x20,0x61,0x6e,0x64,0x20,0x72,
0x6f,0x6f,0x6d,0x20,0x66,0x6f,0x72,0x20,0x61,0x72,0x6f,0x75,0x6e,0x64,0x20,0x34,
0x30,0x20,0x6b,0x69,0x6c,0x6f,0x62,0x79,0x74,0x65,0x73,0x0d,0x0a,0x09,0x20,0x20,
0x20,0x20,0x6f,0x66,0x20,0x63,0x6f,0x64,0x65,0x20,0x52,0x4f,0x4d,0x2e,0x0d,0x0a,
0x09,0x20,0x20,0x3c,0x2f,0x70,0x3e,0x0d,0x0a,0x09,0x20,0x20,0x3c,0x70,0x3e,0x0d,
0x0a,0x09,0x20,0x20,0x20,0x20,0x4d,0x6f,0x72,0x65,0x20,0x69,0x6e,0x66,0x6f,0x72,
0x6d,0x61,0x74,0x69,0x6f,0x6e,0x20,0x61,0x62,0x6f,0x75,0x74,0x20,0x6c,0x77,0x49,
0x50,0x20,0x63,0x61,0x6e,0x20,0x62,0x65,0x20,0x66,0x6f,0x75,0x6e,0x64,0x20,0x61,
0x74,0x20,0x74,0x68,0x65,0x20,0x6c,0x77,0x49,0x50,0x0d,0x0a,0x09,0x20,0x20,0x20,
0x20,0x68,0x6f,0x6d,0x65,0x70,0x61,0x67,0x65,0x20,0x61,0x74,0x20,0x3c,0x61,0x0d,
0x0a,0x09,0x20,0x20,0x20,0x20,0x68,0x72,0x65,0x66,0x3d,0x22,0x68,0x74,0x74,0x70,
0x3a,0x2f,0x2f,0x73,0x61,0x76,0x61,0x6e,0x6e,0x61,0x68,0x2e,0x6e,0x6f,0x6e,0x67,
0x6e,0x75,0x2e,0x6f,0x72,0x67,0x2f,0x70,0x72,0x6f,0x6a,0x65,0x63,0x74,0x73,0x2f,
0x6c,0x77,0x69,0x70,0x2f,0x22,0x3e,0x68,0x74,0x74,0x70,0x3a,0x2f,0x2f,0x73,0x61,
0x76,0x61,0x6e,0x6e,0x61,0x68,0x2e,0x6e,0x6f,0x6e,0x67,0x6e,0x75,0x2e,0x6f,0x72,
0x67,0x2f,0x70,0x72,0x6f,0x6a,0x65,0x63,0x74,0x73,0x2f,0x6c,0x77,0x69,0x70,0x2f,
0x3c,0x2f,0x61,0x3e,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x6f,0x72,0x20,0x61,0x74,
0x20,0x74,0x68,0x65,0x20,0x6c,0x77,0x49,0x50,0x20,0x77,0x69,0x6b,0x69,0x20,0x61,
0x74,0x20,0x3c,0x61,0x0d,0x0a,0x09,0x20,0x20,0x20,0x20,0x68,0x72,0x65,0x66,0x3d,
0x22,0x68,0x74,0x74,0x70,0x3a,0x2f,0x2f,0x6c,0x77,0x69,0x70,0x2e,0x77,0x69,0x6b,
0x69,0x61,0x2e,0x63,0x6f,0x6d,0x2f,0x22,0x3e,0x68,0x74,0x74,0x70,0x3a,0x2f,0x2f,
0x6c,0x77,0x69,0x70,0x2e,0x77,0x69,0x6b,0x69,0x61,0x2e,0x63,0x6f,0x6d,0x2f,0x3c,
0x2f,0x61,0x3e,0x2e,0x0d,0x0a,0x09,0x20,0x20,0x3c,0x2f,0x70,0x3e,0x0d,0x0a,0x09,
0x3c,0x2f,0x74,0x64,0x3e,0x3c,0x74,0x64,0x3e,0x0d,0x0a,0x09,0x20,0x20,0x26,0x6e,
0x62,0x73,0x70,0x3b,0x0d,0x0a,0x09,0x3c,0x2f,0x74,0x64,0x3e,0x3c,0x2f,0x74,0x72,
0x3e,0x0d,0x0a,0x20,0x20,0x20,0x20,0x20,0x20,0x3c,0x2f,0x74,0x61,0x62,0x6c,0x65,
0x3e,0x0d,0x0a,0x3c,0x2f,0x62,0x6f,0x64,0x79,0x3e,0x0d,0x0a,0x3c,0x2f,0x68,0x74,
0x6d,0x6c,0x3e,0x0d,0x0a,0x0d,0x0a,};
const struct fsdata_file file__img_sics_gif[] = { {
file_NULL,
data__img_sics_gif,
data__img_sics_gif + 16,
sizeof(data__img_sics_gif) - 16,
FS_FILE_FLAGS_HEADER_INCLUDED | FS_FILE_FLAGS_HEADER_PERSISTENT,
}};
const struct fsdata_file file__404_html[] = { {
file__img_sics_gif,
data__404_html,
data__404_html + 12,
sizeof(data__404_html) - 12,
FS_FILE_FLAGS_HEADER_INCLUDED | FS_FILE_FLAGS_HEADER_PERSISTENT,
}};
const struct fsdata_file file__index_html[] = { {
file__404_html,
data__index_html,
data__index_html + 12,
sizeof(data__index_html) - 12,
FS_FILE_FLAGS_HEADER_INCLUDED | FS_FILE_FLAGS_HEADER_PERSISTENT,
}};
#define FS_ROOT file__index_html
#define FS_NUMFILES 3

View File

@ -0,0 +1,41 @@
/*
* Copyright (c) 2001-2003 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#ifndef LWIP_FSDATA_H
#define LWIP_FSDATA_H
#include "lwip/apps/httpd_opts.h"
#include "lwip/apps/fs.h"
/* THIS FILE IS DEPRECATED AND WILL BE REMOVED IN THE FUTURE */
/* content was moved to fs.h to simplify #include structure */
#endif /* LWIP_FSDATA_H */

View File

@ -0,0 +1,909 @@
/**
* @file
* HTTP client
*/
/*
* Copyright (c) 2018 Simon Goldschmidt <goldsimon@gmx.de>
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt <goldsimon@gmx.de>
*/
/**
* @defgroup httpc HTTP client
* @ingroup apps
* @todo:
* - persistent connections
* - select outgoing http version
* - optionally follow redirect
* - check request uri for invalid characters? (e.g. encode spaces)
* - IPv6 support
*/
#include "lwip/apps/http_client.h"
#include "lwip/altcp_tcp.h"
#include "lwip/dns.h"
#include "lwip/debug.h"
#include "lwip/mem.h"
#include "lwip/altcp_tls.h"
#include "lwip/init.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#if LWIP_TCP && LWIP_CALLBACK_API
/**
* HTTPC_DEBUG: Enable debugging for HTTP client.
*/
#ifndef HTTPC_DEBUG
#define HTTPC_DEBUG LWIP_DBG_OFF
#endif
/** Set this to 1 to keep server name and uri in request state */
#ifndef HTTPC_DEBUG_REQUEST
#define HTTPC_DEBUG_REQUEST 0
#endif
/** This string is passed in the HTTP header as "User-Agent: " */
#ifndef HTTPC_CLIENT_AGENT
#define HTTPC_CLIENT_AGENT "lwIP/" LWIP_VERSION_STRING " (http://savannah.nongnu.org/projects/lwip)"
#endif
/* the various debug levels for this file */
#define HTTPC_DEBUG_TRACE (HTTPC_DEBUG | LWIP_DBG_TRACE)
#define HTTPC_DEBUG_STATE (HTTPC_DEBUG | LWIP_DBG_STATE)
#define HTTPC_DEBUG_WARN (HTTPC_DEBUG | LWIP_DBG_LEVEL_WARNING)
#define HTTPC_DEBUG_WARN_STATE (HTTPC_DEBUG | LWIP_DBG_LEVEL_WARNING | LWIP_DBG_STATE)
#define HTTPC_DEBUG_SERIOUS (HTTPC_DEBUG | LWIP_DBG_LEVEL_SERIOUS)
#define HTTPC_POLL_INTERVAL 1
#define HTTPC_POLL_TIMEOUT 30 /* 15 seconds */
#define HTTPC_CONTENT_LEN_INVALID 0xFFFFFFFF
/* GET request basic */
#define HTTPC_REQ_11 "GET %s HTTP/1.1\r\n" /* URI */\
"User-Agent: %s\r\n" /* User-Agent */ \
"Accept: */*\r\n" \
"Connection: Close\r\n" /* we don't support persistent connections, yet */ \
"\r\n"
#define HTTPC_REQ_11_FORMAT(uri) HTTPC_REQ_11, uri, HTTPC_CLIENT_AGENT
/* GET request with host */
#define HTTPC_REQ_11_HOST "GET %s HTTP/1.1\r\n" /* URI */\
"User-Agent: %s\r\n" /* User-Agent */ \
"Accept: */*\r\n" \
"Host: %s\r\n" /* server name */ \
"Connection: Close\r\n" /* we don't support persistent connections, yet */ \
"\r\n"
#define HTTPC_REQ_11_HOST_FORMAT(uri, srv_name) HTTPC_REQ_11_HOST, uri, HTTPC_CLIENT_AGENT, srv_name
/* GET request with proxy */
#define HTTPC_REQ_11_PROXY "GET http://%s%s HTTP/1.1\r\n" /* HOST, URI */\
"User-Agent: %s\r\n" /* User-Agent */ \
"Accept: */*\r\n" \
"Host: %s\r\n" /* server name */ \
"Connection: Close\r\n" /* we don't support persistent connections, yet */ \
"\r\n"
#define HTTPC_REQ_11_PROXY_FORMAT(host, uri, srv_name) HTTPC_REQ_11_PROXY, host, uri, HTTPC_CLIENT_AGENT, srv_name
/* GET request with proxy (non-default server port) */
#define HTTPC_REQ_11_PROXY_PORT "GET http://%s:%d%s HTTP/1.1\r\n" /* HOST, host-port, URI */\
"User-Agent: %s\r\n" /* User-Agent */ \
"Accept: */*\r\n" \
"Host: %s\r\n" /* server name */ \
"Connection: Close\r\n" /* we don't support persistent connections, yet */ \
"\r\n"
#define HTTPC_REQ_11_PROXY_PORT_FORMAT(host, host_port, uri, srv_name) HTTPC_REQ_11_PROXY_PORT, host, host_port, uri, HTTPC_CLIENT_AGENT, srv_name
typedef enum ehttpc_parse_state {
HTTPC_PARSE_WAIT_FIRST_LINE = 0,
HTTPC_PARSE_WAIT_HEADERS,
HTTPC_PARSE_RX_DATA
} httpc_parse_state_t;
typedef struct _httpc_state
{
struct altcp_pcb* pcb;
ip_addr_t remote_addr;
u16_t remote_port;
int timeout_ticks;
struct pbuf *request;
struct pbuf *rx_hdrs;
u16_t rx_http_version;
u16_t rx_status;
altcp_recv_fn recv_fn;
const httpc_connection_t *conn_settings;
void* callback_arg;
u32_t rx_content_len;
u32_t hdr_content_len;
httpc_parse_state_t parse_state;
#if HTTPC_DEBUG_REQUEST
char* server_name;
char* uri;
#endif
} httpc_state_t;
/** Free http client state and deallocate all resources within */
static err_t
httpc_free_state(httpc_state_t* req)
{
struct altcp_pcb* tpcb;
if (req->request != NULL) {
pbuf_free(req->request);
req->request = NULL;
}
if (req->rx_hdrs != NULL) {
pbuf_free(req->rx_hdrs);
req->rx_hdrs = NULL;
}
tpcb = req->pcb;
mem_free(req);
req = NULL;
if (tpcb != NULL) {
err_t r;
altcp_arg(tpcb, NULL);
altcp_recv(tpcb, NULL);
altcp_err(tpcb, NULL);
altcp_poll(tpcb, NULL, 0);
altcp_sent(tpcb, NULL);
r = altcp_close(tpcb);
if (r != ERR_OK) {
altcp_abort(tpcb);
return ERR_ABRT;
}
}
return ERR_OK;
}
/** Close the connection: call finished callback and free the state */
static err_t
httpc_close(httpc_state_t* req, httpc_result_t result, u32_t server_response, err_t err)
{
if (req != NULL) {
if (req->conn_settings != NULL) {
if (req->conn_settings->result_fn != NULL) {
req->conn_settings->result_fn(req->callback_arg, result, req->rx_content_len, server_response, err);
}
}
return httpc_free_state(req);
}
return ERR_OK;
}
/** Parse http header response line 1 */
static err_t
http_parse_response_status(struct pbuf *p, u16_t *http_version, u16_t *http_status, u16_t *http_status_str_offset)
{
u16_t end1 = pbuf_memfind(p, "\r\n", 2, 0);
if (end1 != 0xFFFF) {
/* get parts of first line */
u16_t space1, space2;
space1 = pbuf_memfind(p, " ", 1, 0);
if (space1 != 0xFFFF) {
if ((pbuf_memcmp(p, 0, "HTTP/", 5) == 0) && (pbuf_get_at(p, 6) == '.')) {
char status_num[10];
size_t status_num_len;
/* parse http version */
u16_t version = pbuf_get_at(p, 5) - '0';
version <<= 8;
version |= pbuf_get_at(p, 7) - '0';
*http_version = version;
/* parse http status number */
space2 = pbuf_memfind(p, " ", 1, space1 + 1);
if (space2 != 0xFFFF) {
*http_status_str_offset = space2 + 1;
status_num_len = space2 - space1 - 1;
} else {
status_num_len = end1 - space1 - 1;
}
memset(status_num, 0, sizeof(status_num));
if (pbuf_copy_partial(p, status_num, (u16_t)status_num_len, space1 + 1) == status_num_len) {
int status = atoi(status_num);
if ((status > 0) && (status <= 0xFFFF)) {
*http_status = (u16_t)status;
return ERR_OK;
}
}
}
}
}
return ERR_VAL;
}
/** Wait for all headers to be received, return its length and content-length (if available) */
static err_t
http_wait_headers(struct pbuf *p, u32_t *content_length, u16_t *total_header_len)
{
u16_t end1 = pbuf_memfind(p, "\r\n\r\n", 4, 0);
if (end1 < (0xFFFF - 2)) {
/* all headers received */
/* check if we have a content length (@todo: case insensitive?) */
u16_t content_len_hdr;
*content_length = HTTPC_CONTENT_LEN_INVALID;
*total_header_len = end1 + 4;
content_len_hdr = pbuf_memfind(p, "Content-Length: ", 16, 0);
if (content_len_hdr != 0xFFFF) {
u16_t content_len_line_end = pbuf_memfind(p, "\r\n", 2, content_len_hdr);
if (content_len_line_end != 0xFFFF) {
char content_len_num[16];
u16_t content_len_num_len = (u16_t)(content_len_line_end - content_len_hdr - 16);
memset(content_len_num, 0, sizeof(content_len_num));
if (pbuf_copy_partial(p, content_len_num, content_len_num_len, content_len_hdr + 16) == content_len_num_len) {
int len = atoi(content_len_num);
if ((len >= 0) && ((u32_t)len < HTTPC_CONTENT_LEN_INVALID)) {
*content_length = (u32_t)len;
}
}
}
}
return ERR_OK;
}
return ERR_VAL;
}
/** http client tcp recv callback */
static err_t
httpc_tcp_recv(void *arg, struct altcp_pcb *pcb, struct pbuf *p, err_t r)
{
httpc_state_t* req = (httpc_state_t*)arg;
LWIP_UNUSED_ARG(r);
if (p == NULL) {
httpc_result_t result;
if (req->parse_state != HTTPC_PARSE_RX_DATA) {
/* did not get RX data yet */
result = HTTPC_RESULT_ERR_CLOSED;
} else if ((req->hdr_content_len != HTTPC_CONTENT_LEN_INVALID) &&
(req->hdr_content_len != req->rx_content_len)) {
/* header has been received with content length but not all data received */
result = HTTPC_RESULT_ERR_CONTENT_LEN;
} else {
/* receiving data and either all data received or no content length header */
result = HTTPC_RESULT_OK;
}
return httpc_close(req, result, req->rx_status, ERR_OK);
}
if (req->parse_state != HTTPC_PARSE_RX_DATA) {
if (req->rx_hdrs == NULL) {
req->rx_hdrs = p;
} else {
pbuf_cat(req->rx_hdrs, p);
}
if (req->parse_state == HTTPC_PARSE_WAIT_FIRST_LINE) {
u16_t status_str_off;
err_t err = http_parse_response_status(req->rx_hdrs, &req->rx_http_version, &req->rx_status, &status_str_off);
if (err == ERR_OK) {
/* don't care status string */
req->parse_state = HTTPC_PARSE_WAIT_HEADERS;
}
}
if (req->parse_state == HTTPC_PARSE_WAIT_HEADERS) {
u16_t total_header_len;
err_t err = http_wait_headers(req->rx_hdrs, &req->hdr_content_len, &total_header_len);
if (err == ERR_OK) {
struct pbuf *q;
/* full header received, send window update for header bytes and call into client callback */
altcp_recved(pcb, total_header_len);
if (req->conn_settings) {
if (req->conn_settings->headers_done_fn) {
err = req->conn_settings->headers_done_fn(req, req->callback_arg, req->rx_hdrs, total_header_len, req->hdr_content_len);
if (err != ERR_OK) {
return httpc_close(req, HTTPC_RESULT_LOCAL_ABORT, req->rx_status, err);
}
}
}
/* hide header bytes in pbuf */
q = pbuf_free_header(req->rx_hdrs, total_header_len);
p = q;
req->rx_hdrs = NULL;
/* go on with data */
req->parse_state = HTTPC_PARSE_RX_DATA;
}
}
}
if ((p != NULL) && (req->parse_state == HTTPC_PARSE_RX_DATA)) {
req->rx_content_len += p->tot_len;
if (req->recv_fn != NULL) {
/* directly return here: the connection migth already be aborted from the callback! */
return req->recv_fn(req->callback_arg, pcb, p, r);
} else {
altcp_recved(pcb, p->tot_len);
pbuf_free(p);
}
}
return ERR_OK;
}
/** http client tcp err callback */
static void
httpc_tcp_err(void *arg, err_t err)
{
httpc_state_t* req = (httpc_state_t*)arg;
if (req != NULL) {
/* pcb has already been deallocated */
req->pcb = NULL;
httpc_close(req, HTTPC_RESULT_ERR_CLOSED, 0, err);
}
}
/** http client tcp poll callback */
static err_t
httpc_tcp_poll(void *arg, struct altcp_pcb *pcb)
{
/* implement timeout */
httpc_state_t* req = (httpc_state_t*)arg;
LWIP_UNUSED_ARG(pcb);
if (req != NULL) {
if (req->timeout_ticks) {
req->timeout_ticks--;
}
if (!req->timeout_ticks) {
return httpc_close(req, HTTPC_RESULT_ERR_TIMEOUT, 0, ERR_OK);
}
}
return ERR_OK;
}
/** http client tcp sent callback */
static err_t
httpc_tcp_sent(void *arg, struct altcp_pcb *pcb, u16_t len)
{
/* nothing to do here for now */
LWIP_UNUSED_ARG(arg);
LWIP_UNUSED_ARG(pcb);
LWIP_UNUSED_ARG(len);
return ERR_OK;
}
/** http client tcp connected callback */
static err_t
httpc_tcp_connected(void *arg, struct altcp_pcb *pcb, err_t err)
{
err_t r;
httpc_state_t* req = (httpc_state_t*)arg;
LWIP_UNUSED_ARG(pcb);
LWIP_UNUSED_ARG(err);
/* send request; last char is zero termination */
r = altcp_write(req->pcb, req->request->payload, req->request->len - 1, TCP_WRITE_FLAG_COPY);
if (r != ERR_OK) {
/* could not write the single small request -> fail, don't retry */
return httpc_close(req, HTTPC_RESULT_ERR_MEM, 0, r);
}
/* everything written, we can free the request */
pbuf_free(req->request);
req->request = NULL;
altcp_output(req->pcb);
return ERR_OK;
}
/** Start the http request when the server IP addr is known */
static err_t
httpc_get_internal_addr(httpc_state_t* req, const ip_addr_t *ipaddr)
{
err_t err;
LWIP_ASSERT("req != NULL", req != NULL);
if (&req->remote_addr != ipaddr) {
/* fill in remote addr if called externally */
req->remote_addr = *ipaddr;
}
err = altcp_connect(req->pcb, &req->remote_addr, req->remote_port, httpc_tcp_connected);
if (err == ERR_OK) {
return ERR_OK;
}
LWIP_DEBUGF(HTTPC_DEBUG_WARN_STATE, ("tcp_connect failed: %d\n", (int)err));
return err;
}
#if LWIP_DNS
/** DNS callback
* If ipaddr is non-NULL, resolving succeeded and the request can be sent, otherwise it failed.
*/
static void
httpc_dns_found(const char* hostname, const ip_addr_t *ipaddr, void *arg)
{
httpc_state_t* req = (httpc_state_t*)arg;
err_t err;
httpc_result_t result;
LWIP_UNUSED_ARG(hostname);
if (ipaddr != NULL) {
err = httpc_get_internal_addr(req, ipaddr);
if (err == ERR_OK) {
return;
}
result = HTTPC_RESULT_ERR_CONNECT;
} else {
LWIP_DEBUGF(HTTPC_DEBUG_WARN_STATE, ("httpc_dns_found: failed to resolve hostname: %s\n",
hostname));
result = HTTPC_RESULT_ERR_HOSTNAME;
err = ERR_ARG;
}
httpc_close(req, result, 0, err);
}
#endif /* LWIP_DNS */
/** Start the http request after converting 'server_name' to ip address (DNS or address string) */
static err_t
httpc_get_internal_dns(httpc_state_t* req, const char* server_name)
{
err_t err;
LWIP_ASSERT("req != NULL", req != NULL);
#if LWIP_DNS
err = dns_gethostbyname(server_name, &req->remote_addr, httpc_dns_found, req);
#else
err = ipaddr_aton(server_name, &req->remote_addr) ? ERR_OK : ERR_ARG;
#endif
if (err == ERR_OK) {
/* cached or IP-string */
err = httpc_get_internal_addr(req, &req->remote_addr);
} else if (err == ERR_INPROGRESS) {
return ERR_OK;
}
return err;
}
static int
httpc_create_request_string(const httpc_connection_t *settings, const char* server_name, int server_port, const char* uri,
int use_host, char *buffer, size_t buffer_size)
{
if (settings->use_proxy) {
LWIP_ASSERT("server_name != NULL", server_name != NULL);
if (server_port != HTTP_DEFAULT_PORT) {
return snprintf(buffer, buffer_size, HTTPC_REQ_11_PROXY_PORT_FORMAT(server_name, server_port, uri, server_name));
} else {
return snprintf(buffer, buffer_size, HTTPC_REQ_11_PROXY_FORMAT(server_name, uri, server_name));
}
} else if (use_host) {
LWIP_ASSERT("server_name != NULL", server_name != NULL);
return snprintf(buffer, buffer_size, HTTPC_REQ_11_HOST_FORMAT(uri, server_name));
} else {
return snprintf(buffer, buffer_size, HTTPC_REQ_11_FORMAT(uri));
}
}
/** Initialize the connection struct */
static err_t
httpc_init_connection_common(httpc_state_t **connection, const httpc_connection_t *settings, const char* server_name,
u16_t server_port, const char* uri, altcp_recv_fn recv_fn, void* callback_arg, int use_host)
{
size_t alloc_len;
mem_size_t mem_alloc_len;
int req_len, req_len2;
httpc_state_t *req;
#if HTTPC_DEBUG_REQUEST
size_t server_name_len, uri_len;
#endif
LWIP_ASSERT("uri != NULL", uri != NULL);
/* get request len */
req_len = httpc_create_request_string(settings, server_name, server_port, uri, use_host, NULL, 0);
if ((req_len < 0) || (req_len > 0xFFFF)) {
return ERR_VAL;
}
/* alloc state and request in one block */
alloc_len = sizeof(httpc_state_t);
#if HTTPC_DEBUG_REQUEST
server_name_len = server_name ? strlen(server_name) : 0;
uri_len = strlen(uri);
alloc_len += server_name_len + 1 + uri_len + 1;
#endif
mem_alloc_len = (mem_size_t)alloc_len;
if ((mem_alloc_len < alloc_len) || (req_len + 1 > 0xFFFF)) {
return ERR_VAL;
}
req = (httpc_state_t*)mem_malloc((mem_size_t)alloc_len);
if(req == NULL) {
return ERR_MEM;
}
memset(req, 0, sizeof(httpc_state_t));
req->timeout_ticks = HTTPC_POLL_TIMEOUT;
req->request = pbuf_alloc(PBUF_RAW, (u16_t)(req_len + 1), PBUF_RAM);
if (req->request == NULL) {
httpc_free_state(req);
return ERR_MEM;
}
if (req->request->next != NULL) {
/* need a pbuf in one piece */
httpc_free_state(req);
return ERR_MEM;
}
req->hdr_content_len = HTTPC_CONTENT_LEN_INVALID;
#if HTTPC_DEBUG_REQUEST
req->server_name = (char*)(req + 1);
if (server_name) {
memcpy(req->server_name, server_name, server_name_len + 1);
}
req->uri = req->server_name + server_name_len + 1;
memcpy(req->uri, uri, uri_len + 1);
#endif
req->pcb = altcp_new(settings->altcp_allocator);
if(req->pcb == NULL) {
httpc_free_state(req);
return ERR_MEM;
}
req->remote_port = settings->use_proxy ? settings->proxy_port : server_port;
altcp_arg(req->pcb, req);
altcp_recv(req->pcb, httpc_tcp_recv);
altcp_err(req->pcb, httpc_tcp_err);
altcp_poll(req->pcb, httpc_tcp_poll, HTTPC_POLL_INTERVAL);
altcp_sent(req->pcb, httpc_tcp_sent);
/* set up request buffer */
req_len2 = httpc_create_request_string(settings, server_name, server_port, uri, use_host,
(char *)req->request->payload, req_len + 1);
if (req_len2 != req_len) {
httpc_free_state(req);
return ERR_VAL;
}
req->recv_fn = recv_fn;
req->conn_settings = settings;
req->callback_arg = callback_arg;
*connection = req;
return ERR_OK;
}
/**
* Initialize the connection struct
*/
static err_t
httpc_init_connection(httpc_state_t **connection, const httpc_connection_t *settings, const char* server_name,
u16_t server_port, const char* uri, altcp_recv_fn recv_fn, void* callback_arg)
{
return httpc_init_connection_common(connection, settings, server_name, server_port, uri, recv_fn, callback_arg, 1);
}
/**
* Initialize the connection struct (from IP address)
*/
static err_t
httpc_init_connection_addr(httpc_state_t **connection, const httpc_connection_t *settings,
const ip_addr_t* server_addr, u16_t server_port, const char* uri,
altcp_recv_fn recv_fn, void* callback_arg)
{
char *server_addr_str = ipaddr_ntoa(server_addr);
if (server_addr_str == NULL) {
return ERR_VAL;
}
return httpc_init_connection_common(connection, settings, server_addr_str, server_port, uri,
recv_fn, callback_arg, 1);
}
/**
* @ingroup httpc
* HTTP client API: get a file by passing server IP address
*
* @param server_addr IP address of the server to connect
* @param port tcp port of the server
* @param uri uri to get from the server, remember leading "/"!
* @param settings connection settings (callbacks, proxy, etc.)
* @param recv_fn the http body (not the headers) are passed to this callback
* @param callback_arg argument passed to all the callbacks
* @param connection retreives the connection handle (to match in callbacks)
* @return ERR_OK if starting the request succeeds (callback_fn will be called later)
* or an error code
*/
err_t
httpc_get_file(const ip_addr_t* server_addr, u16_t port, const char* uri, const httpc_connection_t *settings,
altcp_recv_fn recv_fn, void* callback_arg, httpc_state_t **connection)
{
err_t err;
httpc_state_t* req;
LWIP_ERROR("invalid parameters", (server_addr != NULL) && (uri != NULL) && (recv_fn != NULL), return ERR_ARG;);
err = httpc_init_connection_addr(&req, settings, server_addr, port,
uri, recv_fn, callback_arg);
if (err != ERR_OK) {
return err;
}
if (settings->use_proxy) {
err = httpc_get_internal_addr(req, &settings->proxy_addr);
} else {
err = httpc_get_internal_addr(req, server_addr);
}
if(err != ERR_OK) {
httpc_free_state(req);
return err;
}
if (connection != NULL) {
*connection = req;
}
return ERR_OK;
}
/**
* @ingroup httpc
* HTTP client API: get a file by passing server name as string (DNS name or IP address string)
*
* @param server_name server name as string (DNS name or IP address string)
* @param port tcp port of the server
* @param uri uri to get from the server, remember leading "/"!
* @param settings connection settings (callbacks, proxy, etc.)
* @param recv_fn the http body (not the headers) are passed to this callback
* @param callback_arg argument passed to all the callbacks
* @param connection retreives the connection handle (to match in callbacks)
* @return ERR_OK if starting the request succeeds (callback_fn will be called later)
* or an error code
*/
err_t
httpc_get_file_dns(const char* server_name, u16_t port, const char* uri, const httpc_connection_t *settings,
altcp_recv_fn recv_fn, void* callback_arg, httpc_state_t **connection)
{
err_t err;
httpc_state_t* req;
LWIP_ERROR("invalid parameters", (server_name != NULL) && (uri != NULL) && (recv_fn != NULL), return ERR_ARG;);
err = httpc_init_connection(&req, settings, server_name, port, uri, recv_fn, callback_arg);
if (err != ERR_OK) {
return err;
}
if (settings->use_proxy) {
err = httpc_get_internal_addr(req, &settings->proxy_addr);
} else {
err = httpc_get_internal_dns(req, server_name);
}
if(err != ERR_OK) {
httpc_free_state(req);
return err;
}
if (connection != NULL) {
*connection = req;
}
return ERR_OK;
}
#if LWIP_HTTPC_HAVE_FILE_IO
/* Implementation to disk via fopen/fwrite/fclose follows */
typedef struct _httpc_filestate
{
const char* local_file_name;
FILE *file;
httpc_connection_t settings;
const httpc_connection_t *client_settings;
void *callback_arg;
} httpc_filestate_t;
static void httpc_fs_result(void *arg, httpc_result_t httpc_result, u32_t rx_content_len,
u32_t srv_res, err_t err);
/** Initalize http client state for download to file system */
static err_t
httpc_fs_init(httpc_filestate_t **filestate_out, const char* local_file_name,
const httpc_connection_t *settings, void* callback_arg)
{
httpc_filestate_t *filestate;
size_t file_len, alloc_len;
FILE *f;
file_len = strlen(local_file_name);
alloc_len = sizeof(httpc_filestate_t) + file_len + 1;
filestate = (httpc_filestate_t *)mem_malloc((mem_size_t)alloc_len);
if (filestate == NULL) {
return ERR_MEM;
}
memset(filestate, 0, sizeof(httpc_filestate_t));
filestate->local_file_name = (const char *)(filestate + 1);
memcpy((char *)(filestate + 1), local_file_name, file_len + 1);
filestate->file = NULL;
filestate->client_settings = settings;
filestate->callback_arg = callback_arg;
/* copy client settings but override result callback */
memcpy(&filestate->settings, settings, sizeof(httpc_connection_t));
filestate->settings.result_fn = httpc_fs_result;
f = fopen(local_file_name, "wb");
if(f == NULL) {
/* could not open file */
mem_free(filestate);
return ERR_VAL;
}
filestate->file = f;
*filestate_out = filestate;
return ERR_OK;
}
/** Free http client state for download to file system */
static void
httpc_fs_free(httpc_filestate_t *filestate)
{
if (filestate != NULL) {
if (filestate->file != NULL) {
fclose(filestate->file);
filestate->file = NULL;
}
mem_free(filestate);
}
}
/** Connection closed (success or error) */
static void
httpc_fs_result(void *arg, httpc_result_t httpc_result, u32_t rx_content_len,
u32_t srv_res, err_t err)
{
httpc_filestate_t *filestate = (httpc_filestate_t *)arg;
if (filestate != NULL) {
if (filestate->client_settings->result_fn != NULL) {
filestate->client_settings->result_fn(filestate->callback_arg, httpc_result, rx_content_len,
srv_res, err);
}
httpc_fs_free(filestate);
}
}
/** tcp recv callback */
static err_t
httpc_fs_tcp_recv(void *arg, struct altcp_pcb *pcb, struct pbuf *p, err_t err)
{
httpc_filestate_t *filestate = (httpc_filestate_t*)arg;
struct pbuf* q;
LWIP_UNUSED_ARG(err);
LWIP_ASSERT("p != NULL", p != NULL);
for (q = p; q != NULL; q = q->next) {
fwrite(q->payload, 1, q->len, filestate->file);
}
altcp_recved(pcb, p->tot_len);
pbuf_free(p);
return ERR_OK;
}
/**
* @ingroup httpc
* HTTP client API: get a file to disk by passing server IP address
*
* @param server_addr IP address of the server to connect
* @param port tcp port of the server
* @param uri uri to get from the server, remember leading "/"!
* @param settings connection settings (callbacks, proxy, etc.)
* @param callback_arg argument passed to all the callbacks
* @param connection retreives the connection handle (to match in callbacks)
* @return ERR_OK if starting the request succeeds (callback_fn will be called later)
* or an error code
*/
err_t
httpc_get_file_to_disk(const ip_addr_t* server_addr, u16_t port, const char* uri, const httpc_connection_t *settings,
void* callback_arg, const char* local_file_name, httpc_state_t **connection)
{
err_t err;
httpc_state_t* req;
httpc_filestate_t *filestate;
LWIP_ERROR("invalid parameters", (server_addr != NULL) && (uri != NULL) && (local_file_name != NULL), return ERR_ARG;);
err = httpc_fs_init(&filestate, local_file_name, settings, callback_arg);
if (err != ERR_OK) {
return err;
}
err = httpc_init_connection_addr(&req, &filestate->settings, server_addr, port,
uri, httpc_fs_tcp_recv, filestate);
if (err != ERR_OK) {
httpc_fs_free(filestate);
return err;
}
if (settings->use_proxy) {
err = httpc_get_internal_addr(req, &settings->proxy_addr);
} else {
err = httpc_get_internal_addr(req, server_addr);
}
if(err != ERR_OK) {
httpc_fs_free(filestate);
httpc_free_state(req);
return err;
}
if (connection != NULL) {
*connection = req;
}
return ERR_OK;
}
/**
* @ingroup httpc
* HTTP client API: get a file to disk by passing server name as string (DNS name or IP address string)
*
* @param server_name server name as string (DNS name or IP address string)
* @param port tcp port of the server
* @param uri uri to get from the server, remember leading "/"!
* @param settings connection settings (callbacks, proxy, etc.)
* @param callback_arg argument passed to all the callbacks
* @param connection retreives the connection handle (to match in callbacks)
* @return ERR_OK if starting the request succeeds (callback_fn will be called later)
* or an error code
*/
err_t
httpc_get_file_dns_to_disk(const char* server_name, u16_t port, const char* uri, const httpc_connection_t *settings,
void* callback_arg, const char* local_file_name, httpc_state_t **connection)
{
err_t err;
httpc_state_t* req;
httpc_filestate_t *filestate;
LWIP_ERROR("invalid parameters", (server_name != NULL) && (uri != NULL) && (local_file_name != NULL), return ERR_ARG;);
err = httpc_fs_init(&filestate, local_file_name, settings, callback_arg);
if (err != ERR_OK) {
return err;
}
err = httpc_init_connection(&req, &filestate->settings, server_name, port,
uri, httpc_fs_tcp_recv, filestate);
if (err != ERR_OK) {
httpc_fs_free(filestate);
return err;
}
if (settings->use_proxy) {
err = httpc_get_internal_addr(req, &settings->proxy_addr);
} else {
err = httpc_get_internal_dns(req, server_name);
}
if(err != ERR_OK) {
httpc_fs_free(filestate);
httpc_free_state(req);
return err;
}
if (connection != NULL) {
*connection = req;
}
return ERR_OK;
}
#endif /* LWIP_HTTPC_HAVE_FILE_IO */
#endif /* LWIP_TCP && LWIP_CALLBACK_API */

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,123 @@
#ifndef LWIP_HTTPD_STRUCTS_H
#define LWIP_HTTPD_STRUCTS_H
#include "lwip/apps/httpd.h"
#if LWIP_HTTPD_DYNAMIC_HEADERS
/** This struct is used for a list of HTTP header strings for various
* filename extensions. */
typedef struct {
const char *extension;
const char *content_type;
} tHTTPHeader;
/** A list of strings used in HTTP headers (see RFC 1945 HTTP/1.0 and
* RFC 2616 HTTP/1.1 for header field definitions) */
static const char *const g_psHTTPHeaderStrings[] = {
"HTTP/1.0 200 OK\r\n",
"HTTP/1.0 404 File not found\r\n",
"HTTP/1.0 400 Bad Request\r\n",
"HTTP/1.0 501 Not Implemented\r\n",
"HTTP/1.1 200 OK\r\n",
"HTTP/1.1 404 File not found\r\n",
"HTTP/1.1 400 Bad Request\r\n",
"HTTP/1.1 501 Not Implemented\r\n",
"Content-Length: ",
"Connection: Close\r\n",
"Connection: keep-alive\r\n",
"Connection: keep-alive\r\nContent-Length: ",
"Server: "HTTPD_SERVER_AGENT"\r\n",
"\r\n<html><body><h2>404: The requested file cannot be found.</h2></body></html>\r\n"
#if LWIP_HTTPD_SUPPORT_11_KEEPALIVE
, "Connection: keep-alive\r\nContent-Length: 77\r\n\r\n<html><body><h2>404: The requested file cannot be found.</h2></body></html>\r\n"
#endif
};
/* Indexes into the g_psHTTPHeaderStrings array */
#define HTTP_HDR_OK 0 /* 200 OK */
#define HTTP_HDR_NOT_FOUND 1 /* 404 File not found */
#define HTTP_HDR_BAD_REQUEST 2 /* 400 Bad request */
#define HTTP_HDR_NOT_IMPL 3 /* 501 Not Implemented */
#define HTTP_HDR_OK_11 4 /* 200 OK */
#define HTTP_HDR_NOT_FOUND_11 5 /* 404 File not found */
#define HTTP_HDR_BAD_REQUEST_11 6 /* 400 Bad request */
#define HTTP_HDR_NOT_IMPL_11 7 /* 501 Not Implemented */
#define HTTP_HDR_CONTENT_LENGTH 8 /* Content-Length: (HTTP 1.0)*/
#define HTTP_HDR_CONN_CLOSE 9 /* Connection: Close (HTTP 1.1) */
#define HTTP_HDR_CONN_KEEPALIVE 10 /* Connection: keep-alive (HTTP 1.1) */
#define HTTP_HDR_KEEPALIVE_LEN 11 /* Connection: keep-alive + Content-Length: (HTTP 1.1)*/
#define HTTP_HDR_SERVER 12 /* Server: HTTPD_SERVER_AGENT */
#define DEFAULT_404_HTML 13 /* default 404 body */
#if LWIP_HTTPD_SUPPORT_11_KEEPALIVE
#define DEFAULT_404_HTML_PERSISTENT 14 /* default 404 body, but including Connection: keep-alive */
#endif
#define HTTP_CONTENT_TYPE(contenttype) "Content-Type: "contenttype"\r\n\r\n"
#define HTTP_CONTENT_TYPE_ENCODING(contenttype, encoding) "Content-Type: "contenttype"\r\nContent-Encoding: "encoding"\r\n\r\n"
#define HTTP_HDR_HTML HTTP_CONTENT_TYPE("text/html")
#define HTTP_HDR_SSI HTTP_CONTENT_TYPE("text/html\r\nExpires: Fri, 10 Apr 2008 14:00:00 GMT\r\nPragma: no-cache")
#define HTTP_HDR_GIF HTTP_CONTENT_TYPE("image/gif")
#define HTTP_HDR_PNG HTTP_CONTENT_TYPE("image/png")
#define HTTP_HDR_JPG HTTP_CONTENT_TYPE("image/jpeg")
#define HTTP_HDR_BMP HTTP_CONTENT_TYPE("image/bmp")
#define HTTP_HDR_ICO HTTP_CONTENT_TYPE("image/x-icon")
#define HTTP_HDR_APP HTTP_CONTENT_TYPE("application/octet-stream")
#define HTTP_HDR_JS HTTP_CONTENT_TYPE("application/javascript")
#define HTTP_HDR_RA HTTP_CONTENT_TYPE("application/javascript")
#define HTTP_HDR_CSS HTTP_CONTENT_TYPE("text/css")
#define HTTP_HDR_SWF HTTP_CONTENT_TYPE("application/x-shockwave-flash")
#define HTTP_HDR_XML HTTP_CONTENT_TYPE("text/xml")
#define HTTP_HDR_PDF HTTP_CONTENT_TYPE("application/pdf")
#define HTTP_HDR_JSON HTTP_CONTENT_TYPE("application/json")
#define HTTP_HDR_CSV HTTP_CONTENT_TYPE("text/csv")
#define HTTP_HDR_TSV HTTP_CONTENT_TYPE("text/tsv")
#define HTTP_HDR_SVG HTTP_CONTENT_TYPE("image/svg+xml")
#define HTTP_HDR_SVGZ HTTP_CONTENT_TYPE_ENCODING("image/svg+xml", "gzip")
#define HTTP_HDR_DEFAULT_TYPE HTTP_CONTENT_TYPE("text/plain")
/** A list of extension-to-HTTP header strings (see outdated RFC 1700 MEDIA TYPES
* and http://www.iana.org/assignments/media-types for registered content types
* and subtypes) */
static const tHTTPHeader g_psHTTPHeaders[] = {
{ "html", HTTP_HDR_HTML},
{ "htm", HTTP_HDR_HTML},
{ "shtml", HTTP_HDR_SSI},
{ "shtm", HTTP_HDR_SSI},
{ "ssi", HTTP_HDR_SSI},
{ "gif", HTTP_HDR_GIF},
{ "png", HTTP_HDR_PNG},
{ "jpg", HTTP_HDR_JPG},
{ "bmp", HTTP_HDR_BMP},
{ "ico", HTTP_HDR_ICO},
{ "class", HTTP_HDR_APP},
{ "cls", HTTP_HDR_APP},
{ "js", HTTP_HDR_JS},
{ "ram", HTTP_HDR_RA},
{ "css", HTTP_HDR_CSS},
{ "swf", HTTP_HDR_SWF},
{ "xml", HTTP_HDR_XML},
{ "xsl", HTTP_HDR_XML},
{ "pdf", HTTP_HDR_PDF},
{ "json", HTTP_HDR_JSON}
#ifdef HTTPD_ADDITIONAL_CONTENT_TYPES
/* If you need to add content types not listed here:
* #define HTTPD_ADDITIONAL_CONTENT_TYPES {"ct1", HTTP_CONTENT_TYPE("text/ct1")}, {"exe", HTTP_CONTENT_TYPE("application/exe")}
*/
, HTTPD_ADDITIONAL_CONTENT_TYPES
#endif
};
#define NUM_HTTP_HEADERS LWIP_ARRAYSIZE(g_psHTTPHeaders)
#endif /* LWIP_HTTPD_DYNAMIC_HEADERS */
#if LWIP_HTTPD_SSI
static const char *const g_pcSSIExtensions[] = {
".shtml", ".shtm", ".ssi", ".xml", ".json"
};
#define NUM_SHTML_EXTENSIONS LWIP_ARRAYSIZE(g_pcSSIExtensions)
#endif /* LWIP_HTTPD_SSI */
#endif /* LWIP_HTTPD_STRUCTS_H */

View File

@ -0,0 +1,97 @@
#!/usr/bin/perl
open(OUTPUT, "> fsdata.c");
chdir("fs");
open(FILES, "find . -type f |");
while($file = <FILES>) {
# Do not include files in CVS directories nor backup files.
if($file =~ /(CVS|~)/) {
next;
}
chop($file);
open(HEADER, "> /tmp/header") || die $!;
if($file =~ /404/) {
print(HEADER "HTTP/1.0 404 File not found\r\n");
} else {
print(HEADER "HTTP/1.0 200 OK\r\n");
}
print(HEADER "Server: lwIP/pre-0.6 (http://www.sics.se/~adam/lwip/)\r\n");
if($file =~ /\.html$/) {
print(HEADER "Content-type: text/html\r\n");
} elsif($file =~ /\.gif$/) {
print(HEADER "Content-type: image/gif\r\n");
} elsif($file =~ /\.png$/) {
print(HEADER "Content-type: image/png\r\n");
} elsif($file =~ /\.jpg$/) {
print(HEADER "Content-type: image/jpeg\r\n");
} elsif($file =~ /\.class$/) {
print(HEADER "Content-type: application/octet-stream\r\n");
} elsif($file =~ /\.ram$/) {
print(HEADER "Content-type: audio/x-pn-realaudio\r\n");
} else {
print(HEADER "Content-type: text/plain\r\n");
}
print(HEADER "\r\n");
close(HEADER);
unless($file =~ /\.plain$/ || $file =~ /cgi/) {
system("cat /tmp/header $file > /tmp/file");
} else {
system("cp $file /tmp/file");
}
open(FILE, "/tmp/file");
unlink("/tmp/file");
unlink("/tmp/header");
$file =~ s/\.//;
$fvar = $file;
$fvar =~ s-/-_-g;
$fvar =~ s-\.-_-g;
print(OUTPUT "static const unsigned char data".$fvar."[] = {\n");
print(OUTPUT "\t/* $file */\n\t");
for($j = 0; $j < length($file); $j++) {
printf(OUTPUT "%#02x, ", unpack("C", substr($file, $j, 1)));
}
printf(OUTPUT "0,\n");
$i = 0;
while(read(FILE, $data, 1)) {
if($i == 0) {
print(OUTPUT "\t");
}
printf(OUTPUT "%#02x, ", unpack("C", $data));
$i++;
if($i == 10) {
print(OUTPUT "\n");
$i = 0;
}
}
print(OUTPUT "};\n\n");
close(FILE);
push(@fvars, $fvar);
push(@files, $file);
}
for($i = 0; $i < @fvars; $i++) {
$file = $files[$i];
$fvar = $fvars[$i];
if($i == 0) {
$prevfile = "NULL";
} else {
$prevfile = "file" . $fvars[$i - 1];
}
print(OUTPUT "const struct fsdata_file file".$fvar."[] = {{$prevfile, data$fvar, ");
print(OUTPUT "data$fvar + ". (length($file) + 1) .", ");
print(OUTPUT "sizeof(data$fvar) - ". (length($file) + 1) ."}};\n\n");
}
print(OUTPUT "#define FS_ROOT file$fvars[$i - 1]\n\n");
print(OUTPUT "#define FS_NUMFILES $i\n");

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,13 @@
This directory contains a script ('makefsdata') to create C code suitable for
httpd for given html pages (or other files) in a directory.
There is also a plain C console application doing the same and extended a bit.
Usage: htmlgen [targetdir] [-s] [-i]s
targetdir: relative or absolute path to files to convert
switch -s: toggle processing of subdirectories (default is on)
switch -e: exclude HTTP header from file (header is created at runtime, default is on)
switch -11: include HTTP 1.1 header (1.0 is default)
if targetdir not specified, makefsdata will attempt to
process files in subdirectory 'fs'.

View File

@ -0,0 +1,808 @@
/*
Copyright (c) 2013-2017, tinydir authors:
- Cong Xu
- Lautis Sun
- Baudouin Feildel
- Andargor <andargor@yahoo.com>
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef TINYDIR_H
#define TINYDIR_H
#ifdef __cplusplus
extern "C" {
#endif
#if ((defined _UNICODE) && !(defined UNICODE))
#define UNICODE
#endif
#if ((defined UNICODE) && !(defined _UNICODE))
#define _UNICODE
#endif
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#ifdef _MSC_VER
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
# include <tchar.h>
# pragma warning(push)
# pragma warning (disable : 4996)
#else
# include <dirent.h>
# include <libgen.h>
# include <sys/stat.h>
# include <stddef.h>
#endif
#ifdef __MINGW32__
# include <tchar.h>
#endif
/* types */
/* Windows UNICODE wide character support */
#if defined _MSC_VER || defined __MINGW32__
#define _tinydir_char_t TCHAR
#define TINYDIR_STRING(s) _TEXT(s)
#define _tinydir_strlen _tcslen
#define _tinydir_strcpy _tcscpy
#define _tinydir_strcat _tcscat
#define _tinydir_strcmp _tcscmp
#define _tinydir_strrchr _tcsrchr
#define _tinydir_strncmp _tcsncmp
#else
#define _tinydir_char_t char
#define TINYDIR_STRING(s) s
#define _tinydir_strlen strlen
#define _tinydir_strcpy strcpy
#define _tinydir_strcat strcat
#define _tinydir_strcmp strcmp
#define _tinydir_strrchr strrchr
#define _tinydir_strncmp strncmp
#endif
#if (defined _MSC_VER || defined __MINGW32__)
#include <windows.h>
#define _TINYDIR_PATH_MAX MAX_PATH
#elif defined __linux__
#include <linux/limits.h>
#define _TINYDIR_PATH_MAX PATH_MAX
#else
#define _TINYDIR_PATH_MAX 4096
#endif
#ifdef _MSC_VER
/* extra chars for the "\\*" mask */
# define _TINYDIR_PATH_EXTRA 2
#else
# define _TINYDIR_PATH_EXTRA 0
#endif
#define _TINYDIR_FILENAME_MAX 256
#if (defined _MSC_VER || defined __MINGW32__)
#define _TINYDIR_DRIVE_MAX 3
#endif
#ifdef _MSC_VER
# define _TINYDIR_FUNC static __inline
#elif !defined __STDC_VERSION__ || __STDC_VERSION__ < 199901L
# define _TINYDIR_FUNC static __inline__
#else
# define _TINYDIR_FUNC static inline
#endif
/* readdir_r usage; define TINYDIR_USE_READDIR_R to use it (if supported) */
#ifdef TINYDIR_USE_READDIR_R
/* readdir_r is a POSIX-only function, and may not be available under various
* environments/settings, e.g. MinGW. Use readdir fallback */
#if _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _BSD_SOURCE || _SVID_SOURCE ||\
_POSIX_SOURCE
# define _TINYDIR_HAS_READDIR_R
#endif
#if _POSIX_C_SOURCE >= 200112L
# define _TINYDIR_HAS_FPATHCONF
# include <unistd.h>
#endif
#if _BSD_SOURCE || _SVID_SOURCE || \
(_POSIX_C_SOURCE >= 200809L || _XOPEN_SOURCE >= 700)
# define _TINYDIR_HAS_DIRFD
# include <sys/types.h>
#endif
#if defined _TINYDIR_HAS_FPATHCONF && defined _TINYDIR_HAS_DIRFD &&\
defined _PC_NAME_MAX
# define _TINYDIR_USE_FPATHCONF
#endif
#if defined __MINGW32__ || !defined _TINYDIR_HAS_READDIR_R ||\
!(defined _TINYDIR_USE_FPATHCONF || defined NAME_MAX)
# define _TINYDIR_USE_READDIR
#endif
/* Use readdir by default */
#else
# define _TINYDIR_USE_READDIR
#endif
/* MINGW32 has two versions of dirent, ASCII and UNICODE*/
#ifndef _MSC_VER
#if (defined __MINGW32__) && (defined _UNICODE)
#define _TINYDIR_DIR _WDIR
#define _tinydir_dirent _wdirent
#define _tinydir_opendir _wopendir
#define _tinydir_readdir _wreaddir
#define _tinydir_closedir _wclosedir
#else
#define _TINYDIR_DIR DIR
#define _tinydir_dirent dirent
#define _tinydir_opendir opendir
#define _tinydir_readdir readdir
#define _tinydir_closedir closedir
#endif
#endif
/* Allow user to use a custom allocator by defining _TINYDIR_MALLOC and _TINYDIR_FREE. */
#if defined(_TINYDIR_MALLOC) && defined(_TINYDIR_FREE)
#elif !defined(_TINYDIR_MALLOC) && !defined(_TINYDIR_FREE)
#else
#error "Either define both alloc and free or none of them!"
#endif
#if !defined(_TINYDIR_MALLOC)
#define _TINYDIR_MALLOC(_size) malloc(_size)
#define _TINYDIR_FREE(_ptr) free(_ptr)
#endif /* !defined(_TINYDIR_MALLOC) */
typedef struct tinydir_file
{
_tinydir_char_t path[_TINYDIR_PATH_MAX];
_tinydir_char_t name[_TINYDIR_FILENAME_MAX];
_tinydir_char_t *extension;
int is_dir;
int is_reg;
#ifndef _MSC_VER
#ifdef __MINGW32__
struct _stat _s;
#else
struct stat _s;
#endif
#endif
} tinydir_file;
typedef struct tinydir_dir
{
_tinydir_char_t path[_TINYDIR_PATH_MAX];
int has_next;
size_t n_files;
tinydir_file *_files;
#ifdef _MSC_VER
HANDLE _h;
WIN32_FIND_DATA _f;
#else
_TINYDIR_DIR *_d;
struct _tinydir_dirent *_e;
#ifndef _TINYDIR_USE_READDIR
struct _tinydir_dirent *_ep;
#endif
#endif
} tinydir_dir;
/* declarations */
_TINYDIR_FUNC
int tinydir_open(tinydir_dir *dir, const _tinydir_char_t *path);
_TINYDIR_FUNC
int tinydir_open_sorted(tinydir_dir *dir, const _tinydir_char_t *path);
_TINYDIR_FUNC
void tinydir_close(tinydir_dir *dir);
_TINYDIR_FUNC
int tinydir_next(tinydir_dir *dir);
_TINYDIR_FUNC
int tinydir_readfile(const tinydir_dir *dir, tinydir_file *file);
_TINYDIR_FUNC
int tinydir_readfile_n(const tinydir_dir *dir, tinydir_file *file, size_t i);
_TINYDIR_FUNC
int tinydir_open_subdir_n(tinydir_dir *dir, size_t i);
_TINYDIR_FUNC
int tinydir_file_open(tinydir_file *file, const _tinydir_char_t *path);
_TINYDIR_FUNC
void _tinydir_get_ext(tinydir_file *file);
_TINYDIR_FUNC
int _tinydir_file_cmp(const void *a, const void *b);
#ifndef _MSC_VER
#ifndef _TINYDIR_USE_READDIR
_TINYDIR_FUNC
size_t _tinydir_dirent_buf_size(_TINYDIR_DIR *dirp);
#endif
#endif
/* definitions*/
_TINYDIR_FUNC
int tinydir_open(tinydir_dir *dir, const _tinydir_char_t *path)
{
#ifndef _MSC_VER
#ifndef _TINYDIR_USE_READDIR
int error;
int size; /* using int size */
#endif
#else
_tinydir_char_t path_buf[_TINYDIR_PATH_MAX];
#endif
_tinydir_char_t *pathp;
if (dir == NULL || path == NULL || _tinydir_strlen(path) == 0)
{
errno = EINVAL;
return -1;
}
if (_tinydir_strlen(path) + _TINYDIR_PATH_EXTRA >= _TINYDIR_PATH_MAX)
{
errno = ENAMETOOLONG;
return -1;
}
/* initialise dir */
dir->_files = NULL;
#ifdef _MSC_VER
dir->_h = INVALID_HANDLE_VALUE;
#else
dir->_d = NULL;
#ifndef _TINYDIR_USE_READDIR
dir->_ep = NULL;
#endif
#endif
tinydir_close(dir);
_tinydir_strcpy(dir->path, path);
/* Remove trailing slashes */
pathp = &dir->path[_tinydir_strlen(dir->path) - 1];
while (pathp != dir->path && (*pathp == TINYDIR_STRING('\\') || *pathp == TINYDIR_STRING('/')))
{
*pathp = TINYDIR_STRING('\0');
pathp++;
}
#ifdef _MSC_VER
_tinydir_strcpy(path_buf, dir->path);
_tinydir_strcat(path_buf, TINYDIR_STRING("\\*"));
#if (defined WINAPI_FAMILY) && (WINAPI_FAMILY != WINAPI_FAMILY_DESKTOP_APP)
dir->_h = FindFirstFileEx(path_buf, FindExInfoStandard, &dir->_f, FindExSearchNameMatch, NULL, 0);
#else
dir->_h = FindFirstFile(path_buf, &dir->_f);
#endif
if (dir->_h == INVALID_HANDLE_VALUE)
{
errno = ENOENT;
#else
dir->_d = _tinydir_opendir(path);
if (dir->_d == NULL)
{
#endif
goto bail;
}
/* read first file */
dir->has_next = 1;
#ifndef _MSC_VER
#ifdef _TINYDIR_USE_READDIR
dir->_e = _tinydir_readdir(dir->_d);
#else
/* allocate dirent buffer for readdir_r */
size = _tinydir_dirent_buf_size(dir->_d); /* conversion to int */
if (size == -1) return -1;
dir->_ep = (struct _tinydir_dirent*)_TINYDIR_MALLOC(size);
if (dir->_ep == NULL) return -1;
error = readdir_r(dir->_d, dir->_ep, &dir->_e);
if (error != 0) return -1;
#endif
if (dir->_e == NULL)
{
dir->has_next = 0;
}
#endif
return 0;
bail:
tinydir_close(dir);
return -1;
}
_TINYDIR_FUNC
int tinydir_open_sorted(tinydir_dir *dir, const _tinydir_char_t *path)
{
/* Count the number of files first, to pre-allocate the files array */
size_t n_files = 0;
if (tinydir_open(dir, path) == -1)
{
return -1;
}
while (dir->has_next)
{
n_files++;
if (tinydir_next(dir) == -1)
{
goto bail;
}
}
tinydir_close(dir);
if (tinydir_open(dir, path) == -1)
{
return -1;
}
dir->n_files = 0;
dir->_files = (tinydir_file *)_TINYDIR_MALLOC(sizeof *dir->_files * n_files);
if (dir->_files == NULL)
{
goto bail;
}
while (dir->has_next)
{
tinydir_file *p_file;
dir->n_files++;
p_file = &dir->_files[dir->n_files - 1];
if (tinydir_readfile(dir, p_file) == -1)
{
goto bail;
}
if (tinydir_next(dir) == -1)
{
goto bail;
}
/* Just in case the number of files has changed between the first and
second reads, terminate without writing into unallocated memory */
if (dir->n_files == n_files)
{
break;
}
}
qsort(dir->_files, dir->n_files, sizeof(tinydir_file), _tinydir_file_cmp);
return 0;
bail:
tinydir_close(dir);
return -1;
}
_TINYDIR_FUNC
void tinydir_close(tinydir_dir *dir)
{
if (dir == NULL)
{
return;
}
memset(dir->path, 0, sizeof(dir->path));
dir->has_next = 0;
dir->n_files = 0;
_TINYDIR_FREE(dir->_files);
dir->_files = NULL;
#ifdef _MSC_VER
if (dir->_h != INVALID_HANDLE_VALUE)
{
FindClose(dir->_h);
}
dir->_h = INVALID_HANDLE_VALUE;
#else
if (dir->_d)
{
_tinydir_closedir(dir->_d);
}
dir->_d = NULL;
dir->_e = NULL;
#ifndef _TINYDIR_USE_READDIR
_TINYDIR_FREE(dir->_ep);
dir->_ep = NULL;
#endif
#endif
}
_TINYDIR_FUNC
int tinydir_next(tinydir_dir *dir)
{
if (dir == NULL)
{
errno = EINVAL;
return -1;
}
if (!dir->has_next)
{
errno = ENOENT;
return -1;
}
#ifdef _MSC_VER
if (FindNextFile(dir->_h, &dir->_f) == 0)
#else
#ifdef _TINYDIR_USE_READDIR
dir->_e = _tinydir_readdir(dir->_d);
#else
if (dir->_ep == NULL)
{
return -1;
}
if (readdir_r(dir->_d, dir->_ep, &dir->_e) != 0)
{
return -1;
}
#endif
if (dir->_e == NULL)
#endif
{
dir->has_next = 0;
#ifdef _MSC_VER
if (GetLastError() != ERROR_SUCCESS &&
GetLastError() != ERROR_NO_MORE_FILES)
{
tinydir_close(dir);
errno = EIO;
return -1;
}
#endif
}
return 0;
}
_TINYDIR_FUNC
int tinydir_readfile(const tinydir_dir *dir, tinydir_file *file)
{
if (dir == NULL || file == NULL)
{
errno = EINVAL;
return -1;
}
#ifdef _MSC_VER
if (dir->_h == INVALID_HANDLE_VALUE)
#else
if (dir->_e == NULL)
#endif
{
errno = ENOENT;
return -1;
}
if (_tinydir_strlen(dir->path) +
_tinydir_strlen(
#ifdef _MSC_VER
dir->_f.cFileName
#else
dir->_e->d_name
#endif
) + 1 + _TINYDIR_PATH_EXTRA >=
_TINYDIR_PATH_MAX)
{
/* the path for the file will be too long */
errno = ENAMETOOLONG;
return -1;
}
if (_tinydir_strlen(
#ifdef _MSC_VER
dir->_f.cFileName
#else
dir->_e->d_name
#endif
) >= _TINYDIR_FILENAME_MAX)
{
errno = ENAMETOOLONG;
return -1;
}
_tinydir_strcpy(file->path, dir->path);
_tinydir_strcat(file->path, TINYDIR_STRING("/"));
_tinydir_strcpy(file->name,
#ifdef _MSC_VER
dir->_f.cFileName
#else
dir->_e->d_name
#endif
);
_tinydir_strcat(file->path, file->name);
#ifndef _MSC_VER
#ifdef __MINGW32__
if (_tstat(
#else
if (stat(
#endif
file->path, &file->_s) == -1)
{
return -1;
}
#endif
_tinydir_get_ext(file);
file->is_dir =
#ifdef _MSC_VER
!!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY);
#else
S_ISDIR(file->_s.st_mode);
#endif
file->is_reg =
#ifdef _MSC_VER
!!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_NORMAL) ||
(
!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_DEVICE) &&
!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) &&
!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_ENCRYPTED) &&
#ifdef FILE_ATTRIBUTE_INTEGRITY_STREAM
!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_INTEGRITY_STREAM) &&
#endif
#ifdef FILE_ATTRIBUTE_NO_SCRUB_DATA
!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_NO_SCRUB_DATA) &&
#endif
!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_OFFLINE) &&
!(dir->_f.dwFileAttributes & FILE_ATTRIBUTE_TEMPORARY));
#else
S_ISREG(file->_s.st_mode);
#endif
return 0;
}
_TINYDIR_FUNC
int tinydir_readfile_n(const tinydir_dir *dir, tinydir_file *file, size_t i)
{
if (dir == NULL || file == NULL)
{
errno = EINVAL;
return -1;
}
if (i >= dir->n_files)
{
errno = ENOENT;
return -1;
}
memcpy(file, &dir->_files[i], sizeof(tinydir_file));
_tinydir_get_ext(file);
return 0;
}
_TINYDIR_FUNC
int tinydir_open_subdir_n(tinydir_dir *dir, size_t i)
{
_tinydir_char_t path[_TINYDIR_PATH_MAX];
if (dir == NULL)
{
errno = EINVAL;
return -1;
}
if (i >= dir->n_files || !dir->_files[i].is_dir)
{
errno = ENOENT;
return -1;
}
_tinydir_strcpy(path, dir->_files[i].path);
tinydir_close(dir);
if (tinydir_open_sorted(dir, path) == -1)
{
return -1;
}
return 0;
}
/* Open a single file given its path */
_TINYDIR_FUNC
int tinydir_file_open(tinydir_file *file, const _tinydir_char_t *path)
{
tinydir_dir dir;
int result = 0;
int found = 0;
_tinydir_char_t dir_name_buf[_TINYDIR_PATH_MAX];
_tinydir_char_t file_name_buf[_TINYDIR_FILENAME_MAX];
_tinydir_char_t *dir_name;
_tinydir_char_t *base_name;
#if (defined _MSC_VER || defined __MINGW32__)
_tinydir_char_t drive_buf[_TINYDIR_PATH_MAX];
_tinydir_char_t ext_buf[_TINYDIR_FILENAME_MAX];
#endif
if (file == NULL || path == NULL || _tinydir_strlen(path) == 0)
{
errno = EINVAL;
return -1;
}
if (_tinydir_strlen(path) + _TINYDIR_PATH_EXTRA >= _TINYDIR_PATH_MAX)
{
errno = ENAMETOOLONG;
return -1;
}
/* Get the parent path */
#if (defined _MSC_VER || defined __MINGW32__)
#if ((defined _MSC_VER) && (_MSC_VER >= 1400))
_tsplitpath_s(
path,
drive_buf, _TINYDIR_DRIVE_MAX,
dir_name_buf, _TINYDIR_FILENAME_MAX,
file_name_buf, _TINYDIR_FILENAME_MAX,
ext_buf, _TINYDIR_FILENAME_MAX);
#else
_tsplitpath(
path,
drive_buf,
dir_name_buf,
file_name_buf,
ext_buf);
#endif
/* _splitpath_s not work fine with only filename and widechar support */
#ifdef _UNICODE
if (drive_buf[0] == L'\xFEFE')
drive_buf[0] = '\0';
if (dir_name_buf[0] == L'\xFEFE')
dir_name_buf[0] = '\0';
#endif
if (errno)
{
errno = EINVAL;
return -1;
}
/* Emulate the behavior of dirname by returning "." for dir name if it's
empty */
if (drive_buf[0] == '\0' && dir_name_buf[0] == '\0')
{
_tinydir_strcpy(dir_name_buf, TINYDIR_STRING("."));
}
/* Concatenate the drive letter and dir name to form full dir name */
_tinydir_strcat(drive_buf, dir_name_buf);
dir_name = drive_buf;
/* Concatenate the file name and extension to form base name */
_tinydir_strcat(file_name_buf, ext_buf);
base_name = file_name_buf;
#else
_tinydir_strcpy(dir_name_buf, path);
dir_name = dirname(dir_name_buf);
_tinydir_strcpy(file_name_buf, path);
base_name =basename(file_name_buf);
#endif
/* Open the parent directory */
if (tinydir_open(&dir, dir_name) == -1)
{
return -1;
}
/* Read through the parent directory and look for the file */
while (dir.has_next)
{
if (tinydir_readfile(&dir, file) == -1)
{
result = -1;
goto bail;
}
if (_tinydir_strcmp(file->name, base_name) == 0)
{
/* File found */
found = 1;
break;
}
tinydir_next(&dir);
}
if (!found)
{
result = -1;
errno = ENOENT;
}
bail:
tinydir_close(&dir);
return result;
}
_TINYDIR_FUNC
void _tinydir_get_ext(tinydir_file *file)
{
_tinydir_char_t *period = _tinydir_strrchr(file->name, TINYDIR_STRING('.'));
if (period == NULL)
{
file->extension = &(file->name[_tinydir_strlen(file->name)]);
}
else
{
file->extension = period + 1;
}
}
_TINYDIR_FUNC
int _tinydir_file_cmp(const void *a, const void *b)
{
const tinydir_file *fa = (const tinydir_file *)a;
const tinydir_file *fb = (const tinydir_file *)b;
if (fa->is_dir != fb->is_dir)
{
return -(fa->is_dir - fb->is_dir);
}
return _tinydir_strncmp(fa->name, fb->name, _TINYDIR_FILENAME_MAX);
}
#ifndef _MSC_VER
#ifndef _TINYDIR_USE_READDIR
/*
The following authored by Ben Hutchings <ben@decadent.org.uk>
from https://womble.decadent.org.uk/readdir_r-advisory.html
*/
/* Calculate the required buffer size (in bytes) for directory *
* entries read from the given directory handle. Return -1 if this *
* this cannot be done. *
* *
* This code does not trust values of NAME_MAX that are less than *
* 255, since some systems (including at least HP-UX) incorrectly *
* define it to be a smaller value. */
_TINYDIR_FUNC
size_t _tinydir_dirent_buf_size(_TINYDIR_DIR *dirp)
{
long name_max;
size_t name_end;
/* parameter may be unused */
(void)dirp;
#if defined _TINYDIR_USE_FPATHCONF
name_max = fpathconf(dirfd(dirp), _PC_NAME_MAX);
if (name_max == -1)
#if defined(NAME_MAX)
name_max = (NAME_MAX > 255) ? NAME_MAX : 255;
#else
return (size_t)(-1);
#endif
#elif defined(NAME_MAX)
name_max = (NAME_MAX > 255) ? NAME_MAX : 255;
#else
#error "buffer size for readdir_r cannot be determined"
#endif
name_end = (size_t)offsetof(struct _tinydir_dirent, d_name) + name_max + 1;
return (name_end > sizeof(struct _tinydir_dirent) ?
name_end : sizeof(struct _tinydir_dirent));
}
#endif
#endif
#ifdef __cplusplus
}
#endif
# if defined (_MSC_VER)
# pragma warning(pop)
# endif
#endif

View File

@ -0,0 +1,841 @@
/**
* @file
* lwIP iPerf server implementation
*/
/**
* @defgroup iperf Iperf server
* @ingroup apps
*
* This is a simple performance measuring client/server to check your bandwith using
* iPerf2 on a PC as server/client.
* It is currently a minimal implementation providing a TCP client/server only.
*
* @todo:
* - implement UDP mode
* - protect combined sessions handling (via 'related_master_state') against reallocation
* (this is a pointer address, currently, so if the same memory is allocated again,
* session pairs (tx/rx) can be confused on reallocation)
*/
/*
* Copyright (c) 2014 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt
*/
#include "lwip/apps/lwiperf.h"
#include "lwip/tcp.h"
#include "lwip/sys.h"
#include <string.h>
/* Currently, only TCP is implemented */
#if LWIP_TCP && LWIP_CALLBACK_API
/** Specify the idle timeout (in seconds) after that the test fails */
#ifndef LWIPERF_TCP_MAX_IDLE_SEC
#define LWIPERF_TCP_MAX_IDLE_SEC 10U
#endif
#if LWIPERF_TCP_MAX_IDLE_SEC > 255
#error LWIPERF_TCP_MAX_IDLE_SEC must fit into an u8_t
#endif
/** Change this if you don't want to lwiperf to listen to any IP version */
#ifndef LWIPERF_SERVER_IP_TYPE
#define LWIPERF_SERVER_IP_TYPE IPADDR_TYPE_ANY
#endif
/* File internal memory allocation (struct lwiperf_*): this defaults to
the heap */
#ifndef LWIPERF_ALLOC
#define LWIPERF_ALLOC(type) mem_malloc(sizeof(type))
#define LWIPERF_FREE(type, item) mem_free(item)
#endif
/** If this is 1, check that received data has the correct format */
#ifndef LWIPERF_CHECK_RX_DATA
#define LWIPERF_CHECK_RX_DATA 0
#endif
/** This is the Iperf settings struct sent from the client */
typedef struct _lwiperf_settings {
#define LWIPERF_FLAGS_ANSWER_TEST 0x80000000
#define LWIPERF_FLAGS_ANSWER_NOW 0x00000001
u32_t flags;
u32_t num_threads; /* unused for now */
u32_t remote_port;
u32_t buffer_len; /* unused for now */
u32_t win_band; /* TCP window / UDP rate: unused for now */
u32_t amount; /* pos. value: bytes?; neg. values: time (unit is 10ms: 1/100 second) */
} lwiperf_settings_t;
/** Basic connection handle */
struct _lwiperf_state_base;
typedef struct _lwiperf_state_base lwiperf_state_base_t;
struct _lwiperf_state_base {
/* linked list */
lwiperf_state_base_t *next;
/* 1=tcp, 0=udp */
u8_t tcp;
/* 1=server, 0=client */
u8_t server;
/* master state used to abort sessions (e.g. listener, main client) */
lwiperf_state_base_t *related_master_state;
};
/** Connection handle for a TCP iperf session */
typedef struct _lwiperf_state_tcp {
lwiperf_state_base_t base;
struct tcp_pcb *server_pcb;
struct tcp_pcb *conn_pcb;
u32_t time_started;
lwiperf_report_fn report_fn;
void *report_arg;
u8_t poll_count;
u8_t next_num;
/* 1=start server when client is closed */
u8_t client_tradeoff_mode;
u32_t bytes_transferred;
lwiperf_settings_t settings;
u8_t have_settings_buf;
u8_t specific_remote;
ip_addr_t remote_addr;
} lwiperf_state_tcp_t;
/** List of active iperf sessions */
static lwiperf_state_base_t *lwiperf_all_connections;
/** A const buffer to send from: we want to measure sending, not copying! */
static const u8_t lwiperf_txbuf_const[1600] = {
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
};
static err_t lwiperf_tcp_poll(void *arg, struct tcp_pcb *tpcb);
static void lwiperf_tcp_err(void *arg, err_t err);
static err_t lwiperf_start_tcp_server_impl(const ip_addr_t *local_addr, u16_t local_port,
lwiperf_report_fn report_fn, void *report_arg,
lwiperf_state_base_t *related_master_state, lwiperf_state_tcp_t **state);
/** Add an iperf session to the 'active' list */
static void
lwiperf_list_add(lwiperf_state_base_t *item)
{
item->next = lwiperf_all_connections;
lwiperf_all_connections = item;
}
/** Remove an iperf session from the 'active' list */
static void
lwiperf_list_remove(lwiperf_state_base_t *item)
{
lwiperf_state_base_t *prev = NULL;
lwiperf_state_base_t *iter;
for (iter = lwiperf_all_connections; iter != NULL; prev = iter, iter = iter->next) {
if (iter == item) {
if (prev == NULL) {
lwiperf_all_connections = iter->next;
} else {
prev->next = iter->next;
}
/* @debug: ensure this item is listed only once */
for (iter = iter->next; iter != NULL; iter = iter->next) {
LWIP_ASSERT("duplicate entry", iter != item);
}
break;
}
}
}
static lwiperf_state_base_t *
lwiperf_list_find(lwiperf_state_base_t *item)
{
lwiperf_state_base_t *iter;
for (iter = lwiperf_all_connections; iter != NULL; iter = iter->next) {
if (iter == item) {
return item;
}
}
return NULL;
}
/** Call the report function of an iperf tcp session */
static void
lwip_tcp_conn_report(lwiperf_state_tcp_t *conn, enum lwiperf_report_type report_type)
{
if ((conn != NULL) && (conn->report_fn != NULL)) {
u32_t now, duration_ms, bandwidth_kbitpsec;
now = sys_now();
duration_ms = now - conn->time_started;
if (duration_ms == 0) {
bandwidth_kbitpsec = 0;
} else {
bandwidth_kbitpsec = (conn->bytes_transferred / duration_ms) * 8U;
}
conn->report_fn(conn->report_arg, report_type,
&conn->conn_pcb->local_ip, conn->conn_pcb->local_port,
&conn->conn_pcb->remote_ip, conn->conn_pcb->remote_port,
conn->bytes_transferred, duration_ms, bandwidth_kbitpsec);
}
}
/** Close an iperf tcp session */
static void
lwiperf_tcp_close(lwiperf_state_tcp_t *conn, enum lwiperf_report_type report_type)
{
err_t err;
lwiperf_list_remove(&conn->base);
lwip_tcp_conn_report(conn, report_type);
if (conn->conn_pcb != NULL) {
tcp_arg(conn->conn_pcb, NULL);
tcp_poll(conn->conn_pcb, NULL, 0);
tcp_sent(conn->conn_pcb, NULL);
tcp_recv(conn->conn_pcb, NULL);
tcp_err(conn->conn_pcb, NULL);
err = tcp_close(conn->conn_pcb);
if (err != ERR_OK) {
/* don't want to wait for free memory here... */
tcp_abort(conn->conn_pcb);
}
} else {
/* no conn pcb, this is the listener pcb */
err = tcp_close(conn->server_pcb);
LWIP_ASSERT("error", err == ERR_OK);
}
LWIPERF_FREE(lwiperf_state_tcp_t, conn);
}
/** Try to send more data on an iperf tcp session */
static err_t
lwiperf_tcp_client_send_more(lwiperf_state_tcp_t *conn)
{
int send_more;
err_t err;
u16_t txlen;
u16_t txlen_max;
void *txptr;
u8_t apiflags;
LWIP_ASSERT("conn invalid", (conn != NULL) && conn->base.tcp && (conn->base.server == 0));
do {
send_more = 0;
if (conn->settings.amount & PP_HTONL(0x80000000)) {
/* this session is time-limited */
u32_t now = sys_now();
u32_t diff_ms = now - conn->time_started;
u32_t time = (u32_t) - (s32_t)lwip_htonl(conn->settings.amount);
u32_t time_ms = time * 10;
if (diff_ms >= time_ms) {
/* time specified by the client is over -> close the connection */
lwiperf_tcp_close(conn, LWIPERF_TCP_DONE_CLIENT);
return ERR_OK;
}
} else {
/* this session is byte-limited */
u32_t amount_bytes = lwip_htonl(conn->settings.amount);
/* @todo: this can send up to 1*MSS more than requested... */
if (amount_bytes >= conn->bytes_transferred) {
/* all requested bytes transferred -> close the connection */
lwiperf_tcp_close(conn, LWIPERF_TCP_DONE_CLIENT);
return ERR_OK;
}
}
if (conn->bytes_transferred < 24) {
/* transmit the settings a first time */
txptr = &((u8_t *)&conn->settings)[conn->bytes_transferred];
txlen_max = (u16_t)(24 - conn->bytes_transferred);
apiflags = TCP_WRITE_FLAG_COPY;
} else if (conn->bytes_transferred < 48) {
/* transmit the settings a second time */
txptr = &((u8_t *)&conn->settings)[conn->bytes_transferred - 24];
txlen_max = (u16_t)(48 - conn->bytes_transferred);
apiflags = TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE;
send_more = 1;
} else {
/* transmit data */
/* @todo: every x bytes, transmit the settings again */
txptr = LWIP_CONST_CAST(void *, &lwiperf_txbuf_const[conn->bytes_transferred % 10]);
txlen_max = TCP_MSS;
if (conn->bytes_transferred == 48) { /* @todo: fix this for intermediate settings, too */
txlen_max = TCP_MSS - 24;
}
apiflags = 0; /* no copying needed */
send_more = 1;
}
txlen = txlen_max;
do {
err = tcp_write(conn->conn_pcb, txptr, txlen, apiflags);
if (err == ERR_MEM) {
txlen /= 2;
}
} while ((err == ERR_MEM) && (txlen >= (TCP_MSS / 2)));
if (err == ERR_OK) {
conn->bytes_transferred += txlen;
} else {
send_more = 0;
}
} while (send_more);
tcp_output(conn->conn_pcb);
return ERR_OK;
}
/** TCP sent callback, try to send more data */
static err_t
lwiperf_tcp_client_sent(void *arg, struct tcp_pcb *tpcb, u16_t len)
{
lwiperf_state_tcp_t *conn = (lwiperf_state_tcp_t *)arg;
/* @todo: check 'len' (e.g. to time ACK of all data)? for now, we just send more... */
LWIP_ASSERT("invalid conn", conn->conn_pcb == tpcb);
LWIP_UNUSED_ARG(tpcb);
LWIP_UNUSED_ARG(len);
conn->poll_count = 0;
return lwiperf_tcp_client_send_more(conn);
}
/** TCP connected callback (active connection), send data now */
static err_t
lwiperf_tcp_client_connected(void *arg, struct tcp_pcb *tpcb, err_t err)
{
lwiperf_state_tcp_t *conn = (lwiperf_state_tcp_t *)arg;
LWIP_ASSERT("invalid conn", conn->conn_pcb == tpcb);
LWIP_UNUSED_ARG(tpcb);
if (err != ERR_OK) {
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_REMOTE);
return ERR_OK;
}
conn->poll_count = 0;
conn->time_started = sys_now();
return lwiperf_tcp_client_send_more(conn);
}
/** Start TCP connection back to the client (either parallel or after the
* receive test has finished.
*/
static err_t
lwiperf_tx_start_impl(const ip_addr_t *remote_ip, u16_t remote_port, lwiperf_settings_t *settings, lwiperf_report_fn report_fn,
void *report_arg, lwiperf_state_base_t *related_master_state, lwiperf_state_tcp_t **new_conn)
{
err_t err;
lwiperf_state_tcp_t *client_conn;
struct tcp_pcb *newpcb;
ip_addr_t remote_addr;
LWIP_ASSERT("remote_ip != NULL", remote_ip != NULL);
LWIP_ASSERT("remote_ip != NULL", settings != NULL);
LWIP_ASSERT("new_conn != NULL", new_conn != NULL);
*new_conn = NULL;
client_conn = (lwiperf_state_tcp_t *)LWIPERF_ALLOC(lwiperf_state_tcp_t);
if (client_conn == NULL) {
return ERR_MEM;
}
newpcb = tcp_new_ip_type(IP_GET_TYPE(remote_ip));
if (newpcb == NULL) {
LWIPERF_FREE(lwiperf_state_tcp_t, client_conn);
return ERR_MEM;
}
memset(client_conn, 0, sizeof(lwiperf_state_tcp_t));
client_conn->base.tcp = 1;
client_conn->base.related_master_state = related_master_state;
client_conn->conn_pcb = newpcb;
client_conn->time_started = sys_now(); /* @todo: set this again on 'connected' */
client_conn->report_fn = report_fn;
client_conn->report_arg = report_arg;
client_conn->next_num = 4; /* initial nr is '4' since the header has 24 byte */
client_conn->bytes_transferred = 0;
memcpy(&client_conn->settings, settings, sizeof(*settings));
client_conn->have_settings_buf = 1;
tcp_arg(newpcb, client_conn);
tcp_sent(newpcb, lwiperf_tcp_client_sent);
tcp_poll(newpcb, lwiperf_tcp_poll, 2U);
tcp_err(newpcb, lwiperf_tcp_err);
ip_addr_copy(remote_addr, *remote_ip);
err = tcp_connect(newpcb, &remote_addr, remote_port, lwiperf_tcp_client_connected);
if (err != ERR_OK) {
lwiperf_tcp_close(client_conn, LWIPERF_TCP_ABORTED_LOCAL);
return err;
}
lwiperf_list_add(&client_conn->base);
*new_conn = client_conn;
return ERR_OK;
}
static err_t
lwiperf_tx_start_passive(lwiperf_state_tcp_t *conn)
{
err_t ret;
lwiperf_state_tcp_t *new_conn = NULL;
u16_t remote_port = (u16_t)lwip_htonl(conn->settings.remote_port);
ret = lwiperf_tx_start_impl(&conn->conn_pcb->remote_ip, remote_port, &conn->settings, conn->report_fn, conn->report_arg,
conn->base.related_master_state, &new_conn);
if (ret == ERR_OK) {
LWIP_ASSERT("new_conn != NULL", new_conn != NULL);
new_conn->settings.flags = 0; /* prevent the remote side starting back as client again */
}
return ret;
}
/** Receive data on an iperf tcp session */
static err_t
lwiperf_tcp_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err)
{
u8_t tmp;
u16_t tot_len;
u32_t packet_idx;
struct pbuf *q;
lwiperf_state_tcp_t *conn = (lwiperf_state_tcp_t *)arg;
LWIP_ASSERT("pcb mismatch", conn->conn_pcb == tpcb);
LWIP_UNUSED_ARG(tpcb);
if (err != ERR_OK) {
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_REMOTE);
return ERR_OK;
}
if (p == NULL) {
/* connection closed -> test done */
if (conn->settings.flags & PP_HTONL(LWIPERF_FLAGS_ANSWER_TEST)) {
if ((conn->settings.flags & PP_HTONL(LWIPERF_FLAGS_ANSWER_NOW)) == 0) {
/* client requested transmission after end of test */
lwiperf_tx_start_passive(conn);
}
}
lwiperf_tcp_close(conn, LWIPERF_TCP_DONE_SERVER);
return ERR_OK;
}
tot_len = p->tot_len;
conn->poll_count = 0;
if ((!conn->have_settings_buf) || ((conn->bytes_transferred - 24) % (1024 * 128) == 0)) {
/* wait for 24-byte header */
if (p->tot_len < sizeof(lwiperf_settings_t)) {
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_LOCAL_DATAERROR);
pbuf_free(p);
return ERR_OK;
}
if (!conn->have_settings_buf) {
if (pbuf_copy_partial(p, &conn->settings, sizeof(lwiperf_settings_t), 0) != sizeof(lwiperf_settings_t)) {
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_LOCAL);
pbuf_free(p);
return ERR_OK;
}
conn->have_settings_buf = 1;
if (conn->settings.flags & PP_HTONL(LWIPERF_FLAGS_ANSWER_TEST)) {
if (conn->settings.flags & PP_HTONL(LWIPERF_FLAGS_ANSWER_NOW)) {
/* client requested parallel transmission test */
err_t err2 = lwiperf_tx_start_passive(conn);
if (err2 != ERR_OK) {
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_LOCAL_TXERROR);
pbuf_free(p);
return ERR_OK;
}
}
}
} else {
if (conn->settings.flags & PP_HTONL(LWIPERF_FLAGS_ANSWER_TEST)) {
if (pbuf_memcmp(p, 0, &conn->settings, sizeof(lwiperf_settings_t)) != 0) {
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_LOCAL_DATAERROR);
pbuf_free(p);
return ERR_OK;
}
}
}
conn->bytes_transferred += sizeof(lwiperf_settings_t);
if (conn->bytes_transferred <= 24) {
conn->time_started = sys_now();
tcp_recved(tpcb, p->tot_len);
pbuf_free(p);
return ERR_OK;
}
conn->next_num = 4; /* 24 bytes received... */
tmp = pbuf_remove_header(p, 24);
LWIP_ASSERT("pbuf_remove_header failed", tmp == 0);
LWIP_UNUSED_ARG(tmp); /* for LWIP_NOASSERT */
}
packet_idx = 0;
for (q = p; q != NULL; q = q->next) {
#if LWIPERF_CHECK_RX_DATA
const u8_t *payload = (const u8_t *)q->payload;
u16_t i;
for (i = 0; i < q->len; i++) {
u8_t val = payload[i];
u8_t num = val - '0';
if (num == conn->next_num) {
conn->next_num++;
if (conn->next_num == 10) {
conn->next_num = 0;
}
} else {
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_LOCAL_DATAERROR);
pbuf_free(p);
return ERR_OK;
}
}
#endif
packet_idx += q->len;
}
LWIP_ASSERT("count mismatch", packet_idx == p->tot_len);
conn->bytes_transferred += packet_idx;
tcp_recved(tpcb, tot_len);
pbuf_free(p);
return ERR_OK;
}
/** Error callback, iperf tcp session aborted */
static void
lwiperf_tcp_err(void *arg, err_t err)
{
lwiperf_state_tcp_t *conn = (lwiperf_state_tcp_t *)arg;
LWIP_UNUSED_ARG(err);
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_REMOTE);
}
/** TCP poll callback, try to send more data */
static err_t
lwiperf_tcp_poll(void *arg, struct tcp_pcb *tpcb)
{
lwiperf_state_tcp_t *conn = (lwiperf_state_tcp_t *)arg;
LWIP_ASSERT("pcb mismatch", conn->conn_pcb == tpcb);
LWIP_UNUSED_ARG(tpcb);
if (++conn->poll_count >= LWIPERF_TCP_MAX_IDLE_SEC) {
lwiperf_tcp_close(conn, LWIPERF_TCP_ABORTED_LOCAL);
return ERR_OK; /* lwiperf_tcp_close frees conn */
}
if (!conn->base.server) {
lwiperf_tcp_client_send_more(conn);
}
return ERR_OK;
}
/** This is called when a new client connects for an iperf tcp session */
static err_t
lwiperf_tcp_accept(void *arg, struct tcp_pcb *newpcb, err_t err)
{
lwiperf_state_tcp_t *s, *conn;
if ((err != ERR_OK) || (newpcb == NULL) || (arg == NULL)) {
return ERR_VAL;
}
s = (lwiperf_state_tcp_t *)arg;
LWIP_ASSERT("invalid session", s->base.server);
LWIP_ASSERT("invalid listen pcb", s->server_pcb != NULL);
LWIP_ASSERT("invalid conn pcb", s->conn_pcb == NULL);
if (s->specific_remote) {
LWIP_ASSERT("s->base.related_master_state != NULL", s->base.related_master_state != NULL);
if (!ip_addr_cmp(&newpcb->remote_ip, &s->remote_addr)) {
/* this listener belongs to a client session, and this is not the correct remote */
return ERR_VAL;
}
} else {
LWIP_ASSERT("s->base.related_master_state == NULL", s->base.related_master_state == NULL);
}
conn = (lwiperf_state_tcp_t *)LWIPERF_ALLOC(lwiperf_state_tcp_t);
if (conn == NULL) {
return ERR_MEM;
}
memset(conn, 0, sizeof(lwiperf_state_tcp_t));
conn->base.tcp = 1;
conn->base.server = 1;
conn->base.related_master_state = &s->base;
conn->conn_pcb = newpcb;
conn->time_started = sys_now();
conn->report_fn = s->report_fn;
conn->report_arg = s->report_arg;
/* setup the tcp rx connection */
tcp_arg(newpcb, conn);
tcp_recv(newpcb, lwiperf_tcp_recv);
tcp_poll(newpcb, lwiperf_tcp_poll, 2U);
tcp_err(conn->conn_pcb, lwiperf_tcp_err);
if (s->specific_remote) {
/* this listener belongs to a client, so make the client the master of the newly created connection */
conn->base.related_master_state = s->base.related_master_state;
/* if dual mode or (tradeoff mode AND client is done): close the listener */
if (!s->client_tradeoff_mode || !lwiperf_list_find(s->base.related_master_state)) {
/* prevent report when closing: this is expected */
s->report_fn = NULL;
lwiperf_tcp_close(s, LWIPERF_TCP_ABORTED_LOCAL);
}
}
lwiperf_list_add(&conn->base);
return ERR_OK;
}
/**
* @ingroup iperf
* Start a TCP iperf server on the default TCP port (5001) and listen for
* incoming connections from iperf clients.
*
* @returns a connection handle that can be used to abort the server
* by calling @ref lwiperf_abort()
*/
void *
lwiperf_start_tcp_server_default(lwiperf_report_fn report_fn, void *report_arg)
{
return lwiperf_start_tcp_server(IP_ADDR_ANY, LWIPERF_TCP_PORT_DEFAULT,
report_fn, report_arg);
}
/**
* @ingroup iperf
* Start a TCP iperf server on a specific IP address and port and listen for
* incoming connections from iperf clients.
*
* @returns a connection handle that can be used to abort the server
* by calling @ref lwiperf_abort()
*/
void *
lwiperf_start_tcp_server(const ip_addr_t *local_addr, u16_t local_port,
lwiperf_report_fn report_fn, void *report_arg)
{
err_t err;
lwiperf_state_tcp_t *state = NULL;
err = lwiperf_start_tcp_server_impl(local_addr, local_port, report_fn, report_arg,
NULL, &state);
if (err == ERR_OK) {
return state;
}
return NULL;
}
static err_t lwiperf_start_tcp_server_impl(const ip_addr_t *local_addr, u16_t local_port,
lwiperf_report_fn report_fn, void *report_arg,
lwiperf_state_base_t *related_master_state, lwiperf_state_tcp_t **state)
{
err_t err;
struct tcp_pcb *pcb;
lwiperf_state_tcp_t *s;
LWIP_ASSERT_CORE_LOCKED();
LWIP_ASSERT("state != NULL", state != NULL);
if (local_addr == NULL) {
return ERR_ARG;
}
s = (lwiperf_state_tcp_t *)LWIPERF_ALLOC(lwiperf_state_tcp_t);
if (s == NULL) {
return ERR_MEM;
}
memset(s, 0, sizeof(lwiperf_state_tcp_t));
s->base.tcp = 1;
s->base.server = 1;
s->base.related_master_state = related_master_state;
s->report_fn = report_fn;
s->report_arg = report_arg;
pcb = tcp_new_ip_type(LWIPERF_SERVER_IP_TYPE);
if (pcb == NULL) {
return ERR_MEM;
}
err = tcp_bind(pcb, local_addr, local_port);
if (err != ERR_OK) {
return err;
}
s->server_pcb = tcp_listen_with_backlog(pcb, 1);
if (s->server_pcb == NULL) {
if (pcb != NULL) {
tcp_close(pcb);
}
LWIPERF_FREE(lwiperf_state_tcp_t, s);
return ERR_MEM;
}
pcb = NULL;
tcp_arg(s->server_pcb, s);
tcp_accept(s->server_pcb, lwiperf_tcp_accept);
lwiperf_list_add(&s->base);
*state = s;
return ERR_OK;
}
/**
* @ingroup iperf
* Start a TCP iperf client to the default TCP port (5001).
*
* @returns a connection handle that can be used to abort the client
* by calling @ref lwiperf_abort()
*/
void* lwiperf_start_tcp_client_default(const ip_addr_t* remote_addr,
lwiperf_report_fn report_fn, void* report_arg)
{
return lwiperf_start_tcp_client(remote_addr, LWIPERF_TCP_PORT_DEFAULT, LWIPERF_CLIENT,
report_fn, report_arg);
}
/**
* @ingroup iperf
* Start a TCP iperf client to a specific IP address and port.
*
* @returns a connection handle that can be used to abort the client
* by calling @ref lwiperf_abort()
*/
void* lwiperf_start_tcp_client(const ip_addr_t* remote_addr, u16_t remote_port,
enum lwiperf_client_type type, lwiperf_report_fn report_fn, void* report_arg)
{
err_t ret;
lwiperf_settings_t settings;
lwiperf_state_tcp_t *state = NULL;
memset(&settings, 0, sizeof(settings));
switch (type) {
case LWIPERF_CLIENT:
/* Unidirectional tx only test */
settings.flags = 0;
break;
case LWIPERF_DUAL:
/* Do a bidirectional test simultaneously */
settings.flags = htonl(LWIPERF_FLAGS_ANSWER_TEST | LWIPERF_FLAGS_ANSWER_NOW);
break;
case LWIPERF_TRADEOFF:
/* Do a bidirectional test individually */
settings.flags = htonl(LWIPERF_FLAGS_ANSWER_TEST);
break;
default:
/* invalid argument */
return NULL;
}
settings.num_threads = htonl(1);
settings.remote_port = htonl(LWIPERF_TCP_PORT_DEFAULT);
/* TODO: implement passing duration/amount of bytes to transfer */
settings.amount = htonl((u32_t)-1000);
ret = lwiperf_tx_start_impl(remote_addr, remote_port, &settings, report_fn, report_arg, NULL, &state);
if (ret == ERR_OK) {
LWIP_ASSERT("state != NULL", state != NULL);
if (type != LWIPERF_CLIENT) {
/* start corresponding server now */
lwiperf_state_tcp_t *server = NULL;
ret = lwiperf_start_tcp_server_impl(&state->conn_pcb->local_ip, LWIPERF_TCP_PORT_DEFAULT,
report_fn, report_arg, (lwiperf_state_base_t *)state, &server);
if (ret != ERR_OK) {
/* starting server failed, abort client */
lwiperf_abort(state);
return NULL;
}
/* make this server accept one connection only */
server->specific_remote = 1;
server->remote_addr = state->conn_pcb->remote_ip;
if (type == LWIPERF_TRADEOFF) {
/* tradeoff means that the remote host connects only after the client is done,
so keep the listen pcb open until the client is done */
server->client_tradeoff_mode = 1;
}
}
return state;
}
return NULL;
}
/**
* @ingroup iperf
* Abort an iperf session (handle returned by lwiperf_start_tcp_server*())
*/
void
lwiperf_abort(void *lwiperf_session)
{
lwiperf_state_base_t *i, *dealloc, *last = NULL;
LWIP_ASSERT_CORE_LOCKED();
for (i = lwiperf_all_connections; i != NULL; ) {
if ((i == lwiperf_session) || (i->related_master_state == lwiperf_session)) {
dealloc = i;
i = i->next;
if (last != NULL) {
last->next = i;
}
LWIPERF_FREE(lwiperf_state_tcp_t, dealloc); /* @todo: type? */
} else {
last = i;
i = i->next;
}
}
}
#endif /* LWIP_TCP && LWIP_CALLBACK_API */

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,533 @@
/**
* @file
* NetBIOS name service responder
*/
/**
* @defgroup netbiosns NETBIOS responder
* @ingroup apps
*
* This is an example implementation of a NetBIOS name server.
* It responds to name queries for a configurable name.
* Name resolving is not supported.
*
* Note that the device doesn't broadcast it's own name so can't
* detect duplicate names!
*/
/*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Modifications by Ray Abram to respond to NetBIOS name requests when Incoming name = *
* - based on code from "https://github.com/esp8266/Arduino/commit/1f7989b31d26d7df9776a08f36d685eae7ac8f99"
* - with permission to relicense to BSD from original author:
* http://www.xpablo.cz/?p=751#more-751
*/
#include "lwip/apps/netbiosns.h"
#if LWIP_IPV4 && LWIP_UDP /* don't build if not configured for use in lwipopts.h */
#include "lwip/def.h"
#include "lwip/udp.h"
#include "lwip/ip.h"
#include "lwip/netif.h"
#include "lwip/prot/iana.h"
#include <string.h>
/** size of a NetBIOS name */
#define NETBIOS_NAME_LEN 16
/** The Time-To-Live for NetBIOS name responds (in seconds)
* Default is 300000 seconds (3 days, 11 hours, 20 minutes) */
#define NETBIOS_NAME_TTL 300000u
/** NetBIOS header flags */
#define NETB_HFLAG_RESPONSE 0x8000U
#define NETB_HFLAG_OPCODE 0x7800U
#define NETB_HFLAG_OPCODE_NAME_QUERY 0x0000U
#define NETB_HFLAG_AUTHORATIVE 0x0400U
#define NETB_HFLAG_TRUNCATED 0x0200U
#define NETB_HFLAG_RECURS_DESIRED 0x0100U
#define NETB_HFLAG_RECURS_AVAILABLE 0x0080U
#define NETB_HFLAG_BROADCAST 0x0010U
#define NETB_HFLAG_REPLYCODE 0x0008U
#define NETB_HFLAG_REPLYCODE_NOERROR 0x0000U
/* NetBIOS question types */
#define NETB_QTYPE_NB 0x0020U
#define NETB_QTYPE_NBSTAT 0x0021U
/** NetBIOS name flags */
#define NETB_NFLAG_UNIQUE 0x8000U
#define NETB_NFLAG_NODETYPE 0x6000U
#define NETB_NFLAG_NODETYPE_HNODE 0x6000U
#define NETB_NFLAG_NODETYPE_MNODE 0x4000U
#define NETB_NFLAG_NODETYPE_PNODE 0x2000U
#define NETB_NFLAG_NODETYPE_BNODE 0x0000U
#define NETB_NFLAG_NAME_IN_CONFLICT 0x0800U /* 1=Yes, 0=No */
#define NETB_NFLAG_NAME_IS_ACTIVE 0x0400U /* 1=Yes, 0=No */
#define NETB_NFLAG_NAME_IS_PERMANENT 0x0200U /* 1=Yes (Name is Permanent Node Name), 0=No */
/** NetBIOS message header */
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/bpstruct.h"
#endif
PACK_STRUCT_BEGIN
struct netbios_hdr {
PACK_STRUCT_FIELD(u16_t trans_id);
PACK_STRUCT_FIELD(u16_t flags);
PACK_STRUCT_FIELD(u16_t questions);
PACK_STRUCT_FIELD(u16_t answerRRs);
PACK_STRUCT_FIELD(u16_t authorityRRs);
PACK_STRUCT_FIELD(u16_t additionalRRs);
} PACK_STRUCT_STRUCT;
PACK_STRUCT_END
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/epstruct.h"
#endif
/** NetBIOS message question part */
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/bpstruct.h"
#endif
PACK_STRUCT_BEGIN
struct netbios_question_hdr {
PACK_STRUCT_FLD_8(u8_t nametype);
PACK_STRUCT_FLD_8(u8_t encname[(NETBIOS_NAME_LEN * 2) + 1]);
PACK_STRUCT_FIELD(u16_t type);
PACK_STRUCT_FIELD(u16_t cls);
} PACK_STRUCT_STRUCT;
PACK_STRUCT_END
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/epstruct.h"
#endif
/** NetBIOS message name part */
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/bpstruct.h"
#endif
PACK_STRUCT_BEGIN
struct netbios_name_hdr {
PACK_STRUCT_FLD_8(u8_t nametype);
PACK_STRUCT_FLD_8(u8_t encname[(NETBIOS_NAME_LEN * 2) + 1]);
PACK_STRUCT_FIELD(u16_t type);
PACK_STRUCT_FIELD(u16_t cls);
PACK_STRUCT_FIELD(u32_t ttl);
PACK_STRUCT_FIELD(u16_t datalen);
PACK_STRUCT_FIELD(u16_t flags);
PACK_STRUCT_FLD_S(ip4_addr_p_t addr);
} PACK_STRUCT_STRUCT;
PACK_STRUCT_END
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/epstruct.h"
#endif
/** NetBIOS message */
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/bpstruct.h"
#endif
PACK_STRUCT_BEGIN
struct netbios_resp {
struct netbios_hdr resp_hdr;
struct netbios_name_hdr resp_name;
} PACK_STRUCT_STRUCT;
PACK_STRUCT_END
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/epstruct.h"
#endif
/** The NBNS Structure Responds to a Name Query */
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/bpstruct.h"
#endif
PACK_STRUCT_BEGIN
struct netbios_answer {
struct netbios_hdr answer_hdr;
/** the length of the next string */
PACK_STRUCT_FIELD(u8_t name_size);
/** WARNING!!! this item may be of a different length (we use this struct for transmission) */
PACK_STRUCT_FLD_8(u8_t query_name[(NETBIOS_NAME_LEN * 2) + 1]);
PACK_STRUCT_FIELD(u16_t packet_type);
PACK_STRUCT_FIELD(u16_t cls);
PACK_STRUCT_FIELD(u32_t ttl);
PACK_STRUCT_FIELD(u16_t data_length);
#define OFFSETOF_STRUCT_NETBIOS_ANSWER_NUMBER_OF_NAMES 56
/** number of names */
PACK_STRUCT_FLD_8(u8_t number_of_names);
/** node name */
PACK_STRUCT_FLD_8(u8_t answer_name[NETBIOS_NAME_LEN]);
/** node flags */
PACK_STRUCT_FIELD(u16_t answer_name_flags);
/** Unit ID */
PACK_STRUCT_FLD_8(u8_t unit_id[6]);
/** Jumpers */
PACK_STRUCT_FLD_8(u8_t jumpers);
/** Test result */
PACK_STRUCT_FLD_8(u8_t test_result);
/** Version number */
PACK_STRUCT_FIELD(u16_t version_number);
/** Period of statistics */
PACK_STRUCT_FIELD(u16_t period_of_statistics);
/** Statistics */
PACK_STRUCT_FIELD(u16_t number_of_crcs);
/** Statistics */
PACK_STRUCT_FIELD(u16_t number_of_alignment_errors);
/** Statistics */
PACK_STRUCT_FIELD(u16_t number_of_collisions);
/** Statistics */
PACK_STRUCT_FIELD(u16_t number_of_send_aborts);
/** Statistics */
PACK_STRUCT_FIELD(u32_t number_of_good_sends);
/** Statistics */
PACK_STRUCT_FIELD(u32_t number_of_good_receives);
/** Statistics */
PACK_STRUCT_FIELD(u16_t number_of_retransmits);
/** Statistics */
PACK_STRUCT_FIELD(u16_t number_of_no_resource_condition);
/** Statistics */
PACK_STRUCT_FIELD(u16_t number_of_free_command_blocks);
/** Statistics */
PACK_STRUCT_FIELD(u16_t total_number_of_command_blocks);
/** Statistics */
PACK_STRUCT_FIELD(u16_t max_total_number_of_command_blocks);
/** Statistics */
PACK_STRUCT_FIELD(u16_t number_of_pending_sessions);
/** Statistics */
PACK_STRUCT_FIELD(u16_t max_number_of_pending_sessions);
/** Statistics */
PACK_STRUCT_FIELD(u16_t max_total_sessions_possible);
/** Statistics */
PACK_STRUCT_FIELD(u16_t session_data_packet_size);
} PACK_STRUCT_STRUCT;
PACK_STRUCT_END
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/epstruct.h"
#endif
#ifdef NETBIOS_LWIP_NAME
#define NETBIOS_LOCAL_NAME NETBIOS_LWIP_NAME
#else
static char netbiosns_local_name[NETBIOS_NAME_LEN];
#define NETBIOS_LOCAL_NAME netbiosns_local_name
#endif
static struct udp_pcb *netbiosns_pcb;
/** Decode a NetBIOS name (from packet to string) */
static int
netbiosns_name_decode(char *name_enc, char *name_dec, int name_dec_len)
{
char *pname;
char cname;
char cnbname;
int idx = 0;
LWIP_UNUSED_ARG(name_dec_len);
/* Start decoding netbios name. */
pname = name_enc;
for (;;) {
/* Every two characters of the first level-encoded name
* turn into one character in the decoded name. */
cname = *pname;
if (cname == '\0') {
break; /* no more characters */
}
if (cname == '.') {
break; /* scope ID follows */
}
if (!lwip_isupper(cname)) {
/* Not legal. */
return -1;
}
cname -= 'A';
cnbname = cname << 4;
pname++;
cname = *pname;
if (!lwip_isupper(cname)) {
/* Not legal. */
return -1;
}
cname -= 'A';
cnbname |= cname;
pname++;
/* Do we have room to store the character? */
if (idx < NETBIOS_NAME_LEN) {
/* Yes - store the character. */
name_dec[idx++] = (cnbname != ' ' ? cnbname : '\0');
}
}
return 0;
}
#if 0 /* function currently unused */
/** Encode a NetBIOS name (from string to packet) - currently unused because
we don't ask for names. */
static int
netbiosns_name_encode(char *name_enc, char *name_dec, int name_dec_len)
{
char *pname;
char cname;
unsigned char ucname;
int idx = 0;
/* Start encoding netbios name. */
pname = name_enc;
for (;;) {
/* Every two characters of the first level-encoded name
* turn into one character in the decoded name. */
cname = *pname;
if (cname == '\0') {
break; /* no more characters */
}
if (cname == '.') {
break; /* scope ID follows */
}
if ((cname < 'A' || cname > 'Z') && (cname < '0' || cname > '9')) {
/* Not legal. */
return -1;
}
/* Do we have room to store the character? */
if (idx >= name_dec_len) {
return -1;
}
/* Yes - store the character. */
ucname = cname;
name_dec[idx++] = ('A' + ((ucname >> 4) & 0x0F));
name_dec[idx++] = ('A' + ( ucname & 0x0F));
pname++;
}
/* Fill with "space" coding */
for (; idx < name_dec_len - 1;) {
name_dec[idx++] = 'C';
name_dec[idx++] = 'A';
}
/* Terminate string */
name_dec[idx] = '\0';
return 0;
}
#endif /* 0 */
/** NetBIOS Name service recv callback */
static void
netbiosns_recv(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
{
LWIP_UNUSED_ARG(arg);
/* if packet is valid */
if (p != NULL) {
char netbios_name[NETBIOS_NAME_LEN + 1];
struct netbios_hdr *netbios_hdr = (struct netbios_hdr *)p->payload;
struct netbios_question_hdr *netbios_question_hdr = (struct netbios_question_hdr *)(netbios_hdr + 1);
/* is the packet long enough (we need the header in one piece) */
if (p->len < (sizeof(struct netbios_hdr) + sizeof(struct netbios_question_hdr))) {
/* packet too short */
pbuf_free(p);
return;
}
/* we only answer if we got a default interface */
if (netif_default != NULL) {
/* @todo: do we need to check answerRRs/authorityRRs/additionalRRs? */
/* if the packet is a NetBIOS name query question */
if (((netbios_hdr->flags & PP_NTOHS(NETB_HFLAG_OPCODE)) == PP_NTOHS(NETB_HFLAG_OPCODE_NAME_QUERY)) &&
((netbios_hdr->flags & PP_NTOHS(NETB_HFLAG_RESPONSE)) == 0) &&
(netbios_hdr->questions == PP_NTOHS(1))) {
/* decode the NetBIOS name */
netbiosns_name_decode((char *)(netbios_question_hdr->encname), netbios_name, sizeof(netbios_name));
/* check the request type */
if (netbios_question_hdr->type == PP_HTONS(NETB_QTYPE_NB)) {
/* if the packet is for us */
if (lwip_strnicmp(netbios_name, NETBIOS_LOCAL_NAME, sizeof(NETBIOS_LOCAL_NAME)) == 0) {
struct pbuf *q;
struct netbios_resp *resp;
q = pbuf_alloc(PBUF_TRANSPORT, sizeof(struct netbios_resp), PBUF_RAM);
if (q != NULL) {
resp = (struct netbios_resp *)q->payload;
/* prepare NetBIOS header response */
resp->resp_hdr.trans_id = netbios_hdr->trans_id;
resp->resp_hdr.flags = PP_HTONS(NETB_HFLAG_RESPONSE |
NETB_HFLAG_OPCODE_NAME_QUERY |
NETB_HFLAG_AUTHORATIVE |
NETB_HFLAG_RECURS_DESIRED);
resp->resp_hdr.questions = 0;
resp->resp_hdr.answerRRs = PP_HTONS(1);
resp->resp_hdr.authorityRRs = 0;
resp->resp_hdr.additionalRRs = 0;
/* prepare NetBIOS header datas */
MEMCPY( resp->resp_name.encname, netbios_question_hdr->encname, sizeof(netbios_question_hdr->encname));
resp->resp_name.nametype = netbios_question_hdr->nametype;
resp->resp_name.type = netbios_question_hdr->type;
resp->resp_name.cls = netbios_question_hdr->cls;
resp->resp_name.ttl = PP_HTONL(NETBIOS_NAME_TTL);
resp->resp_name.datalen = PP_HTONS(sizeof(resp->resp_name.flags) + sizeof(resp->resp_name.addr));
resp->resp_name.flags = PP_HTONS(NETB_NFLAG_NODETYPE_BNODE);
ip4_addr_copy(resp->resp_name.addr, *netif_ip4_addr(netif_default));
/* send the NetBIOS response */
udp_sendto(upcb, q, addr, port);
/* free the "reference" pbuf */
pbuf_free(q);
}
}
#if LWIP_NETBIOS_RESPOND_NAME_QUERY
} else if (netbios_question_hdr->type == PP_HTONS(NETB_QTYPE_NBSTAT)) {
/* if the packet is for us or general query */
if (!lwip_strnicmp(netbios_name, NETBIOS_LOCAL_NAME, sizeof(NETBIOS_LOCAL_NAME)) ||
!lwip_strnicmp(netbios_name, "*", sizeof(NETBIOS_LOCAL_NAME))) {
/* general query - ask for our IP address */
struct pbuf *q;
struct netbios_answer *resp;
q = pbuf_alloc(PBUF_TRANSPORT, sizeof(struct netbios_answer), PBUF_RAM);
if (q != NULL) {
/* buffer to which a response is compiled */
resp = (struct netbios_answer *) q->payload;
/* Init response to zero, especially the statistics fields */
memset(resp, 0, sizeof(*resp));
/* copy the query to the response ID */
resp->answer_hdr.trans_id = netbios_hdr->trans_id;
/* acknowledgment of termination */
resp->answer_hdr.flags = PP_HTONS(NETB_HFLAG_RESPONSE | NETB_HFLAG_OPCODE_NAME_QUERY | NETB_HFLAG_AUTHORATIVE);
/* resp->answer_hdr.questions = PP_HTONS(0); done by memset() */
/* serial number of the answer */
resp->answer_hdr.answerRRs = PP_HTONS(1);
/* resp->answer_hdr.authorityRRs = PP_HTONS(0); done by memset() */
/* resp->answer_hdr.additionalRRs = PP_HTONS(0); done by memset() */
/* we will copy the length of the station name */
resp->name_size = netbios_question_hdr->nametype;
/* we will copy the queried name */
MEMCPY(resp->query_name, netbios_question_hdr->encname, (NETBIOS_NAME_LEN * 2) + 1);
/* NBSTAT */
resp->packet_type = PP_HTONS(0x21);
/* Internet name */
resp->cls = PP_HTONS(1);
/* resp->ttl = PP_HTONL(0); done by memset() */
resp->data_length = PP_HTONS(sizeof(struct netbios_answer) - offsetof(struct netbios_answer, number_of_names));
resp->number_of_names = 1;
/* make windows see us as workstation, not as a server */
memset(resp->answer_name, 0x20, NETBIOS_NAME_LEN - 1);
/* strlen is checked to be < NETBIOS_NAME_LEN during initialization */
MEMCPY(resp->answer_name, NETBIOS_LOCAL_NAME, strlen(NETBIOS_LOCAL_NAME));
/* b-node, unique, active */
resp->answer_name_flags = PP_HTONS(NETB_NFLAG_NAME_IS_ACTIVE);
/* Set responder netif MAC address */
SMEMCPY(resp->unit_id, ip_current_input_netif()->hwaddr, sizeof(resp->unit_id));
udp_sendto(upcb, q, addr, port);
pbuf_free(q);
}
}
#endif /* LWIP_NETBIOS_RESPOND_NAME_QUERY */
}
}
}
/* free the pbuf */
pbuf_free(p);
}
}
/**
* @ingroup netbiosns
* Init netbios responder
*/
void
netbiosns_init(void)
{
/* LWIP_ASSERT_CORE_LOCKED(); is checked by udp_new() */
#ifdef NETBIOS_LWIP_NAME
LWIP_ASSERT("NetBIOS name is too long!", strlen(NETBIOS_LWIP_NAME) < NETBIOS_NAME_LEN);
#endif
netbiosns_pcb = udp_new_ip_type(IPADDR_TYPE_ANY);
if (netbiosns_pcb != NULL) {
/* we have to be allowed to send broadcast packets! */
ip_set_option(netbiosns_pcb, SOF_BROADCAST);
udp_bind(netbiosns_pcb, IP_ANY_TYPE, LWIP_IANA_PORT_NETBIOS);
udp_recv(netbiosns_pcb, netbiosns_recv, netbiosns_pcb);
}
}
#ifndef NETBIOS_LWIP_NAME
/**
* @ingroup netbiosns
* Set netbios name. ATTENTION: the hostname must be less than 15 characters!
* the NetBIOS name spec says the name MUST be upper case, so incoming name is forced into uppercase :-)
*/
void
netbiosns_set_name(const char *hostname)
{
size_t i;
size_t copy_len = strlen(hostname);
LWIP_ASSERT_CORE_LOCKED();
LWIP_ASSERT("NetBIOS name is too long!", copy_len < NETBIOS_NAME_LEN);
if (copy_len >= NETBIOS_NAME_LEN) {
copy_len = NETBIOS_NAME_LEN - 1;
}
/* make name into upper case */
for (i = 0; i < copy_len; i++ ) {
netbiosns_local_name[i] = (char)lwip_toupper(hostname[i]);
}
netbiosns_local_name[copy_len] = '\0';
}
#endif /* NETBIOS_LWIP_NAME */
/**
* @ingroup netbiosns
* Stop netbios responder
*/
void
netbiosns_stop(void)
{
LWIP_ASSERT_CORE_LOCKED();
if (netbiosns_pcb != NULL) {
udp_remove(netbiosns_pcb);
netbiosns_pcb = NULL;
}
}
#endif /* LWIP_IPV4 && LWIP_UDP */

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,704 @@
/**
* @file
* Abstract Syntax Notation One (ISO 8824, 8825) encoding
*
* @todo not optimised (yet), favor correctness over speed, favor speed over size
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Christiaan Simons <christiaan.simons@axon.tv>
* Martin Hentschel <info@cl-soft.de>
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
#include "snmp_asn1.h"
#define PBUF_OP_EXEC(code) \
if ((code) != ERR_OK) { \
return ERR_BUF; \
}
/**
* Encodes a TLV into a pbuf stream.
*
* @param pbuf_stream points to a pbuf stream
* @param tlv TLV to encode
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
*/
err_t
snmp_ans1_enc_tlv(struct snmp_pbuf_stream *pbuf_stream, struct snmp_asn1_tlv *tlv)
{
u8_t data;
u8_t length_bytes_required;
/* write type */
if ((tlv->type & SNMP_ASN1_DATATYPE_MASK) == SNMP_ASN1_DATATYPE_EXTENDED) {
/* extended format is not used by SNMP so we do not accept those values */
return ERR_ARG;
}
if (tlv->type_len != 0) {
/* any other value as auto is not accepted for type (we always use one byte because extended syntax is prohibited) */
return ERR_ARG;
}
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, tlv->type));
tlv->type_len = 1;
/* write length */
if (tlv->value_len <= 127) {
length_bytes_required = 1;
} else if (tlv->value_len <= 255) {
length_bytes_required = 2;
} else {
length_bytes_required = 3;
}
/* check for forced min length */
if (tlv->length_len > 0) {
if (tlv->length_len < length_bytes_required) {
/* unable to code requested length in requested number of bytes */
return ERR_ARG;
}
length_bytes_required = tlv->length_len;
} else {
tlv->length_len = length_bytes_required;
}
if (length_bytes_required > 1) {
/* multi byte representation required */
length_bytes_required--;
data = 0x80 | length_bytes_required; /* extended length definition, 1 length byte follows */
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, data));
while (length_bytes_required > 1) {
if (length_bytes_required == 2) {
/* append high byte */
data = (u8_t)(tlv->value_len >> 8);
} else {
/* append leading 0x00 */
data = 0x00;
}
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, data));
length_bytes_required--;
}
}
/* append low byte */
data = (u8_t)(tlv->value_len & 0xFF);
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, data));
return ERR_OK;
}
/**
* Encodes raw data (octet string, opaque) into a pbuf chained ASN1 msg.
*
* @param pbuf_stream points to a pbuf stream
* @param raw_len raw data length
* @param raw points raw data
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
*/
err_t
snmp_asn1_enc_raw(struct snmp_pbuf_stream *pbuf_stream, const u8_t *raw, u16_t raw_len)
{
PBUF_OP_EXEC(snmp_pbuf_stream_writebuf(pbuf_stream, raw, raw_len));
return ERR_OK;
}
/**
* Encodes u32_t (counter, gauge, timeticks) into a pbuf chained ASN1 msg.
*
* @param pbuf_stream points to a pbuf stream
* @param octets_needed encoding length (from snmp_asn1_enc_u32t_cnt())
* @param value is the host order u32_t value to be encoded
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
*
* @see snmp_asn1_enc_u32t_cnt()
*/
err_t
snmp_asn1_enc_u32t(struct snmp_pbuf_stream *pbuf_stream, u16_t octets_needed, u32_t value)
{
if (octets_needed > 5) {
return ERR_ARG;
}
if (octets_needed == 5) {
/* not enough bits in 'value' add leading 0x00 */
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, 0x00));
octets_needed--;
}
while (octets_needed > 1) {
octets_needed--;
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(value >> (octets_needed << 3))));
}
/* (only) one least significant octet */
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)value));
return ERR_OK;
}
/**
* Encodes s32_t integer into a pbuf chained ASN1 msg.
*
* @param pbuf_stream points to a pbuf stream
* @param octets_needed encoding length (from snmp_asn1_enc_s32t_cnt())
* @param value is the host order s32_t value to be encoded
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
*
* @see snmp_asn1_enc_s32t_cnt()
*/
err_t
snmp_asn1_enc_s32t(struct snmp_pbuf_stream *pbuf_stream, u16_t octets_needed, s32_t value)
{
while (octets_needed > 1) {
octets_needed--;
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(value >> (octets_needed << 3))));
}
/* (only) one least significant octet */
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)value));
return ERR_OK;
}
/**
* Encodes object identifier into a pbuf chained ASN1 msg.
*
* @param pbuf_stream points to a pbuf stream
* @param oid points to object identifier array
* @param oid_len object identifier array length
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
*/
err_t
snmp_asn1_enc_oid(struct snmp_pbuf_stream *pbuf_stream, const u32_t *oid, u16_t oid_len)
{
if (oid_len > 1) {
/* write compressed first two sub id's */
u32_t compressed_byte = ((oid[0] * 40) + oid[1]);
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)compressed_byte));
oid_len -= 2;
oid += 2;
} else {
/* @bug: allow empty varbinds for symmetry (we must decode them for getnext), allow partial compression?? */
/* ident_len <= 1, at least we need zeroDotZero (0.0) (ident_len == 2) */
return ERR_ARG;
}
while (oid_len > 0) {
u32_t sub_id;
u8_t shift, tail;
oid_len--;
sub_id = *oid;
tail = 0;
shift = 28;
while (shift > 0) {
u8_t code;
code = (u8_t)(sub_id >> shift);
if ((code != 0) || (tail != 0)) {
tail = 1;
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, code | 0x80));
}
shift -= 7;
}
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)sub_id & 0x7F));
/* proceed to next sub-identifier */
oid++;
}
return ERR_OK;
}
/**
* Returns octet count for length.
*
* @param length parameter length
* @param octets_needed points to the return value
*/
void
snmp_asn1_enc_length_cnt(u16_t length, u8_t *octets_needed)
{
if (length < 0x80U) {
*octets_needed = 1;
} else if (length < 0x100U) {
*octets_needed = 2;
} else {
*octets_needed = 3;
}
}
/**
* Returns octet count for an u32_t.
*
* @param value value to be encoded
* @param octets_needed points to the return value
*
* @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
* as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
* of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
*/
void
snmp_asn1_enc_u32t_cnt(u32_t value, u16_t *octets_needed)
{
if (value < 0x80UL) {
*octets_needed = 1;
} else if (value < 0x8000UL) {
*octets_needed = 2;
} else if (value < 0x800000UL) {
*octets_needed = 3;
} else if (value < 0x80000000UL) {
*octets_needed = 4;
} else {
*octets_needed = 5;
}
}
/**
* Returns octet count for an s32_t.
*
* @param value value to be encoded
* @param octets_needed points to the return value
*
* @note ASN coded integers are _always_ signed.
*/
void
snmp_asn1_enc_s32t_cnt(s32_t value, u16_t *octets_needed)
{
if (value < 0) {
value = ~value;
}
if (value < 0x80L) {
*octets_needed = 1;
} else if (value < 0x8000L) {
*octets_needed = 2;
} else if (value < 0x800000L) {
*octets_needed = 3;
} else {
*octets_needed = 4;
}
}
/**
* Returns octet count for an object identifier.
*
* @param oid points to object identifier array
* @param oid_len object identifier array length
* @param octets_needed points to the return value
*/
void
snmp_asn1_enc_oid_cnt(const u32_t *oid, u16_t oid_len, u16_t *octets_needed)
{
u32_t sub_id;
*octets_needed = 0;
if (oid_len > 1) {
/* compressed prefix in one octet */
(*octets_needed)++;
oid_len -= 2;
oid += 2;
}
while (oid_len > 0) {
oid_len--;
sub_id = *oid;
sub_id >>= 7;
(*octets_needed)++;
while (sub_id > 0) {
sub_id >>= 7;
(*octets_needed)++;
}
oid++;
}
}
/**
* Decodes a TLV from a pbuf stream.
*
* @param pbuf_stream points to a pbuf stream
* @param tlv returns decoded TLV
* @return ERR_OK if successful, ERR_VAL if we can't decode
*/
err_t
snmp_asn1_dec_tlv(struct snmp_pbuf_stream *pbuf_stream, struct snmp_asn1_tlv *tlv)
{
u8_t data;
/* decode type first */
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
tlv->type = data;
if ((tlv->type & SNMP_ASN1_DATATYPE_MASK) == SNMP_ASN1_DATATYPE_EXTENDED) {
/* extended format is not used by SNMP so we do not accept those values */
return ERR_VAL;
}
tlv->type_len = 1;
/* now, decode length */
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
if (data < 0x80) { /* short form */
tlv->length_len = 1;
tlv->value_len = data;
} else if (data > 0x80) { /* long form */
u8_t length_bytes = data - 0x80;
if (length_bytes > pbuf_stream->length) {
return ERR_VAL;
}
tlv->length_len = length_bytes + 1; /* this byte + defined number of length bytes following */
tlv->value_len = 0;
while (length_bytes > 0) {
/* we only support up to u16.maxvalue-1 (2 bytes) but have to accept leading zero bytes */
if (tlv->value_len > 0xFF) {
return ERR_VAL;
}
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
tlv->value_len <<= 8;
tlv->value_len |= data;
/* take care for special value used for indefinite length */
if (tlv->value_len == 0xFFFF) {
return ERR_VAL;
}
length_bytes--;
}
} else { /* data == 0x80 indefinite length form */
/* (not allowed for SNMP; RFC 1157, 3.2.2) */
return ERR_VAL;
}
return ERR_OK;
}
/**
* Decodes positive integer (counter, gauge, timeticks) into u32_t.
*
* @param pbuf_stream points to a pbuf stream
* @param len length of the coded integer field
* @param value return host order integer
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
*
* @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
* as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
* of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
*/
err_t
snmp_asn1_dec_u32t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u32_t *value)
{
u8_t data;
if ((len > 0) && (len <= 5)) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
/* expecting sign bit to be zero, only unsigned please! */
if (((len == 5) && (data == 0x00)) || ((len < 5) && ((data & 0x80) == 0))) {
*value = data;
len--;
while (len > 0) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
len--;
*value <<= 8;
*value |= data;
}
return ERR_OK;
}
}
return ERR_VAL;
}
/**
* Decodes integer into s32_t.
*
* @param pbuf_stream points to a pbuf stream
* @param len length of the coded integer field
* @param value return host order integer
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
*
* @note ASN coded integers are _always_ signed!
*/
err_t
snmp_asn1_dec_s32t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, s32_t *value)
{
u8_t data;
if ((len > 0) && (len < 5)) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
if (data & 0x80) {
/* negative, start from -1 */
*value = -1;
*value = (*value << 8) | data;
} else {
/* positive, start from 0 */
*value = data;
}
len--;
/* shift in the remaining value */
while (len > 0) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
*value = (*value << 8) | data;
len--;
}
return ERR_OK;
}
return ERR_VAL;
}
/**
* Decodes object identifier from incoming message into array of u32_t.
*
* @param pbuf_stream points to a pbuf stream
* @param len length of the coded object identifier
* @param oid return decoded object identifier
* @param oid_len return decoded object identifier length
* @param oid_max_len size of oid buffer
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
*/
err_t
snmp_asn1_dec_oid(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u32_t *oid, u8_t *oid_len, u8_t oid_max_len)
{
u32_t *oid_ptr;
u8_t data;
*oid_len = 0;
oid_ptr = oid;
if (len > 0) {
if (oid_max_len < 2) {
return ERR_MEM;
}
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
len--;
/* first compressed octet */
if (data == 0x2B) {
/* (most) common case 1.3 (iso.org) */
*oid_ptr = 1;
oid_ptr++;
*oid_ptr = 3;
oid_ptr++;
} else if (data < 40) {
*oid_ptr = 0;
oid_ptr++;
*oid_ptr = data;
oid_ptr++;
} else if (data < 80) {
*oid_ptr = 1;
oid_ptr++;
*oid_ptr = data - 40;
oid_ptr++;
} else {
*oid_ptr = 2;
oid_ptr++;
*oid_ptr = data - 80;
oid_ptr++;
}
*oid_len = 2;
} else {
/* accepting zero length identifiers e.g. for getnext operation. uncommon but valid */
return ERR_OK;
}
while ((len > 0) && (*oid_len < oid_max_len)) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
len--;
if ((data & 0x80) == 0x00) {
/* sub-identifier uses single octet */
*oid_ptr = data;
} else {
/* sub-identifier uses multiple octets */
u32_t sub_id = (data & ~0x80);
while ((len > 0) && ((data & 0x80) != 0)) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
len--;
sub_id = (sub_id << 7) + (data & ~0x80);
}
if ((data & 0x80) != 0) {
/* "more bytes following" bit still set at end of len */
return ERR_VAL;
}
*oid_ptr = sub_id;
}
oid_ptr++;
(*oid_len)++;
}
if (len > 0) {
/* OID to long to fit in our buffer */
return ERR_MEM;
}
return ERR_OK;
}
/**
* Decodes (copies) raw data (ip-addresses, octet strings, opaque encoding)
* from incoming message into array.
*
* @param pbuf_stream points to a pbuf stream
* @param len length of the coded raw data (zero is valid, e.g. empty string!)
* @param buf return raw bytes
* @param buf_len returns length of the raw return value
* @param buf_max_len buffer size
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
*/
err_t
snmp_asn1_dec_raw(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u8_t *buf, u16_t *buf_len, u16_t buf_max_len)
{
if (len > buf_max_len) {
/* not enough dst space */
return ERR_MEM;
}
*buf_len = len;
while (len > 0) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, buf));
buf++;
len--;
}
return ERR_OK;
}
#if LWIP_HAVE_INT64
/**
* Returns octet count for an u64_t.
*
* @param value value to be encoded
* @param octets_needed points to the return value
*
* @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
* as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
* of 0xFFFFFFFFFFFFFFFF is preceded with 0x00 and the length is 9 octets!!
*/
void
snmp_asn1_enc_u64t_cnt(u64_t value, u16_t *octets_needed)
{
/* check if high u32 is 0 */
if ((value >> 32) == 0) {
/* only low u32 is important */
snmp_asn1_enc_u32t_cnt((u32_t)value, octets_needed);
} else {
/* low u32 does not matter for length determination */
snmp_asn1_enc_u32t_cnt((u32_t)(value >> 32), octets_needed);
*octets_needed = *octets_needed + 4; /* add the 4 bytes of low u32 */
}
}
/**
* Decodes large positive integer (counter64) into 2x u32_t.
*
* @param pbuf_stream points to a pbuf stream
* @param len length of the coded integer field
* @param value return 64 bit integer
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
*
* @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
* as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
* of 0xFFFFFFFFFFFFFFFF is preceded with 0x00 and the length is 9 octets!!
*/
err_t
snmp_asn1_dec_u64t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u64_t *value)
{
u8_t data;
if ((len > 0) && (len <= 9)) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
/* expecting sign bit to be zero, only unsigned please! */
if (((len == 9) && (data == 0x00)) || ((len < 9) && ((data & 0x80) == 0))) {
*value = data;
len--;
while (len > 0) {
PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
*value <<= 8;
*value |= data;
len--;
}
return ERR_OK;
}
}
return ERR_VAL;
}
/**
* Encodes u64_t (counter64) into a pbuf chained ASN1 msg.
*
* @param pbuf_stream points to a pbuf stream
* @param octets_needed encoding length (from snmp_asn1_enc_u32t_cnt())
* @param value is the value to be encoded
* @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
*
* @see snmp_asn1_enc_u64t_cnt()
*/
err_t
snmp_asn1_enc_u64t(struct snmp_pbuf_stream *pbuf_stream, u16_t octets_needed, u64_t value)
{
if (octets_needed > 9) {
return ERR_ARG;
}
if (octets_needed == 9) {
/* not enough bits in 'value' add leading 0x00 */
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, 0x00));
octets_needed--;
}
while (octets_needed > 1) {
octets_needed--;
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(value >> (octets_needed << 3))));
}
/* always write at least one octet (also in case of value == 0) */
PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(value)));
return ERR_OK;
}
#endif
#endif /* LWIP_SNMP */

View File

@ -0,0 +1,113 @@
/**
* @file
* Abstract Syntax Notation One (ISO 8824, 8825) codec.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* Copyright (c) 2016 Elias Oenal.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Christiaan Simons <christiaan.simons@axon.tv>
* Martin Hentschel <info@cl-soft.de>
* Elias Oenal <lwip@eliasoenal.com>
*/
#ifndef LWIP_HDR_APPS_SNMP_ASN1_H
#define LWIP_HDR_APPS_SNMP_ASN1_H
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP
#include "lwip/err.h"
#include "lwip/apps/snmp_core.h"
#include "snmp_pbuf_stream.h"
#ifdef __cplusplus
extern "C" {
#endif
#define SNMP_ASN1_TLV_INDEFINITE_LENGTH 0x80
#define SNMP_ASN1_CLASS_MASK 0xC0
#define SNMP_ASN1_CONTENTTYPE_MASK 0x20
#define SNMP_ASN1_DATATYPE_MASK 0x1F
#define SNMP_ASN1_DATATYPE_EXTENDED 0x1F /* DataType indicating that datatype is encoded in following bytes */
/* context specific (SNMP) tags (from SNMP spec. RFC1157 and RFC1905) */
#define SNMP_ASN1_CONTEXT_PDU_GET_REQ 0
#define SNMP_ASN1_CONTEXT_PDU_GET_NEXT_REQ 1
#define SNMP_ASN1_CONTEXT_PDU_GET_RESP 2
#define SNMP_ASN1_CONTEXT_PDU_SET_REQ 3
#define SNMP_ASN1_CONTEXT_PDU_TRAP 4
#define SNMP_ASN1_CONTEXT_PDU_GET_BULK_REQ 5
#define SNMP_ASN1_CONTEXT_PDU_INFORM_REQ 6
#define SNMP_ASN1_CONTEXT_PDU_V2_TRAP 7
#define SNMP_ASN1_CONTEXT_PDU_REPORT 8
#define SNMP_ASN1_CONTEXT_VARBIND_NO_SUCH_OBJECT 0
#define SNMP_ASN1_CONTEXT_VARBIND_END_OF_MIB_VIEW 2
struct snmp_asn1_tlv {
u8_t type; /* only U8 because extended types are not specified by SNMP */
u8_t type_len; /* encoded length of 'type' field (normally 1) */
u8_t length_len; /* indicates how many bytes are required to encode the 'value_len' field */
u16_t value_len; /* encoded length of the value */
};
#define SNMP_ASN1_TLV_HDR_LENGTH(tlv) ((tlv).type_len + (tlv).length_len)
#define SNMP_ASN1_TLV_LENGTH(tlv) ((tlv).type_len + (tlv).length_len + (tlv).value_len)
#define SNMP_ASN1_SET_TLV_PARAMS(tlv, type_, length_len_, value_len_) do { (tlv).type = (type_); (tlv).type_len = 0; (tlv).length_len = (length_len_); (tlv).value_len = (value_len_); } while (0);
err_t snmp_asn1_dec_tlv(struct snmp_pbuf_stream *pbuf_stream, struct snmp_asn1_tlv *tlv);
err_t snmp_asn1_dec_u32t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u32_t *value);
err_t snmp_asn1_dec_s32t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, s32_t *value);
err_t snmp_asn1_dec_oid(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u32_t *oid, u8_t *oid_len, u8_t oid_max_len);
err_t snmp_asn1_dec_raw(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u8_t *buf, u16_t *buf_len, u16_t buf_max_len);
err_t snmp_ans1_enc_tlv(struct snmp_pbuf_stream *pbuf_stream, struct snmp_asn1_tlv *tlv);
void snmp_asn1_enc_length_cnt(u16_t length, u8_t *octets_needed);
void snmp_asn1_enc_u32t_cnt(u32_t value, u16_t *octets_needed);
void snmp_asn1_enc_s32t_cnt(s32_t value, u16_t *octets_needed);
void snmp_asn1_enc_oid_cnt(const u32_t *oid, u16_t oid_len, u16_t *octets_needed);
err_t snmp_asn1_enc_oid(struct snmp_pbuf_stream *pbuf_stream, const u32_t *oid, u16_t oid_len);
err_t snmp_asn1_enc_s32t(struct snmp_pbuf_stream *pbuf_stream, u16_t octets_needed, s32_t value);
err_t snmp_asn1_enc_u32t(struct snmp_pbuf_stream *pbuf_stream, u16_t octets_needed, u32_t value);
err_t snmp_asn1_enc_raw(struct snmp_pbuf_stream *pbuf_stream, const u8_t *raw, u16_t raw_len);
#if LWIP_HAVE_INT64
err_t snmp_asn1_dec_u64t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u64_t *value);
void snmp_asn1_enc_u64t_cnt(u64_t value, u16_t *octets_needed);
err_t snmp_asn1_enc_u64t(struct snmp_pbuf_stream *pbuf_stream, u16_t octets_needed, u64_t value);
#endif
#ifdef __cplusplus
}
#endif
#endif /* LWIP_SNMP */
#endif /* LWIP_HDR_APPS_SNMP_ASN1_H */

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,83 @@
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Martin Hentschel <info@cl-soft.de>
*
*/
#ifndef LWIP_HDR_APPS_SNMP_CORE_PRIV_H
#define LWIP_HDR_APPS_SNMP_CORE_PRIV_H
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
#include "lwip/apps/snmp_core.h"
#include "snmp_asn1.h"
#ifdef __cplusplus
extern "C" {
#endif
/* (outdated) SNMPv1 error codes
* shall not be used by MIBS anymore, nevertheless required from core for properly answering a v1 request
*/
#define SNMP_ERR_NOSUCHNAME 2
#define SNMP_ERR_BADVALUE 3
#define SNMP_ERR_READONLY 4
/* error codes which are internal and shall not be used by MIBS
* shall not be used by MIBS anymore, nevertheless required from core for properly answering a v1 request
*/
#define SNMP_ERR_TOOBIG 1
#define SNMP_ERR_AUTHORIZATIONERROR 16
#define SNMP_ERR_UNKNOWN_ENGINEID 30
#define SNMP_ERR_UNKNOWN_SECURITYNAME 31
#define SNMP_ERR_UNSUPPORTED_SECLEVEL 32
#define SNMP_ERR_NOTINTIMEWINDOW 33
#define SNMP_ERR_DECRYIPTION_ERROR 34
#define SNMP_ERR_NOSUCHOBJECT SNMP_VARBIND_EXCEPTION_OFFSET + SNMP_ASN1_CONTEXT_VARBIND_NO_SUCH_OBJECT
#define SNMP_ERR_ENDOFMIBVIEW SNMP_VARBIND_EXCEPTION_OFFSET + SNMP_ASN1_CONTEXT_VARBIND_END_OF_MIB_VIEW
const struct snmp_node *snmp_mib_tree_resolve_exact(const struct snmp_mib *mib, const u32_t *oid, u8_t oid_len, u8_t *oid_instance_len);
const struct snmp_node *snmp_mib_tree_resolve_next(const struct snmp_mib *mib, const u32_t *oid, u8_t oid_len, struct snmp_obj_id *oidret);
typedef u8_t (*snmp_validate_node_instance_method)(struct snmp_node_instance *, void *);
u8_t snmp_get_node_instance_from_oid(const u32_t *oid, u8_t oid_len, struct snmp_node_instance *node_instance);
u8_t snmp_get_next_node_instance_from_oid(const u32_t *oid, u8_t oid_len, snmp_validate_node_instance_method validate_node_instance_method, void *validate_node_instance_arg, struct snmp_obj_id *node_oid, struct snmp_node_instance *node_instance);
#ifdef __cplusplus
}
#endif
#endif /* LWIP_SNMP */
#endif /* LWIP_HDR_APPS_SNMP_CORE_PRIV_H */

View File

@ -0,0 +1,116 @@
/**
* @file
* Management Information Base II (RFC1213) objects and functions.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
* Christiaan Simons <christiaan.simons@axon.tv>
*/
/**
* @defgroup snmp_mib2 MIB2
* @ingroup snmp
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP && SNMP_LWIP_MIB2 /* don't build if not configured for use in lwipopts.h */
#if !LWIP_STATS
#error LWIP_SNMP MIB2 needs LWIP_STATS (for MIB2)
#endif
#if !MIB2_STATS
#error LWIP_SNMP MIB2 needs MIB2_STATS (for MIB2)
#endif
#include "lwip/snmp.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_mib2.h"
#include "lwip/apps/snmp_scalar.h"
#if SNMP_USE_NETCONN
#include "lwip/tcpip.h"
#include "lwip/priv/tcpip_priv.h"
void
snmp_mib2_lwip_synchronizer(snmp_threadsync_called_fn fn, void *arg)
{
#if LWIP_TCPIP_CORE_LOCKING
LOCK_TCPIP_CORE();
fn(arg);
UNLOCK_TCPIP_CORE();
#else
tcpip_callback(fn, arg);
#endif
}
struct snmp_threadsync_instance snmp_mib2_lwip_locks;
#endif
/* dot3 and EtherLike MIB not planned. (transmission .1.3.6.1.2.1.10) */
/* historical (some say hysterical). (cmot .1.3.6.1.2.1.9) */
/* lwIP has no EGP, thus may not implement it. (egp .1.3.6.1.2.1.8) */
/* --- mib-2 .1.3.6.1.2.1 ----------------------------------------------------- */
extern const struct snmp_scalar_array_node snmp_mib2_snmp_root;
extern const struct snmp_tree_node snmp_mib2_udp_root;
extern const struct snmp_tree_node snmp_mib2_tcp_root;
extern const struct snmp_scalar_array_node snmp_mib2_icmp_root;
extern const struct snmp_tree_node snmp_mib2_interface_root;
extern const struct snmp_scalar_array_node snmp_mib2_system_node;
extern const struct snmp_tree_node snmp_mib2_at_root;
extern const struct snmp_tree_node snmp_mib2_ip_root;
static const struct snmp_node *const mib2_nodes[] = {
&snmp_mib2_system_node.node.node,
&snmp_mib2_interface_root.node,
#if LWIP_ARP && LWIP_IPV4
&snmp_mib2_at_root.node,
#endif /* LWIP_ARP && LWIP_IPV4 */
#if LWIP_IPV4
&snmp_mib2_ip_root.node,
#endif /* LWIP_IPV4 */
#if LWIP_ICMP
&snmp_mib2_icmp_root.node.node,
#endif /* LWIP_ICMP */
#if LWIP_TCP
&snmp_mib2_tcp_root.node,
#endif /* LWIP_TCP */
#if LWIP_UDP
&snmp_mib2_udp_root.node,
#endif /* LWIP_UDP */
&snmp_mib2_snmp_root.node.node
};
static const struct snmp_tree_node mib2_root = SNMP_CREATE_TREE_NODE(1, mib2_nodes);
static const u32_t mib2_base_oid_arr[] = { 1, 3, 6, 1, 2, 1 };
const struct snmp_mib mib2 = SNMP_MIB_CREATE(mib2_base_oid_arr, &mib2_root.node);
#endif /* LWIP_SNMP && SNMP_LWIP_MIB2 */

View File

@ -0,0 +1,182 @@
/**
* @file
* Management Information Base II (RFC1213) ICMP objects and functions.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
* Christiaan Simons <christiaan.simons@axon.tv>
*/
#include "lwip/snmp.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_mib2.h"
#include "lwip/apps/snmp_table.h"
#include "lwip/apps/snmp_scalar.h"
#include "lwip/icmp.h"
#include "lwip/stats.h"
#if LWIP_SNMP && SNMP_LWIP_MIB2 && LWIP_ICMP
#if SNMP_USE_NETCONN
#define SYNC_NODE_NAME(node_name) node_name ## _synced
#define CREATE_LWIP_SYNC_NODE(oid, node_name) \
static const struct snmp_threadsync_node node_name ## _synced = SNMP_CREATE_THREAD_SYNC_NODE(oid, &node_name.node, &snmp_mib2_lwip_locks);
#else
#define SYNC_NODE_NAME(node_name) node_name
#define CREATE_LWIP_SYNC_NODE(oid, node_name)
#endif
/* --- icmp .1.3.6.1.2.1.5 ----------------------------------------------------- */
static s16_t
icmp_get_value(const struct snmp_scalar_array_node_def *node, void *value)
{
u32_t *uint_ptr = (u32_t *)value;
switch (node->oid) {
case 1: /* icmpInMsgs */
*uint_ptr = STATS_GET(mib2.icmpinmsgs);
return sizeof(*uint_ptr);
case 2: /* icmpInErrors */
*uint_ptr = STATS_GET(mib2.icmpinerrors);
return sizeof(*uint_ptr);
case 3: /* icmpInDestUnreachs */
*uint_ptr = STATS_GET(mib2.icmpindestunreachs);
return sizeof(*uint_ptr);
case 4: /* icmpInTimeExcds */
*uint_ptr = STATS_GET(mib2.icmpintimeexcds);
return sizeof(*uint_ptr);
case 5: /* icmpInParmProbs */
*uint_ptr = STATS_GET(mib2.icmpinparmprobs);
return sizeof(*uint_ptr);
case 6: /* icmpInSrcQuenchs */
*uint_ptr = STATS_GET(mib2.icmpinsrcquenchs);
return sizeof(*uint_ptr);
case 7: /* icmpInRedirects */
*uint_ptr = STATS_GET(mib2.icmpinredirects);
return sizeof(*uint_ptr);
case 8: /* icmpInEchos */
*uint_ptr = STATS_GET(mib2.icmpinechos);
return sizeof(*uint_ptr);
case 9: /* icmpInEchoReps */
*uint_ptr = STATS_GET(mib2.icmpinechoreps);
return sizeof(*uint_ptr);
case 10: /* icmpInTimestamps */
*uint_ptr = STATS_GET(mib2.icmpintimestamps);
return sizeof(*uint_ptr);
case 11: /* icmpInTimestampReps */
*uint_ptr = STATS_GET(mib2.icmpintimestampreps);
return sizeof(*uint_ptr);
case 12: /* icmpInAddrMasks */
*uint_ptr = STATS_GET(mib2.icmpinaddrmasks);
return sizeof(*uint_ptr);
case 13: /* icmpInAddrMaskReps */
*uint_ptr = STATS_GET(mib2.icmpinaddrmaskreps);
return sizeof(*uint_ptr);
case 14: /* icmpOutMsgs */
*uint_ptr = STATS_GET(mib2.icmpoutmsgs);
return sizeof(*uint_ptr);
case 15: /* icmpOutErrors */
*uint_ptr = STATS_GET(mib2.icmpouterrors);
return sizeof(*uint_ptr);
case 16: /* icmpOutDestUnreachs */
*uint_ptr = STATS_GET(mib2.icmpoutdestunreachs);
return sizeof(*uint_ptr);
case 17: /* icmpOutTimeExcds */
*uint_ptr = STATS_GET(mib2.icmpouttimeexcds);
return sizeof(*uint_ptr);
case 18: /* icmpOutParmProbs: not supported -> always 0 */
*uint_ptr = 0;
return sizeof(*uint_ptr);
case 19: /* icmpOutSrcQuenchs: not supported -> always 0 */
*uint_ptr = 0;
return sizeof(*uint_ptr);
case 20: /* icmpOutRedirects: not supported -> always 0 */
*uint_ptr = 0;
return sizeof(*uint_ptr);
case 21: /* icmpOutEchos */
*uint_ptr = STATS_GET(mib2.icmpoutechos);
return sizeof(*uint_ptr);
case 22: /* icmpOutEchoReps */
*uint_ptr = STATS_GET(mib2.icmpoutechoreps);
return sizeof(*uint_ptr);
case 23: /* icmpOutTimestamps: not supported -> always 0 */
*uint_ptr = 0;
return sizeof(*uint_ptr);
case 24: /* icmpOutTimestampReps: not supported -> always 0 */
*uint_ptr = 0;
return sizeof(*uint_ptr);
case 25: /* icmpOutAddrMasks: not supported -> always 0 */
*uint_ptr = 0;
return sizeof(*uint_ptr);
case 26: /* icmpOutAddrMaskReps: not supported -> always 0 */
*uint_ptr = 0;
return sizeof(*uint_ptr);
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("icmp_get_value(): unknown id: %"S32_F"\n", node->oid));
break;
}
return 0;
}
static const struct snmp_scalar_array_node_def icmp_nodes[] = {
{ 1, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{ 2, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{ 3, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{ 4, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{ 5, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{ 6, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{ 7, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{ 8, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{ 9, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{10, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{11, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{12, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{13, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{14, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{15, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{16, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{17, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{18, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{19, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{20, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{21, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{22, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{23, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{24, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{25, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY},
{26, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}
};
const struct snmp_scalar_array_node snmp_mib2_icmp_root = SNMP_SCALAR_CREATE_ARRAY_NODE(5, icmp_nodes, icmp_get_value, NULL, NULL);
#endif /* LWIP_SNMP && SNMP_LWIP_MIB2 && LWIP_ICMP */

View File

@ -0,0 +1,368 @@
/**
* @file
* Management Information Base II (RFC1213) INTERFACES objects and functions.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
* Christiaan Simons <christiaan.simons@axon.tv>
*/
#include "lwip/snmp.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_mib2.h"
#include "lwip/apps/snmp_table.h"
#include "lwip/apps/snmp_scalar.h"
#include "lwip/netif.h"
#include "lwip/stats.h"
#include <string.h>
#if LWIP_SNMP && SNMP_LWIP_MIB2
#if SNMP_USE_NETCONN
#define SYNC_NODE_NAME(node_name) node_name ## _synced
#define CREATE_LWIP_SYNC_NODE(oid, node_name) \
static const struct snmp_threadsync_node node_name ## _synced = SNMP_CREATE_THREAD_SYNC_NODE(oid, &node_name.node, &snmp_mib2_lwip_locks);
#else
#define SYNC_NODE_NAME(node_name) node_name
#define CREATE_LWIP_SYNC_NODE(oid, node_name)
#endif
/* --- interfaces .1.3.6.1.2.1.2 ----------------------------------------------------- */
static s16_t
interfaces_get_value(struct snmp_node_instance *instance, void *value)
{
if (instance->node->oid == 1) {
s32_t *sint_ptr = (s32_t *)value;
s32_t num_netifs = 0;
struct netif *netif;
NETIF_FOREACH(netif) {
num_netifs++;
}
*sint_ptr = num_netifs;
return sizeof(*sint_ptr);
}
return 0;
}
/* list of allowed value ranges for incoming OID */
static const struct snmp_oid_range interfaces_Table_oid_ranges[] = {
{ 1, 0xff } /* netif->num is u8_t */
};
static const u8_t iftable_ifOutQLen = 0;
static const u8_t iftable_ifOperStatus_up = 1;
static const u8_t iftable_ifOperStatus_down = 2;
static const u8_t iftable_ifAdminStatus_up = 1;
static const u8_t iftable_ifAdminStatus_lowerLayerDown = 7;
static const u8_t iftable_ifAdminStatus_down = 2;
static snmp_err_t
interfaces_Table_get_cell_instance(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, struct snmp_node_instance *cell_instance)
{
u32_t ifIndex;
struct netif *netif;
LWIP_UNUSED_ARG(column);
/* check if incoming OID length and if values are in plausible range */
if (!snmp_oid_in_range(row_oid, row_oid_len, interfaces_Table_oid_ranges, LWIP_ARRAYSIZE(interfaces_Table_oid_ranges))) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* get netif index from incoming OID */
ifIndex = row_oid[0];
/* find netif with index */
NETIF_FOREACH(netif) {
if (netif_to_num(netif) == ifIndex) {
/* store netif pointer for subsequent operations (get/test/set) */
cell_instance->reference.ptr = netif;
return SNMP_ERR_NOERROR;
}
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
interfaces_Table_get_next_cell_instance(const u32_t *column, struct snmp_obj_id *row_oid, struct snmp_node_instance *cell_instance)
{
struct netif *netif;
struct snmp_next_oid_state state;
u32_t result_temp[LWIP_ARRAYSIZE(interfaces_Table_oid_ranges)];
LWIP_UNUSED_ARG(column);
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(interfaces_Table_oid_ranges));
/* iterate over all possible OIDs to find the next one */
NETIF_FOREACH(netif) {
u32_t test_oid[LWIP_ARRAYSIZE(interfaces_Table_oid_ranges)];
test_oid[0] = netif_to_num(netif);
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, LWIP_ARRAYSIZE(interfaces_Table_oid_ranges), netif);
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* store netif pointer for subsequent operations (get/test/set) */
cell_instance->reference.ptr = /* (struct netif*) */state.reference;
return SNMP_ERR_NOERROR;
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static s16_t
interfaces_Table_get_value(struct snmp_node_instance *instance, void *value)
{
struct netif *netif = (struct netif *)instance->reference.ptr;
u32_t *value_u32 = (u32_t *)value;
s32_t *value_s32 = (s32_t *)value;
u16_t value_len;
switch (SNMP_TABLE_GET_COLUMN_FROM_OID(instance->instance_oid.id)) {
case 1: /* ifIndex */
*value_s32 = netif_to_num(netif);
value_len = sizeof(*value_s32);
break;
case 2: /* ifDescr */
value_len = sizeof(netif->name);
MEMCPY(value, netif->name, value_len);
break;
case 3: /* ifType */
*value_s32 = netif->link_type;
value_len = sizeof(*value_s32);
break;
case 4: /* ifMtu */
*value_s32 = netif->mtu;
value_len = sizeof(*value_s32);
break;
case 5: /* ifSpeed */
*value_u32 = netif->link_speed;
value_len = sizeof(*value_u32);
break;
case 6: /* ifPhysAddress */
value_len = sizeof(netif->hwaddr);
MEMCPY(value, &netif->hwaddr, value_len);
break;
case 7: /* ifAdminStatus */
if (netif_is_up(netif)) {
*value_s32 = iftable_ifOperStatus_up;
} else {
*value_s32 = iftable_ifOperStatus_down;
}
value_len = sizeof(*value_s32);
break;
case 8: /* ifOperStatus */
if (netif_is_up(netif)) {
if (netif_is_link_up(netif)) {
*value_s32 = iftable_ifAdminStatus_up;
} else {
*value_s32 = iftable_ifAdminStatus_lowerLayerDown;
}
} else {
*value_s32 = iftable_ifAdminStatus_down;
}
value_len = sizeof(*value_s32);
break;
case 9: /* ifLastChange */
*value_u32 = netif->ts;
value_len = sizeof(*value_u32);
break;
case 10: /* ifInOctets */
*value_u32 = netif->mib2_counters.ifinoctets;
value_len = sizeof(*value_u32);
break;
case 11: /* ifInUcastPkts */
*value_u32 = netif->mib2_counters.ifinucastpkts;
value_len = sizeof(*value_u32);
break;
case 12: /* ifInNUcastPkts */
*value_u32 = netif->mib2_counters.ifinnucastpkts;
value_len = sizeof(*value_u32);
break;
case 13: /* ifInDiscards */
*value_u32 = netif->mib2_counters.ifindiscards;
value_len = sizeof(*value_u32);
break;
case 14: /* ifInErrors */
*value_u32 = netif->mib2_counters.ifinerrors;
value_len = sizeof(*value_u32);
break;
case 15: /* ifInUnkownProtos */
*value_u32 = netif->mib2_counters.ifinunknownprotos;
value_len = sizeof(*value_u32);
break;
case 16: /* ifOutOctets */
*value_u32 = netif->mib2_counters.ifoutoctets;
value_len = sizeof(*value_u32);
break;
case 17: /* ifOutUcastPkts */
*value_u32 = netif->mib2_counters.ifoutucastpkts;
value_len = sizeof(*value_u32);
break;
case 18: /* ifOutNUcastPkts */
*value_u32 = netif->mib2_counters.ifoutnucastpkts;
value_len = sizeof(*value_u32);
break;
case 19: /* ifOutDiscarts */
*value_u32 = netif->mib2_counters.ifoutdiscards;
value_len = sizeof(*value_u32);
break;
case 20: /* ifOutErrors */
*value_u32 = netif->mib2_counters.ifouterrors;
value_len = sizeof(*value_u32);
break;
case 21: /* ifOutQLen */
*value_u32 = iftable_ifOutQLen;
value_len = sizeof(*value_u32);
break;
/** @note returning zeroDotZero (0.0) no media specific MIB support */
case 22: /* ifSpecific */
value_len = snmp_zero_dot_zero.len * sizeof(u32_t);
MEMCPY(value, snmp_zero_dot_zero.id, value_len);
break;
default:
return 0;
}
return value_len;
}
#if !SNMP_SAFE_REQUESTS
static snmp_err_t
interfaces_Table_set_test(struct snmp_node_instance *instance, u16_t len, void *value)
{
s32_t *sint_ptr = (s32_t *)value;
/* stack should never call this method for another column,
because all other columns are set to readonly */
LWIP_ASSERT("Invalid column", (SNMP_TABLE_GET_COLUMN_FROM_OID(instance->instance_oid.id) == 7));
LWIP_UNUSED_ARG(len);
if (*sint_ptr == 1 || *sint_ptr == 2) {
return SNMP_ERR_NOERROR;
}
return SNMP_ERR_WRONGVALUE;
}
static snmp_err_t
interfaces_Table_set_value(struct snmp_node_instance *instance, u16_t len, void *value)
{
struct netif *netif = (struct netif *)instance->reference.ptr;
s32_t *sint_ptr = (s32_t *)value;
/* stack should never call this method for another column,
because all other columns are set to readonly */
LWIP_ASSERT("Invalid column", (SNMP_TABLE_GET_COLUMN_FROM_OID(instance->instance_oid.id) == 7));
LWIP_UNUSED_ARG(len);
if (*sint_ptr == 1) {
netif_set_up(netif);
} else if (*sint_ptr == 2) {
netif_set_down(netif);
}
return SNMP_ERR_NOERROR;
}
#endif /* SNMP_SAFE_REQUESTS */
static const struct snmp_scalar_node interfaces_Number = SNMP_SCALAR_CREATE_NODE_READONLY(1, SNMP_ASN1_TYPE_INTEGER, interfaces_get_value);
static const struct snmp_table_col_def interfaces_Table_columns[] = {
{ 1, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifIndex */
{ 2, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifDescr */
{ 3, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifType */
{ 4, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifMtu */
{ 5, SNMP_ASN1_TYPE_GAUGE, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifSpeed */
{ 6, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifPhysAddress */
#if !SNMP_SAFE_REQUESTS
{ 7, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_WRITE }, /* ifAdminStatus */
#else
{ 7, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifAdminStatus */
#endif
{ 8, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifOperStatus */
{ 9, SNMP_ASN1_TYPE_TIMETICKS, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifLastChange */
{ 10, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifInOctets */
{ 11, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifInUcastPkts */
{ 12, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifInNUcastPkts */
{ 13, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifInDiscarts */
{ 14, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifInErrors */
{ 15, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifInUnkownProtos */
{ 16, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifOutOctets */
{ 17, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifOutUcastPkts */
{ 18, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifOutNUcastPkts */
{ 19, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifOutDiscarts */
{ 20, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifOutErrors */
{ 21, SNMP_ASN1_TYPE_GAUGE, SNMP_NODE_INSTANCE_READ_ONLY }, /* ifOutQLen */
{ 22, SNMP_ASN1_TYPE_OBJECT_ID, SNMP_NODE_INSTANCE_READ_ONLY } /* ifSpecific */
};
#if !SNMP_SAFE_REQUESTS
static const struct snmp_table_node interfaces_Table = SNMP_TABLE_CREATE(
2, interfaces_Table_columns,
interfaces_Table_get_cell_instance, interfaces_Table_get_next_cell_instance,
interfaces_Table_get_value, interfaces_Table_set_test, interfaces_Table_set_value);
#else
static const struct snmp_table_node interfaces_Table = SNMP_TABLE_CREATE(
2, interfaces_Table_columns,
interfaces_Table_get_cell_instance, interfaces_Table_get_next_cell_instance,
interfaces_Table_get_value, NULL, NULL);
#endif
/* the following nodes access variables in LWIP stack from SNMP worker thread and must therefore be synced to LWIP (TCPIP) thread */
CREATE_LWIP_SYNC_NODE(1, interfaces_Number)
CREATE_LWIP_SYNC_NODE(2, interfaces_Table)
static const struct snmp_node *const interface_nodes[] = {
&SYNC_NODE_NAME(interfaces_Number).node.node,
&SYNC_NODE_NAME(interfaces_Table).node.node
};
const struct snmp_tree_node snmp_mib2_interface_root = SNMP_CREATE_TREE_NODE(2, interface_nodes);
#endif /* LWIP_SNMP && SNMP_LWIP_MIB2 */

View File

@ -0,0 +1,731 @@
/**
* @file
* Management Information Base II (RFC1213) IP objects and functions.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
* Christiaan Simons <christiaan.simons@axon.tv>
*/
#include "lwip/snmp.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_mib2.h"
#include "lwip/apps/snmp_table.h"
#include "lwip/apps/snmp_scalar.h"
#include "lwip/stats.h"
#include "lwip/netif.h"
#include "lwip/ip.h"
#include "lwip/etharp.h"
#if LWIP_SNMP && SNMP_LWIP_MIB2
#if SNMP_USE_NETCONN
#define SYNC_NODE_NAME(node_name) node_name ## _synced
#define CREATE_LWIP_SYNC_NODE(oid, node_name) \
static const struct snmp_threadsync_node node_name ## _synced = SNMP_CREATE_THREAD_SYNC_NODE(oid, &node_name.node, &snmp_mib2_lwip_locks);
#else
#define SYNC_NODE_NAME(node_name) node_name
#define CREATE_LWIP_SYNC_NODE(oid, node_name)
#endif
#if LWIP_IPV4
/* --- ip .1.3.6.1.2.1.4 ----------------------------------------------------- */
static s16_t
ip_get_value(struct snmp_node_instance *instance, void *value)
{
s32_t *sint_ptr = (s32_t *)value;
u32_t *uint_ptr = (u32_t *)value;
switch (instance->node->oid) {
case 1: /* ipForwarding */
#if IP_FORWARD
/* forwarding */
*sint_ptr = 1;
#else
/* not-forwarding */
*sint_ptr = 2;
#endif
return sizeof(*sint_ptr);
case 2: /* ipDefaultTTL */
*sint_ptr = IP_DEFAULT_TTL;
return sizeof(*sint_ptr);
case 3: /* ipInReceives */
*uint_ptr = STATS_GET(mib2.ipinreceives);
return sizeof(*uint_ptr);
case 4: /* ipInHdrErrors */
*uint_ptr = STATS_GET(mib2.ipinhdrerrors);
return sizeof(*uint_ptr);
case 5: /* ipInAddrErrors */
*uint_ptr = STATS_GET(mib2.ipinaddrerrors);
return sizeof(*uint_ptr);
case 6: /* ipForwDatagrams */
*uint_ptr = STATS_GET(mib2.ipforwdatagrams);
return sizeof(*uint_ptr);
case 7: /* ipInUnknownProtos */
*uint_ptr = STATS_GET(mib2.ipinunknownprotos);
return sizeof(*uint_ptr);
case 8: /* ipInDiscards */
*uint_ptr = STATS_GET(mib2.ipindiscards);
return sizeof(*uint_ptr);
case 9: /* ipInDelivers */
*uint_ptr = STATS_GET(mib2.ipindelivers);
return sizeof(*uint_ptr);
case 10: /* ipOutRequests */
*uint_ptr = STATS_GET(mib2.ipoutrequests);
return sizeof(*uint_ptr);
case 11: /* ipOutDiscards */
*uint_ptr = STATS_GET(mib2.ipoutdiscards);
return sizeof(*uint_ptr);
case 12: /* ipOutNoRoutes */
*uint_ptr = STATS_GET(mib2.ipoutnoroutes);
return sizeof(*uint_ptr);
case 13: /* ipReasmTimeout */
#if IP_REASSEMBLY
*sint_ptr = IP_REASS_MAXAGE;
#else
*sint_ptr = 0;
#endif
return sizeof(*sint_ptr);
case 14: /* ipReasmReqds */
*uint_ptr = STATS_GET(mib2.ipreasmreqds);
return sizeof(*uint_ptr);
case 15: /* ipReasmOKs */
*uint_ptr = STATS_GET(mib2.ipreasmoks);
return sizeof(*uint_ptr);
case 16: /* ipReasmFails */
*uint_ptr = STATS_GET(mib2.ipreasmfails);
return sizeof(*uint_ptr);
case 17: /* ipFragOKs */
*uint_ptr = STATS_GET(mib2.ipfragoks);
return sizeof(*uint_ptr);
case 18: /* ipFragFails */
*uint_ptr = STATS_GET(mib2.ipfragfails);
return sizeof(*uint_ptr);
case 19: /* ipFragCreates */
*uint_ptr = STATS_GET(mib2.ipfragcreates);
return sizeof(*uint_ptr);
case 23: /* ipRoutingDiscards: not supported -> always 0 */
*uint_ptr = 0;
return sizeof(*uint_ptr);
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("ip_get_value(): unknown id: %"S32_F"\n", instance->node->oid));
break;
}
return 0;
}
/**
* Test ip object value before setting.
*
* @param instance node instance
* @param len return value space (in bytes)
* @param value points to (varbind) space to copy value from.
*
* @note we allow set if the value matches the hardwired value,
* otherwise return badvalue.
*/
static snmp_err_t
ip_set_test(struct snmp_node_instance *instance, u16_t len, void *value)
{
snmp_err_t ret = SNMP_ERR_WRONGVALUE;
s32_t *sint_ptr = (s32_t *)value;
LWIP_UNUSED_ARG(len);
switch (instance->node->oid) {
case 1: /* ipForwarding */
#if IP_FORWARD
/* forwarding */
if (*sint_ptr == 1)
#else
/* not-forwarding */
if (*sint_ptr == 2)
#endif
{
ret = SNMP_ERR_NOERROR;
}
break;
case 2: /* ipDefaultTTL */
if (*sint_ptr == IP_DEFAULT_TTL) {
ret = SNMP_ERR_NOERROR;
}
break;
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("ip_set_test(): unknown id: %"S32_F"\n", instance->node->oid));
break;
}
return ret;
}
static snmp_err_t
ip_set_value(struct snmp_node_instance *instance, u16_t len, void *value)
{
LWIP_UNUSED_ARG(instance);
LWIP_UNUSED_ARG(len);
LWIP_UNUSED_ARG(value);
/* nothing to do here because in set_test we only accept values being the same as our own stored value -> no need to store anything */
return SNMP_ERR_NOERROR;
}
/* --- ipAddrTable --- */
/* list of allowed value ranges for incoming OID */
static const struct snmp_oid_range ip_AddrTable_oid_ranges[] = {
{ 0, 0xff }, /* IP A */
{ 0, 0xff }, /* IP B */
{ 0, 0xff }, /* IP C */
{ 0, 0xff } /* IP D */
};
static snmp_err_t
ip_AddrTable_get_cell_value_core(struct netif *netif, const u32_t *column, union snmp_variant_value *value, u32_t *value_len)
{
LWIP_UNUSED_ARG(value_len);
switch (*column) {
case 1: /* ipAdEntAddr */
value->u32 = netif_ip4_addr(netif)->addr;
break;
case 2: /* ipAdEntIfIndex */
value->u32 = netif_to_num(netif);
break;
case 3: /* ipAdEntNetMask */
value->u32 = netif_ip4_netmask(netif)->addr;
break;
case 4: /* ipAdEntBcastAddr */
/* lwIP oddity, there's no broadcast
address in the netif we can rely on */
value->u32 = IPADDR_BROADCAST & 1;
break;
case 5: /* ipAdEntReasmMaxSize */
#if IP_REASSEMBLY
/* @todo The theoretical maximum is IP_REASS_MAX_PBUFS * size of the pbufs,
* but only if receiving one fragmented packet at a time.
* The current solution is to calculate for 2 simultaneous packets...
*/
value->u32 = (IP_HLEN + ((IP_REASS_MAX_PBUFS / 2) *
(PBUF_POOL_BUFSIZE - PBUF_LINK_ENCAPSULATION_HLEN - PBUF_LINK_HLEN - IP_HLEN)));
#else
/** @todo returning MTU would be a bad thing and
returning a wild guess like '576' isn't good either */
value->u32 = 0;
#endif
break;
default:
return SNMP_ERR_NOSUCHINSTANCE;
}
return SNMP_ERR_NOERROR;
}
static snmp_err_t
ip_AddrTable_get_cell_value(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, union snmp_variant_value *value, u32_t *value_len)
{
ip4_addr_t ip;
struct netif *netif;
/* check if incoming OID length and if values are in plausible range */
if (!snmp_oid_in_range(row_oid, row_oid_len, ip_AddrTable_oid_ranges, LWIP_ARRAYSIZE(ip_AddrTable_oid_ranges))) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* get IP from incoming OID */
snmp_oid_to_ip4(&row_oid[0], &ip); /* we know it succeeds because of oid_in_range check above */
/* find netif with requested ip */
NETIF_FOREACH(netif) {
if (ip4_addr_cmp(&ip, netif_ip4_addr(netif))) {
/* fill in object properties */
return ip_AddrTable_get_cell_value_core(netif, column, value, value_len);
}
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
ip_AddrTable_get_next_cell_instance_and_value(const u32_t *column, struct snmp_obj_id *row_oid, union snmp_variant_value *value, u32_t *value_len)
{
struct netif *netif;
struct snmp_next_oid_state state;
u32_t result_temp[LWIP_ARRAYSIZE(ip_AddrTable_oid_ranges)];
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(ip_AddrTable_oid_ranges));
/* iterate over all possible OIDs to find the next one */
NETIF_FOREACH(netif) {
u32_t test_oid[LWIP_ARRAYSIZE(ip_AddrTable_oid_ranges)];
snmp_ip4_to_oid(netif_ip4_addr(netif), &test_oid[0]);
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, LWIP_ARRAYSIZE(ip_AddrTable_oid_ranges), netif);
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* fill in object properties */
return ip_AddrTable_get_cell_value_core((struct netif *)state.reference, column, value, value_len);
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
/* --- ipRouteTable --- */
/* list of allowed value ranges for incoming OID */
static const struct snmp_oid_range ip_RouteTable_oid_ranges[] = {
{ 0, 0xff }, /* IP A */
{ 0, 0xff }, /* IP B */
{ 0, 0xff }, /* IP C */
{ 0, 0xff }, /* IP D */
};
static snmp_err_t
ip_RouteTable_get_cell_value_core(struct netif *netif, u8_t default_route, const u32_t *column, union snmp_variant_value *value, u32_t *value_len)
{
switch (*column) {
case 1: /* ipRouteDest */
if (default_route) {
/* default rte has 0.0.0.0 dest */
value->u32 = IP4_ADDR_ANY4->addr;
} else {
/* netifs have netaddress dest */
ip4_addr_t tmp;
ip4_addr_get_network(&tmp, netif_ip4_addr(netif), netif_ip4_netmask(netif));
value->u32 = tmp.addr;
}
break;
case 2: /* ipRouteIfIndex */
value->u32 = netif_to_num(netif);
break;
case 3: /* ipRouteMetric1 */
if (default_route) {
value->s32 = 1; /* default */
} else {
value->s32 = 0; /* normal */
}
break;
case 4: /* ipRouteMetric2 */
case 5: /* ipRouteMetric3 */
case 6: /* ipRouteMetric4 */
value->s32 = -1; /* none */
break;
case 7: /* ipRouteNextHop */
if (default_route) {
/* default rte: gateway */
value->u32 = netif_ip4_gw(netif)->addr;
} else {
/* other rtes: netif ip_addr */
value->u32 = netif_ip4_addr(netif)->addr;
}
break;
case 8: /* ipRouteType */
if (default_route) {
/* default rte is indirect */
value->u32 = 4; /* indirect */
} else {
/* other rtes are direct */
value->u32 = 3; /* direct */
}
break;
case 9: /* ipRouteProto */
/* locally defined routes */
value->u32 = 2; /* local */
break;
case 10: /* ipRouteAge */
/* @todo (sysuptime - timestamp last change) / 100 */
value->u32 = 0;
break;
case 11: /* ipRouteMask */
if (default_route) {
/* default rte use 0.0.0.0 mask */
value->u32 = IP4_ADDR_ANY4->addr;
} else {
/* other rtes use netmask */
value->u32 = netif_ip4_netmask(netif)->addr;
}
break;
case 12: /* ipRouteMetric5 */
value->s32 = -1; /* none */
break;
case 13: /* ipRouteInfo */
value->const_ptr = snmp_zero_dot_zero.id;
*value_len = snmp_zero_dot_zero.len * sizeof(u32_t);
break;
default:
return SNMP_ERR_NOSUCHINSTANCE;
}
return SNMP_ERR_NOERROR;
}
static snmp_err_t
ip_RouteTable_get_cell_value(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, union snmp_variant_value *value, u32_t *value_len)
{
ip4_addr_t test_ip;
struct netif *netif;
/* check if incoming OID length and if values are in plausible range */
if (!snmp_oid_in_range(row_oid, row_oid_len, ip_RouteTable_oid_ranges, LWIP_ARRAYSIZE(ip_RouteTable_oid_ranges))) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* get IP and port from incoming OID */
snmp_oid_to_ip4(&row_oid[0], &test_ip); /* we know it succeeds because of oid_in_range check above */
/* default route is on default netif */
if (ip4_addr_isany_val(test_ip) && (netif_default != NULL)) {
/* fill in object properties */
return ip_RouteTable_get_cell_value_core(netif_default, 1, column, value, value_len);
}
/* find netif with requested route */
NETIF_FOREACH(netif) {
ip4_addr_t dst;
ip4_addr_get_network(&dst, netif_ip4_addr(netif), netif_ip4_netmask(netif));
if (ip4_addr_cmp(&dst, &test_ip)) {
/* fill in object properties */
return ip_RouteTable_get_cell_value_core(netif, 0, column, value, value_len);
}
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
ip_RouteTable_get_next_cell_instance_and_value(const u32_t *column, struct snmp_obj_id *row_oid, union snmp_variant_value *value, u32_t *value_len)
{
struct netif *netif;
struct snmp_next_oid_state state;
u32_t result_temp[LWIP_ARRAYSIZE(ip_RouteTable_oid_ranges)];
u32_t test_oid[LWIP_ARRAYSIZE(ip_RouteTable_oid_ranges)];
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(ip_RouteTable_oid_ranges));
/* check default route */
if (netif_default != NULL) {
snmp_ip4_to_oid(IP4_ADDR_ANY4, &test_oid[0]);
snmp_next_oid_check(&state, test_oid, LWIP_ARRAYSIZE(ip_RouteTable_oid_ranges), netif_default);
}
/* iterate over all possible OIDs to find the next one */
NETIF_FOREACH(netif) {
ip4_addr_t dst;
ip4_addr_get_network(&dst, netif_ip4_addr(netif), netif_ip4_netmask(netif));
/* check generated OID: is it a candidate for the next one? */
if (!ip4_addr_isany_val(dst)) {
snmp_ip4_to_oid(&dst, &test_oid[0]);
snmp_next_oid_check(&state, test_oid, LWIP_ARRAYSIZE(ip_RouteTable_oid_ranges), netif);
}
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
ip4_addr_t dst;
snmp_oid_to_ip4(&result_temp[0], &dst);
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* fill in object properties */
return ip_RouteTable_get_cell_value_core((struct netif *)state.reference, ip4_addr_isany_val(dst), column, value, value_len);
} else {
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
}
#if LWIP_ARP && LWIP_IPV4
/* --- ipNetToMediaTable --- */
/* list of allowed value ranges for incoming OID */
static const struct snmp_oid_range ip_NetToMediaTable_oid_ranges[] = {
{ 1, 0xff }, /* IfIndex */
{ 0, 0xff }, /* IP A */
{ 0, 0xff }, /* IP B */
{ 0, 0xff }, /* IP C */
{ 0, 0xff } /* IP D */
};
static snmp_err_t
ip_NetToMediaTable_get_cell_value_core(size_t arp_table_index, const u32_t *column, union snmp_variant_value *value, u32_t *value_len)
{
ip4_addr_t *ip;
struct netif *netif;
struct eth_addr *ethaddr;
etharp_get_entry(arp_table_index, &ip, &netif, &ethaddr);
/* value */
switch (*column) {
case 1: /* atIfIndex / ipNetToMediaIfIndex */
value->u32 = netif_to_num(netif);
break;
case 2: /* atPhysAddress / ipNetToMediaPhysAddress */
value->ptr = ethaddr;
*value_len = sizeof(*ethaddr);
break;
case 3: /* atNetAddress / ipNetToMediaNetAddress */
value->u32 = ip->addr;
break;
case 4: /* ipNetToMediaType */
value->u32 = 3; /* dynamic*/
break;
default:
return SNMP_ERR_NOSUCHINSTANCE;
}
return SNMP_ERR_NOERROR;
}
static snmp_err_t
ip_NetToMediaTable_get_cell_value(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, union snmp_variant_value *value, u32_t *value_len)
{
ip4_addr_t ip_in;
u8_t netif_index;
size_t i;
/* check if incoming OID length and if values are in plausible range */
if (!snmp_oid_in_range(row_oid, row_oid_len, ip_NetToMediaTable_oid_ranges, LWIP_ARRAYSIZE(ip_NetToMediaTable_oid_ranges))) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* get IP from incoming OID */
netif_index = (u8_t)row_oid[0];
snmp_oid_to_ip4(&row_oid[1], &ip_in); /* we know it succeeds because of oid_in_range check above */
/* find requested entry */
for (i = 0; i < ARP_TABLE_SIZE; i++) {
ip4_addr_t *ip;
struct netif *netif;
struct eth_addr *ethaddr;
if (etharp_get_entry(i, &ip, &netif, &ethaddr)) {
if ((netif_index == netif_to_num(netif)) && ip4_addr_cmp(&ip_in, ip)) {
/* fill in object properties */
return ip_NetToMediaTable_get_cell_value_core(i, column, value, value_len);
}
}
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
ip_NetToMediaTable_get_next_cell_instance_and_value(const u32_t *column, struct snmp_obj_id *row_oid, union snmp_variant_value *value, u32_t *value_len)
{
size_t i;
struct snmp_next_oid_state state;
u32_t result_temp[LWIP_ARRAYSIZE(ip_NetToMediaTable_oid_ranges)];
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(ip_NetToMediaTable_oid_ranges));
/* iterate over all possible OIDs to find the next one */
for (i = 0; i < ARP_TABLE_SIZE; i++) {
ip4_addr_t *ip;
struct netif *netif;
struct eth_addr *ethaddr;
if (etharp_get_entry(i, &ip, &netif, &ethaddr)) {
u32_t test_oid[LWIP_ARRAYSIZE(ip_NetToMediaTable_oid_ranges)];
test_oid[0] = netif_to_num(netif);
snmp_ip4_to_oid(ip, &test_oid[1]);
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, LWIP_ARRAYSIZE(ip_NetToMediaTable_oid_ranges), LWIP_PTR_NUMERIC_CAST(void *, i));
}
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* fill in object properties */
return ip_NetToMediaTable_get_cell_value_core(LWIP_PTR_NUMERIC_CAST(size_t, state.reference), column, value, value_len);
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
#endif /* LWIP_ARP && LWIP_IPV4 */
static const struct snmp_scalar_node ip_Forwarding = SNMP_SCALAR_CREATE_NODE(1, SNMP_NODE_INSTANCE_READ_WRITE, SNMP_ASN1_TYPE_INTEGER, ip_get_value, ip_set_test, ip_set_value);
static const struct snmp_scalar_node ip_DefaultTTL = SNMP_SCALAR_CREATE_NODE(2, SNMP_NODE_INSTANCE_READ_WRITE, SNMP_ASN1_TYPE_INTEGER, ip_get_value, ip_set_test, ip_set_value);
static const struct snmp_scalar_node ip_InReceives = SNMP_SCALAR_CREATE_NODE_READONLY(3, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_InHdrErrors = SNMP_SCALAR_CREATE_NODE_READONLY(4, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_InAddrErrors = SNMP_SCALAR_CREATE_NODE_READONLY(5, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_ForwDatagrams = SNMP_SCALAR_CREATE_NODE_READONLY(6, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_InUnknownProtos = SNMP_SCALAR_CREATE_NODE_READONLY(7, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_InDiscards = SNMP_SCALAR_CREATE_NODE_READONLY(8, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_InDelivers = SNMP_SCALAR_CREATE_NODE_READONLY(9, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_OutRequests = SNMP_SCALAR_CREATE_NODE_READONLY(10, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_OutDiscards = SNMP_SCALAR_CREATE_NODE_READONLY(11, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_OutNoRoutes = SNMP_SCALAR_CREATE_NODE_READONLY(12, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_ReasmTimeout = SNMP_SCALAR_CREATE_NODE_READONLY(13, SNMP_ASN1_TYPE_INTEGER, ip_get_value);
static const struct snmp_scalar_node ip_ReasmReqds = SNMP_SCALAR_CREATE_NODE_READONLY(14, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_ReasmOKs = SNMP_SCALAR_CREATE_NODE_READONLY(15, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_ReasmFails = SNMP_SCALAR_CREATE_NODE_READONLY(16, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_FragOKs = SNMP_SCALAR_CREATE_NODE_READONLY(17, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_FragFails = SNMP_SCALAR_CREATE_NODE_READONLY(18, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_FragCreates = SNMP_SCALAR_CREATE_NODE_READONLY(19, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_scalar_node ip_RoutingDiscards = SNMP_SCALAR_CREATE_NODE_READONLY(23, SNMP_ASN1_TYPE_COUNTER, ip_get_value);
static const struct snmp_table_simple_col_def ip_AddrTable_columns[] = {
{ 1, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipAdEntAddr */
{ 2, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipAdEntIfIndex */
{ 3, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipAdEntNetMask */
{ 4, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipAdEntBcastAddr */
{ 5, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 } /* ipAdEntReasmMaxSize */
};
static const struct snmp_table_simple_node ip_AddrTable = SNMP_TABLE_CREATE_SIMPLE(20, ip_AddrTable_columns, ip_AddrTable_get_cell_value, ip_AddrTable_get_next_cell_instance_and_value);
static const struct snmp_table_simple_col_def ip_RouteTable_columns[] = {
{ 1, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipRouteDest */
{ 2, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipRouteIfIndex */
{ 3, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_S32 }, /* ipRouteMetric1 */
{ 4, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_S32 }, /* ipRouteMetric2 */
{ 5, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_S32 }, /* ipRouteMetric3 */
{ 6, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_S32 }, /* ipRouteMetric4 */
{ 7, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipRouteNextHop */
{ 8, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipRouteType */
{ 9, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipRouteProto */
{ 10, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipRouteAge */
{ 11, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipRouteMask */
{ 12, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_S32 }, /* ipRouteMetric5 */
{ 13, SNMP_ASN1_TYPE_OBJECT_ID, SNMP_VARIANT_VALUE_TYPE_PTR } /* ipRouteInfo */
};
static const struct snmp_table_simple_node ip_RouteTable = SNMP_TABLE_CREATE_SIMPLE(21, ip_RouteTable_columns, ip_RouteTable_get_cell_value, ip_RouteTable_get_next_cell_instance_and_value);
#endif /* LWIP_IPV4 */
#if LWIP_ARP && LWIP_IPV4
static const struct snmp_table_simple_col_def ip_NetToMediaTable_columns[] = {
{ 1, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipNetToMediaIfIndex */
{ 2, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_VARIANT_VALUE_TYPE_PTR }, /* ipNetToMediaPhysAddress */
{ 3, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* ipNetToMediaNetAddress */
{ 4, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 } /* ipNetToMediaType */
};
static const struct snmp_table_simple_node ip_NetToMediaTable = SNMP_TABLE_CREATE_SIMPLE(22, ip_NetToMediaTable_columns, ip_NetToMediaTable_get_cell_value, ip_NetToMediaTable_get_next_cell_instance_and_value);
#endif /* LWIP_ARP && LWIP_IPV4 */
#if LWIP_IPV4
/* the following nodes access variables in LWIP stack from SNMP worker thread and must therefore be synced to LWIP (TCPIP) thread */
CREATE_LWIP_SYNC_NODE( 1, ip_Forwarding)
CREATE_LWIP_SYNC_NODE( 2, ip_DefaultTTL)
CREATE_LWIP_SYNC_NODE( 3, ip_InReceives)
CREATE_LWIP_SYNC_NODE( 4, ip_InHdrErrors)
CREATE_LWIP_SYNC_NODE( 5, ip_InAddrErrors)
CREATE_LWIP_SYNC_NODE( 6, ip_ForwDatagrams)
CREATE_LWIP_SYNC_NODE( 7, ip_InUnknownProtos)
CREATE_LWIP_SYNC_NODE( 8, ip_InDiscards)
CREATE_LWIP_SYNC_NODE( 9, ip_InDelivers)
CREATE_LWIP_SYNC_NODE(10, ip_OutRequests)
CREATE_LWIP_SYNC_NODE(11, ip_OutDiscards)
CREATE_LWIP_SYNC_NODE(12, ip_OutNoRoutes)
CREATE_LWIP_SYNC_NODE(13, ip_ReasmTimeout)
CREATE_LWIP_SYNC_NODE(14, ip_ReasmReqds)
CREATE_LWIP_SYNC_NODE(15, ip_ReasmOKs)
CREATE_LWIP_SYNC_NODE(15, ip_ReasmFails)
CREATE_LWIP_SYNC_NODE(17, ip_FragOKs)
CREATE_LWIP_SYNC_NODE(18, ip_FragFails)
CREATE_LWIP_SYNC_NODE(19, ip_FragCreates)
CREATE_LWIP_SYNC_NODE(20, ip_AddrTable)
CREATE_LWIP_SYNC_NODE(21, ip_RouteTable)
#if LWIP_ARP
CREATE_LWIP_SYNC_NODE(22, ip_NetToMediaTable)
#endif /* LWIP_ARP */
CREATE_LWIP_SYNC_NODE(23, ip_RoutingDiscards)
static const struct snmp_node *const ip_nodes[] = {
&SYNC_NODE_NAME(ip_Forwarding).node.node,
&SYNC_NODE_NAME(ip_DefaultTTL).node.node,
&SYNC_NODE_NAME(ip_InReceives).node.node,
&SYNC_NODE_NAME(ip_InHdrErrors).node.node,
&SYNC_NODE_NAME(ip_InAddrErrors).node.node,
&SYNC_NODE_NAME(ip_ForwDatagrams).node.node,
&SYNC_NODE_NAME(ip_InUnknownProtos).node.node,
&SYNC_NODE_NAME(ip_InDiscards).node.node,
&SYNC_NODE_NAME(ip_InDelivers).node.node,
&SYNC_NODE_NAME(ip_OutRequests).node.node,
&SYNC_NODE_NAME(ip_OutDiscards).node.node,
&SYNC_NODE_NAME(ip_OutNoRoutes).node.node,
&SYNC_NODE_NAME(ip_ReasmTimeout).node.node,
&SYNC_NODE_NAME(ip_ReasmReqds).node.node,
&SYNC_NODE_NAME(ip_ReasmOKs).node.node,
&SYNC_NODE_NAME(ip_ReasmFails).node.node,
&SYNC_NODE_NAME(ip_FragOKs).node.node,
&SYNC_NODE_NAME(ip_FragFails).node.node,
&SYNC_NODE_NAME(ip_FragCreates).node.node,
&SYNC_NODE_NAME(ip_AddrTable).node.node,
&SYNC_NODE_NAME(ip_RouteTable).node.node,
#if LWIP_ARP
&SYNC_NODE_NAME(ip_NetToMediaTable).node.node,
#endif /* LWIP_ARP */
&SYNC_NODE_NAME(ip_RoutingDiscards).node.node
};
const struct snmp_tree_node snmp_mib2_ip_root = SNMP_CREATE_TREE_NODE(4, ip_nodes);
#endif /* LWIP_IPV4 */
/* --- at .1.3.6.1.2.1.3 ----------------------------------------------------- */
#if LWIP_ARP && LWIP_IPV4
/* at node table is a subset of ip_nettomedia table (same rows but less columns) */
static const struct snmp_table_simple_col_def at_Table_columns[] = {
{ 1, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* atIfIndex */
{ 2, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_VARIANT_VALUE_TYPE_PTR }, /* atPhysAddress */
{ 3, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 } /* atNetAddress */
};
static const struct snmp_table_simple_node at_Table = SNMP_TABLE_CREATE_SIMPLE(1, at_Table_columns, ip_NetToMediaTable_get_cell_value, ip_NetToMediaTable_get_next_cell_instance_and_value);
/* the following nodes access variables in LWIP stack from SNMP worker thread and must therefore be synced to LWIP (TCPIP) thread */
CREATE_LWIP_SYNC_NODE(1, at_Table)
static const struct snmp_node *const at_nodes[] = {
&SYNC_NODE_NAME(at_Table).node.node
};
const struct snmp_tree_node snmp_mib2_at_root = SNMP_CREATE_TREE_NODE(3, at_nodes);
#endif /* LWIP_ARP && LWIP_IPV4 */
#endif /* LWIP_SNMP && SNMP_LWIP_MIB2 */

View File

@ -0,0 +1,227 @@
/**
* @file
* Management Information Base II (RFC1213) SNMP objects and functions.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
* Christiaan Simons <christiaan.simons@axon.tv>
*/
#include "lwip/snmp.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_mib2.h"
#include "lwip/apps/snmp_scalar.h"
#if LWIP_SNMP && SNMP_LWIP_MIB2
#define MIB2_AUTH_TRAPS_ENABLED 1
#define MIB2_AUTH_TRAPS_DISABLED 2
/* --- snmp .1.3.6.1.2.1.11 ----------------------------------------------------- */
static s16_t
snmp_get_value(const struct snmp_scalar_array_node_def *node, void *value)
{
u32_t *uint_ptr = (u32_t *)value;
switch (node->oid) {
case 1: /* snmpInPkts */
*uint_ptr = snmp_stats.inpkts;
break;
case 2: /* snmpOutPkts */
*uint_ptr = snmp_stats.outpkts;
break;
case 3: /* snmpInBadVersions */
*uint_ptr = snmp_stats.inbadversions;
break;
case 4: /* snmpInBadCommunityNames */
*uint_ptr = snmp_stats.inbadcommunitynames;
break;
case 5: /* snmpInBadCommunityUses */
*uint_ptr = snmp_stats.inbadcommunityuses;
break;
case 6: /* snmpInASNParseErrs */
*uint_ptr = snmp_stats.inasnparseerrs;
break;
case 8: /* snmpInTooBigs */
*uint_ptr = snmp_stats.intoobigs;
break;
case 9: /* snmpInNoSuchNames */
*uint_ptr = snmp_stats.innosuchnames;
break;
case 10: /* snmpInBadValues */
*uint_ptr = snmp_stats.inbadvalues;
break;
case 11: /* snmpInReadOnlys */
*uint_ptr = snmp_stats.inreadonlys;
break;
case 12: /* snmpInGenErrs */
*uint_ptr = snmp_stats.ingenerrs;
break;
case 13: /* snmpInTotalReqVars */
*uint_ptr = snmp_stats.intotalreqvars;
break;
case 14: /* snmpInTotalSetVars */
*uint_ptr = snmp_stats.intotalsetvars;
break;
case 15: /* snmpInGetRequests */
*uint_ptr = snmp_stats.ingetrequests;
break;
case 16: /* snmpInGetNexts */
*uint_ptr = snmp_stats.ingetnexts;
break;
case 17: /* snmpInSetRequests */
*uint_ptr = snmp_stats.insetrequests;
break;
case 18: /* snmpInGetResponses */
*uint_ptr = snmp_stats.ingetresponses;
break;
case 19: /* snmpInTraps */
*uint_ptr = snmp_stats.intraps;
break;
case 20: /* snmpOutTooBigs */
*uint_ptr = snmp_stats.outtoobigs;
break;
case 21: /* snmpOutNoSuchNames */
*uint_ptr = snmp_stats.outnosuchnames;
break;
case 22: /* snmpOutBadValues */
*uint_ptr = snmp_stats.outbadvalues;
break;
case 24: /* snmpOutGenErrs */
*uint_ptr = snmp_stats.outgenerrs;
break;
case 25: /* snmpOutGetRequests */
*uint_ptr = snmp_stats.outgetrequests;
break;
case 26: /* snmpOutGetNexts */
*uint_ptr = snmp_stats.outgetnexts;
break;
case 27: /* snmpOutSetRequests */
*uint_ptr = snmp_stats.outsetrequests;
break;
case 28: /* snmpOutGetResponses */
*uint_ptr = snmp_stats.outgetresponses;
break;
case 29: /* snmpOutTraps */
*uint_ptr = snmp_stats.outtraps;
break;
case 30: /* snmpEnableAuthenTraps */
if (snmp_get_auth_traps_enabled() == SNMP_AUTH_TRAPS_DISABLED) {
*uint_ptr = MIB2_AUTH_TRAPS_DISABLED;
} else {
*uint_ptr = MIB2_AUTH_TRAPS_ENABLED;
}
break;
case 31: /* snmpSilentDrops */
*uint_ptr = 0; /* not supported */
break;
case 32: /* snmpProxyDrops */
*uint_ptr = 0; /* not supported */
break;
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("snmp_get_value(): unknown id: %"S32_F"\n", node->oid));
return 0;
}
return sizeof(*uint_ptr);
}
static snmp_err_t
snmp_set_test(const struct snmp_scalar_array_node_def *node, u16_t len, void *value)
{
snmp_err_t ret = SNMP_ERR_WRONGVALUE;
LWIP_UNUSED_ARG(len);
if (node->oid == 30) {
/* snmpEnableAuthenTraps */
s32_t *sint_ptr = (s32_t *)value;
/* we should have writable non-volatile mem here */
if ((*sint_ptr == MIB2_AUTH_TRAPS_DISABLED) || (*sint_ptr == MIB2_AUTH_TRAPS_ENABLED)) {
ret = SNMP_ERR_NOERROR;
}
}
return ret;
}
static snmp_err_t
snmp_set_value(const struct snmp_scalar_array_node_def *node, u16_t len, void *value)
{
LWIP_UNUSED_ARG(len);
if (node->oid == 30) {
/* snmpEnableAuthenTraps */
s32_t *sint_ptr = (s32_t *)value;
if (*sint_ptr == MIB2_AUTH_TRAPS_DISABLED) {
snmp_set_auth_traps_enabled(SNMP_AUTH_TRAPS_DISABLED);
} else {
snmp_set_auth_traps_enabled(SNMP_AUTH_TRAPS_ENABLED);
}
}
return SNMP_ERR_NOERROR;
}
/* the following nodes access variables in SNMP stack (snmp_stats) from SNMP worker thread -> OK, no sync needed */
static const struct snmp_scalar_array_node_def snmp_nodes[] = {
{ 1, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInPkts */
{ 2, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutPkts */
{ 3, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInBadVersions */
{ 4, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInBadCommunityNames */
{ 5, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInBadCommunityUses */
{ 6, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInASNParseErrs */
{ 8, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInTooBigs */
{ 9, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInNoSuchNames */
{10, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInBadValues */
{11, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInReadOnlys */
{12, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInGenErrs */
{13, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInTotalReqVars */
{14, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInTotalSetVars */
{15, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInGetRequests */
{16, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInGetNexts */
{17, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInSetRequests */
{18, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInGetResponses */
{19, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpInTraps */
{20, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutTooBigs */
{21, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutNoSuchNames */
{22, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutBadValues */
{24, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutGenErrs */
{25, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutGetRequests */
{26, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutGetNexts */
{27, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutSetRequests */
{28, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutGetResponses */
{29, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpOutTraps */
{30, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_WRITE}, /* snmpEnableAuthenTraps */
{31, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpSilentDrops */
{32, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY} /* snmpProxyDrops */
};
const struct snmp_scalar_array_node snmp_mib2_snmp_root = SNMP_SCALAR_CREATE_ARRAY_NODE(11, snmp_nodes, snmp_get_value, snmp_set_test, snmp_set_value);
#endif /* LWIP_SNMP && SNMP_LWIP_MIB2 */

View File

@ -0,0 +1,376 @@
/**
* @file
* Management Information Base II (RFC1213) SYSTEM objects and functions.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
* Christiaan Simons <christiaan.simons@axon.tv>
*/
#include "lwip/snmp.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_mib2.h"
#include "lwip/apps/snmp_table.h"
#include "lwip/apps/snmp_scalar.h"
#include "lwip/sys.h"
#include <string.h>
#if LWIP_SNMP && SNMP_LWIP_MIB2
#if SNMP_USE_NETCONN
#define SYNC_NODE_NAME(node_name) node_name ## _synced
#define CREATE_LWIP_SYNC_NODE(oid, node_name) \
static const struct snmp_threadsync_node node_name ## _synced = SNMP_CREATE_THREAD_SYNC_NODE(oid, &node_name.node, &snmp_mib2_lwip_locks);
#else
#define SYNC_NODE_NAME(node_name) node_name
#define CREATE_LWIP_SYNC_NODE(oid, node_name)
#endif
/* --- system .1.3.6.1.2.1.1 ----------------------------------------------------- */
/** mib-2.system.sysDescr */
static const u8_t sysdescr_default[] = SNMP_LWIP_MIB2_SYSDESC;
static const u8_t *sysdescr = sysdescr_default;
static const u16_t *sysdescr_len = NULL; /* use strlen for determining len */
/** mib-2.system.sysContact */
static const u8_t syscontact_default[] = SNMP_LWIP_MIB2_SYSCONTACT;
static const u8_t *syscontact = syscontact_default;
static const u16_t *syscontact_len = NULL; /* use strlen for determining len */
static u8_t *syscontact_wr = NULL; /* if writable, points to the same buffer as syscontact (required for correct constness) */
static u16_t *syscontact_wr_len = NULL; /* if writable, points to the same buffer as syscontact_len (required for correct constness) */
static u16_t syscontact_bufsize = 0; /* 0=not writable */
/** mib-2.system.sysName */
static const u8_t sysname_default[] = SNMP_LWIP_MIB2_SYSNAME;
static const u8_t *sysname = sysname_default;
static const u16_t *sysname_len = NULL; /* use strlen for determining len */
static u8_t *sysname_wr = NULL; /* if writable, points to the same buffer as sysname (required for correct constness) */
static u16_t *sysname_wr_len = NULL; /* if writable, points to the same buffer as sysname_len (required for correct constness) */
static u16_t sysname_bufsize = 0; /* 0=not writable */
/** mib-2.system.sysLocation */
static const u8_t syslocation_default[] = SNMP_LWIP_MIB2_SYSLOCATION;
static const u8_t *syslocation = syslocation_default;
static const u16_t *syslocation_len = NULL; /* use strlen for determining len */
static u8_t *syslocation_wr = NULL; /* if writable, points to the same buffer as syslocation (required for correct constness) */
static u16_t *syslocation_wr_len = NULL; /* if writable, points to the same buffer as syslocation_len (required for correct constness) */
static u16_t syslocation_bufsize = 0; /* 0=not writable */
/**
* @ingroup snmp_mib2
* Initializes sysDescr pointers.
*
* @param str if non-NULL then copy str pointer
* @param len points to string length, excluding zero terminator
*/
void
snmp_mib2_set_sysdescr(const u8_t *str, const u16_t *len)
{
if (str != NULL) {
sysdescr = str;
sysdescr_len = len;
}
}
/**
* @ingroup snmp_mib2
* Initializes sysContact pointers
*
* @param ocstr if non-NULL then copy str pointer
* @param ocstrlen points to string length, excluding zero terminator.
* if set to NULL it is assumed that ocstr is NULL-terminated.
* @param bufsize size of the buffer in bytes.
* (this is required because the buffer can be overwritten by snmp-set)
* if ocstrlen is NULL buffer needs space for terminating 0 byte.
* otherwise complete buffer is used for string.
* if bufsize is set to 0, the value is regarded as read-only.
*/
void
snmp_mib2_set_syscontact(u8_t *ocstr, u16_t *ocstrlen, u16_t bufsize)
{
if (ocstr != NULL) {
syscontact = ocstr;
syscontact_wr = ocstr;
syscontact_len = ocstrlen;
syscontact_wr_len = ocstrlen;
syscontact_bufsize = bufsize;
}
}
/**
* @ingroup snmp_mib2
* see \ref snmp_mib2_set_syscontact but set pointer to readonly memory
*/
void
snmp_mib2_set_syscontact_readonly(const u8_t *ocstr, const u16_t *ocstrlen)
{
if (ocstr != NULL) {
syscontact = ocstr;
syscontact_len = ocstrlen;
syscontact_wr = NULL;
syscontact_wr_len = NULL;
syscontact_bufsize = 0;
}
}
/**
* @ingroup snmp_mib2
* Initializes sysName pointers
*
* @param ocstr if non-NULL then copy str pointer
* @param ocstrlen points to string length, excluding zero terminator.
* if set to NULL it is assumed that ocstr is NULL-terminated.
* @param bufsize size of the buffer in bytes.
* (this is required because the buffer can be overwritten by snmp-set)
* if ocstrlen is NULL buffer needs space for terminating 0 byte.
* otherwise complete buffer is used for string.
* if bufsize is set to 0, the value is regarded as read-only.
*/
void
snmp_mib2_set_sysname(u8_t *ocstr, u16_t *ocstrlen, u16_t bufsize)
{
if (ocstr != NULL) {
sysname = ocstr;
sysname_wr = ocstr;
sysname_len = ocstrlen;
sysname_wr_len = ocstrlen;
sysname_bufsize = bufsize;
}
}
/**
* @ingroup snmp_mib2
* see \ref snmp_mib2_set_sysname but set pointer to readonly memory
*/
void
snmp_mib2_set_sysname_readonly(const u8_t *ocstr, const u16_t *ocstrlen)
{
if (ocstr != NULL) {
sysname = ocstr;
sysname_len = ocstrlen;
sysname_wr = NULL;
sysname_wr_len = NULL;
sysname_bufsize = 0;
}
}
/**
* @ingroup snmp_mib2
* Initializes sysLocation pointers
*
* @param ocstr if non-NULL then copy str pointer
* @param ocstrlen points to string length, excluding zero terminator.
* if set to NULL it is assumed that ocstr is NULL-terminated.
* @param bufsize size of the buffer in bytes.
* (this is required because the buffer can be overwritten by snmp-set)
* if ocstrlen is NULL buffer needs space for terminating 0 byte.
* otherwise complete buffer is used for string.
* if bufsize is set to 0, the value is regarded as read-only.
*/
void
snmp_mib2_set_syslocation(u8_t *ocstr, u16_t *ocstrlen, u16_t bufsize)
{
if (ocstr != NULL) {
syslocation = ocstr;
syslocation_wr = ocstr;
syslocation_len = ocstrlen;
syslocation_wr_len = ocstrlen;
syslocation_bufsize = bufsize;
}
}
/**
* @ingroup snmp_mib2
* see \ref snmp_mib2_set_syslocation but set pointer to readonly memory
*/
void
snmp_mib2_set_syslocation_readonly(const u8_t *ocstr, const u16_t *ocstrlen)
{
if (ocstr != NULL) {
syslocation = ocstr;
syslocation_len = ocstrlen;
syslocation_wr = NULL;
syslocation_wr_len = NULL;
syslocation_bufsize = 0;
}
}
static s16_t
system_get_value(const struct snmp_scalar_array_node_def *node, void *value)
{
const u8_t *var = NULL;
const s16_t *var_len;
u16_t result;
switch (node->oid) {
case 1: /* sysDescr */
var = sysdescr;
var_len = (const s16_t *)sysdescr_len;
break;
case 2: { /* sysObjectID */
const struct snmp_obj_id *dev_enterprise_oid = snmp_get_device_enterprise_oid();
MEMCPY(value, dev_enterprise_oid->id, dev_enterprise_oid->len * sizeof(u32_t));
return dev_enterprise_oid->len * sizeof(u32_t);
}
case 3: /* sysUpTime */
MIB2_COPY_SYSUPTIME_TO((u32_t *)value);
return sizeof(u32_t);
case 4: /* sysContact */
var = syscontact;
var_len = (const s16_t *)syscontact_len;
break;
case 5: /* sysName */
var = sysname;
var_len = (const s16_t *)sysname_len;
break;
case 6: /* sysLocation */
var = syslocation;
var_len = (const s16_t *)syslocation_len;
break;
case 7: /* sysServices */
*(s32_t *)value = SNMP_SYSSERVICES;
return sizeof(s32_t);
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("system_get_value(): unknown id: %"S32_F"\n", node->oid));
return 0;
}
/* handle string values (OID 1,4,5 and 6) */
LWIP_ASSERT("", (value != NULL));
if (var_len == NULL) {
result = (s16_t)strlen((const char *)var);
} else {
result = *var_len;
}
MEMCPY(value, var, result);
return result;
}
static snmp_err_t
system_set_test(const struct snmp_scalar_array_node_def *node, u16_t len, void *value)
{
snmp_err_t ret = SNMP_ERR_WRONGVALUE;
const u16_t *var_bufsize = NULL;
const u16_t *var_wr_len;
LWIP_UNUSED_ARG(value);
switch (node->oid) {
case 4: /* sysContact */
var_bufsize = &syscontact_bufsize;
var_wr_len = syscontact_wr_len;
break;
case 5: /* sysName */
var_bufsize = &sysname_bufsize;
var_wr_len = sysname_wr_len;
break;
case 6: /* sysLocation */
var_bufsize = &syslocation_bufsize;
var_wr_len = syslocation_wr_len;
break;
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("system_set_test(): unknown id: %"S32_F"\n", node->oid));
return ret;
}
/* check if value is writable at all */
if (*var_bufsize > 0) {
if (var_wr_len == NULL) {
/* we have to take the terminating 0 into account */
if (len < *var_bufsize) {
ret = SNMP_ERR_NOERROR;
}
} else {
if (len <= *var_bufsize) {
ret = SNMP_ERR_NOERROR;
}
}
} else {
ret = SNMP_ERR_NOTWRITABLE;
}
return ret;
}
static snmp_err_t
system_set_value(const struct snmp_scalar_array_node_def *node, u16_t len, void *value)
{
u8_t *var_wr = NULL;
u16_t *var_wr_len;
switch (node->oid) {
case 4: /* sysContact */
var_wr = syscontact_wr;
var_wr_len = syscontact_wr_len;
break;
case 5: /* sysName */
var_wr = sysname_wr;
var_wr_len = sysname_wr_len;
break;
case 6: /* sysLocation */
var_wr = syslocation_wr;
var_wr_len = syslocation_wr_len;
break;
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("system_set_value(): unknown id: %"S32_F"\n", node->oid));
return SNMP_ERR_GENERROR;
}
/* no need to check size of target buffer, this was already done in set_test method */
LWIP_ASSERT("", var_wr != NULL);
MEMCPY(var_wr, value, len);
if (var_wr_len == NULL) {
/* add terminating 0 */
var_wr[len] = 0;
} else {
*var_wr_len = len;
}
return SNMP_ERR_NOERROR;
}
static const struct snmp_scalar_array_node_def system_nodes[] = {
{1, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY}, /* sysDescr */
{2, SNMP_ASN1_TYPE_OBJECT_ID, SNMP_NODE_INSTANCE_READ_ONLY}, /* sysObjectID */
{3, SNMP_ASN1_TYPE_TIMETICKS, SNMP_NODE_INSTANCE_READ_ONLY}, /* sysUpTime */
{4, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_WRITE}, /* sysContact */
{5, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_WRITE}, /* sysName */
{6, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_WRITE}, /* sysLocation */
{7, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY} /* sysServices */
};
const struct snmp_scalar_array_node snmp_mib2_system_node = SNMP_SCALAR_CREATE_ARRAY_NODE(1, system_nodes, system_get_value, system_set_test, system_set_value);
#endif /* LWIP_SNMP && SNMP_LWIP_MIB2 */

View File

@ -0,0 +1,607 @@
/**
* @file
* Management Information Base II (RFC1213) TCP objects and functions.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
* Christiaan Simons <christiaan.simons@axon.tv>
*/
#include "lwip/snmp.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_mib2.h"
#include "lwip/apps/snmp_table.h"
#include "lwip/apps/snmp_scalar.h"
#include "lwip/tcp.h"
#include "lwip/priv/tcp_priv.h"
#include "lwip/stats.h"
#include <string.h>
#if LWIP_SNMP && SNMP_LWIP_MIB2 && LWIP_TCP
#if SNMP_USE_NETCONN
#define SYNC_NODE_NAME(node_name) node_name ## _synced
#define CREATE_LWIP_SYNC_NODE(oid, node_name) \
static const struct snmp_threadsync_node node_name ## _synced = SNMP_CREATE_THREAD_SYNC_NODE(oid, &node_name.node, &snmp_mib2_lwip_locks);
#else
#define SYNC_NODE_NAME(node_name) node_name
#define CREATE_LWIP_SYNC_NODE(oid, node_name)
#endif
/* --- tcp .1.3.6.1.2.1.6 ----------------------------------------------------- */
static s16_t
tcp_get_value(struct snmp_node_instance *instance, void *value)
{
u32_t *uint_ptr = (u32_t *)value;
s32_t *sint_ptr = (s32_t *)value;
switch (instance->node->oid) {
case 1: /* tcpRtoAlgorithm, vanj(4) */
*sint_ptr = 4;
return sizeof(*sint_ptr);
case 2: /* tcpRtoMin */
/* @todo not the actual value, a guess,
needs to be calculated */
*sint_ptr = 1000;
return sizeof(*sint_ptr);
case 3: /* tcpRtoMax */
/* @todo not the actual value, a guess,
needs to be calculated */
*sint_ptr = 60000;
return sizeof(*sint_ptr);
case 4: /* tcpMaxConn */
*sint_ptr = MEMP_NUM_TCP_PCB;
return sizeof(*sint_ptr);
case 5: /* tcpActiveOpens */
*uint_ptr = STATS_GET(mib2.tcpactiveopens);
return sizeof(*uint_ptr);
case 6: /* tcpPassiveOpens */
*uint_ptr = STATS_GET(mib2.tcppassiveopens);
return sizeof(*uint_ptr);
case 7: /* tcpAttemptFails */
*uint_ptr = STATS_GET(mib2.tcpattemptfails);
return sizeof(*uint_ptr);
case 8: /* tcpEstabResets */
*uint_ptr = STATS_GET(mib2.tcpestabresets);
return sizeof(*uint_ptr);
case 9: { /* tcpCurrEstab */
u16_t tcpcurrestab = 0;
struct tcp_pcb *pcb = tcp_active_pcbs;
while (pcb != NULL) {
if ((pcb->state == ESTABLISHED) ||
(pcb->state == CLOSE_WAIT)) {
tcpcurrestab++;
}
pcb = pcb->next;
}
*uint_ptr = tcpcurrestab;
}
return sizeof(*uint_ptr);
case 10: /* tcpInSegs */
*uint_ptr = STATS_GET(mib2.tcpinsegs);
return sizeof(*uint_ptr);
case 11: /* tcpOutSegs */
*uint_ptr = STATS_GET(mib2.tcpoutsegs);
return sizeof(*uint_ptr);
case 12: /* tcpRetransSegs */
*uint_ptr = STATS_GET(mib2.tcpretranssegs);
return sizeof(*uint_ptr);
case 14: /* tcpInErrs */
*uint_ptr = STATS_GET(mib2.tcpinerrs);
return sizeof(*uint_ptr);
case 15: /* tcpOutRsts */
*uint_ptr = STATS_GET(mib2.tcpoutrsts);
return sizeof(*uint_ptr);
#if LWIP_HAVE_INT64
case 17: { /* tcpHCInSegs */
/* use the 32 bit counter for now... */
u64_t val64 = STATS_GET(mib2.tcpinsegs);
*((u64_t *)value) = val64;
}
return sizeof(u64_t);
case 18: { /* tcpHCOutSegs */
/* use the 32 bit counter for now... */
u64_t val64 = STATS_GET(mib2.tcpoutsegs);
*((u64_t *)value) = val64;
}
return sizeof(u64_t);
#endif
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("tcp_get_value(): unknown id: %"S32_F"\n", instance->node->oid));
break;
}
return 0;
}
/* --- tcpConnTable --- */
#if LWIP_IPV4
/* list of allowed value ranges for incoming OID */
static const struct snmp_oid_range tcp_ConnTable_oid_ranges[] = {
{ 0, 0xff }, /* IP A */
{ 0, 0xff }, /* IP B */
{ 0, 0xff }, /* IP C */
{ 0, 0xff }, /* IP D */
{ 0, 0xffff }, /* Port */
{ 0, 0xff }, /* IP A */
{ 0, 0xff }, /* IP B */
{ 0, 0xff }, /* IP C */
{ 0, 0xff }, /* IP D */
{ 0, 0xffff } /* Port */
};
static snmp_err_t
tcp_ConnTable_get_cell_value_core(struct tcp_pcb *pcb, const u32_t *column, union snmp_variant_value *value, u32_t *value_len)
{
LWIP_UNUSED_ARG(value_len);
/* value */
switch (*column) {
case 1: /* tcpConnState */
value->u32 = pcb->state + 1;
break;
case 2: /* tcpConnLocalAddress */
value->u32 = ip_2_ip4(&pcb->local_ip)->addr;
break;
case 3: /* tcpConnLocalPort */
value->u32 = pcb->local_port;
break;
case 4: /* tcpConnRemAddress */
if (pcb->state == LISTEN) {
value->u32 = IP4_ADDR_ANY4->addr;
} else {
value->u32 = ip_2_ip4(&pcb->remote_ip)->addr;
}
break;
case 5: /* tcpConnRemPort */
if (pcb->state == LISTEN) {
value->u32 = 0;
} else {
value->u32 = pcb->remote_port;
}
break;
default:
LWIP_ASSERT("invalid id", 0);
return SNMP_ERR_NOSUCHINSTANCE;
}
return SNMP_ERR_NOERROR;
}
static snmp_err_t
tcp_ConnTable_get_cell_value(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, union snmp_variant_value *value, u32_t *value_len)
{
u8_t i;
ip4_addr_t local_ip;
ip4_addr_t remote_ip;
u16_t local_port;
u16_t remote_port;
struct tcp_pcb *pcb;
/* check if incoming OID length and if values are in plausible range */
if (!snmp_oid_in_range(row_oid, row_oid_len, tcp_ConnTable_oid_ranges, LWIP_ARRAYSIZE(tcp_ConnTable_oid_ranges))) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* get IPs and ports from incoming OID */
snmp_oid_to_ip4(&row_oid[0], &local_ip); /* we know it succeeds because of oid_in_range check above */
local_port = (u16_t)row_oid[4];
snmp_oid_to_ip4(&row_oid[5], &remote_ip); /* we know it succeeds because of oid_in_range check above */
remote_port = (u16_t)row_oid[9];
/* find tcp_pcb with requested ips and ports */
for (i = 0; i < LWIP_ARRAYSIZE(tcp_pcb_lists); i++) {
pcb = *tcp_pcb_lists[i];
while (pcb != NULL) {
/* do local IP and local port match? */
if (IP_IS_V4_VAL(pcb->local_ip) &&
ip4_addr_cmp(&local_ip, ip_2_ip4(&pcb->local_ip)) && (local_port == pcb->local_port)) {
/* PCBs in state LISTEN are not connected and have no remote_ip or remote_port */
if (pcb->state == LISTEN) {
if (ip4_addr_cmp(&remote_ip, IP4_ADDR_ANY4) && (remote_port == 0)) {
/* fill in object properties */
return tcp_ConnTable_get_cell_value_core(pcb, column, value, value_len);
}
} else {
if (IP_IS_V4_VAL(pcb->remote_ip) &&
ip4_addr_cmp(&remote_ip, ip_2_ip4(&pcb->remote_ip)) && (remote_port == pcb->remote_port)) {
/* fill in object properties */
return tcp_ConnTable_get_cell_value_core(pcb, column, value, value_len);
}
}
}
pcb = pcb->next;
}
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
tcp_ConnTable_get_next_cell_instance_and_value(const u32_t *column, struct snmp_obj_id *row_oid, union snmp_variant_value *value, u32_t *value_len)
{
u8_t i;
struct tcp_pcb *pcb;
struct snmp_next_oid_state state;
u32_t result_temp[LWIP_ARRAYSIZE(tcp_ConnTable_oid_ranges)];
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(tcp_ConnTable_oid_ranges));
/* iterate over all possible OIDs to find the next one */
for (i = 0; i < LWIP_ARRAYSIZE(tcp_pcb_lists); i++) {
pcb = *tcp_pcb_lists[i];
while (pcb != NULL) {
u32_t test_oid[LWIP_ARRAYSIZE(tcp_ConnTable_oid_ranges)];
if (IP_IS_V4_VAL(pcb->local_ip)) {
snmp_ip4_to_oid(ip_2_ip4(&pcb->local_ip), &test_oid[0]);
test_oid[4] = pcb->local_port;
/* PCBs in state LISTEN are not connected and have no remote_ip or remote_port */
if (pcb->state == LISTEN) {
snmp_ip4_to_oid(IP4_ADDR_ANY4, &test_oid[5]);
test_oid[9] = 0;
} else {
if (IP_IS_V6_VAL(pcb->remote_ip)) { /* should never happen */
continue;
}
snmp_ip4_to_oid(ip_2_ip4(&pcb->remote_ip), &test_oid[5]);
test_oid[9] = pcb->remote_port;
}
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, LWIP_ARRAYSIZE(tcp_ConnTable_oid_ranges), pcb);
}
pcb = pcb->next;
}
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* fill in object properties */
return tcp_ConnTable_get_cell_value_core((struct tcp_pcb *)state.reference, column, value, value_len);
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
#endif /* LWIP_IPV4 */
/* --- tcpConnectionTable --- */
static snmp_err_t
tcp_ConnectionTable_get_cell_value_core(const u32_t *column, struct tcp_pcb *pcb, union snmp_variant_value *value)
{
/* all items except tcpConnectionState and tcpConnectionProcess are declared as not-accessible */
switch (*column) {
case 7: /* tcpConnectionState */
value->u32 = pcb->state + 1;
break;
case 8: /* tcpConnectionProcess */
value->u32 = 0; /* not supported */
break;
default:
return SNMP_ERR_NOSUCHINSTANCE;
}
return SNMP_ERR_NOERROR;
}
static snmp_err_t
tcp_ConnectionTable_get_cell_value(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, union snmp_variant_value *value, u32_t *value_len)
{
ip_addr_t local_ip, remote_ip;
u16_t local_port, remote_port;
struct tcp_pcb *pcb;
u8_t idx = 0;
u8_t i;
struct tcp_pcb **const tcp_pcb_nonlisten_lists[] = {&tcp_bound_pcbs, &tcp_active_pcbs, &tcp_tw_pcbs};
LWIP_UNUSED_ARG(value_len);
/* tcpConnectionLocalAddressType + tcpConnectionLocalAddress + tcpConnectionLocalPort */
idx += snmp_oid_to_ip_port(&row_oid[idx], row_oid_len - idx, &local_ip, &local_port);
if (idx == 0) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* tcpConnectionRemAddressType + tcpConnectionRemAddress + tcpConnectionRemPort */
idx += snmp_oid_to_ip_port(&row_oid[idx], row_oid_len - idx, &remote_ip, &remote_port);
if (idx == 0) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* find tcp_pcb with requested ip and port*/
for (i = 0; i < LWIP_ARRAYSIZE(tcp_pcb_nonlisten_lists); i++) {
pcb = *tcp_pcb_nonlisten_lists[i];
while (pcb != NULL) {
if (ip_addr_cmp(&local_ip, &pcb->local_ip) &&
(local_port == pcb->local_port) &&
ip_addr_cmp(&remote_ip, &pcb->remote_ip) &&
(remote_port == pcb->remote_port)) {
/* fill in object properties */
return tcp_ConnectionTable_get_cell_value_core(column, pcb, value);
}
pcb = pcb->next;
}
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
tcp_ConnectionTable_get_next_cell_instance_and_value(const u32_t *column, struct snmp_obj_id *row_oid, union snmp_variant_value *value, u32_t *value_len)
{
struct tcp_pcb *pcb;
struct snmp_next_oid_state state;
/* 1x tcpConnectionLocalAddressType + 1x OID len + 16x tcpConnectionLocalAddress + 1x tcpConnectionLocalPort
* 1x tcpConnectionRemAddressType + 1x OID len + 16x tcpConnectionRemAddress + 1x tcpConnectionRemPort */
u32_t result_temp[38];
u8_t i;
struct tcp_pcb **const tcp_pcb_nonlisten_lists[] = {&tcp_bound_pcbs, &tcp_active_pcbs, &tcp_tw_pcbs};
LWIP_UNUSED_ARG(value_len);
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(result_temp));
/* iterate over all possible OIDs to find the next one */
for (i = 0; i < LWIP_ARRAYSIZE(tcp_pcb_nonlisten_lists); i++) {
pcb = *tcp_pcb_nonlisten_lists[i];
while (pcb != NULL) {
u8_t idx = 0;
u32_t test_oid[LWIP_ARRAYSIZE(result_temp)];
/* tcpConnectionLocalAddressType + tcpConnectionLocalAddress + tcpConnectionLocalPort */
idx += snmp_ip_port_to_oid(&pcb->local_ip, pcb->local_port, &test_oid[idx]);
/* tcpConnectionRemAddressType + tcpConnectionRemAddress + tcpConnectionRemPort */
idx += snmp_ip_port_to_oid(&pcb->remote_ip, pcb->remote_port, &test_oid[idx]);
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, idx, pcb);
pcb = pcb->next;
}
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* fill in object properties */
return tcp_ConnectionTable_get_cell_value_core(column, (struct tcp_pcb *)state.reference, value);
} else {
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
}
/* --- tcpListenerTable --- */
static snmp_err_t
tcp_ListenerTable_get_cell_value_core(const u32_t *column, union snmp_variant_value *value)
{
/* all items except tcpListenerProcess are declared as not-accessible */
switch (*column) {
case 4: /* tcpListenerProcess */
value->u32 = 0; /* not supported */
break;
default:
return SNMP_ERR_NOSUCHINSTANCE;
}
return SNMP_ERR_NOERROR;
}
static snmp_err_t
tcp_ListenerTable_get_cell_value(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, union snmp_variant_value *value, u32_t *value_len)
{
ip_addr_t local_ip;
u16_t local_port;
struct tcp_pcb_listen *pcb;
u8_t idx = 0;
LWIP_UNUSED_ARG(value_len);
/* tcpListenerLocalAddressType + tcpListenerLocalAddress + tcpListenerLocalPort */
idx += snmp_oid_to_ip_port(&row_oid[idx], row_oid_len - idx, &local_ip, &local_port);
if (idx == 0) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* find tcp_pcb with requested ip and port*/
pcb = tcp_listen_pcbs.listen_pcbs;
while (pcb != NULL) {
if (ip_addr_cmp(&local_ip, &pcb->local_ip) &&
(local_port == pcb->local_port)) {
/* fill in object properties */
return tcp_ListenerTable_get_cell_value_core(column, value);
}
pcb = pcb->next;
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
tcp_ListenerTable_get_next_cell_instance_and_value(const u32_t *column, struct snmp_obj_id *row_oid, union snmp_variant_value *value, u32_t *value_len)
{
struct tcp_pcb_listen *pcb;
struct snmp_next_oid_state state;
/* 1x tcpListenerLocalAddressType + 1x OID len + 16x tcpListenerLocalAddress + 1x tcpListenerLocalPort */
u32_t result_temp[19];
LWIP_UNUSED_ARG(value_len);
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(result_temp));
/* iterate over all possible OIDs to find the next one */
pcb = tcp_listen_pcbs.listen_pcbs;
while (pcb != NULL) {
u8_t idx = 0;
u32_t test_oid[LWIP_ARRAYSIZE(result_temp)];
/* tcpListenerLocalAddressType + tcpListenerLocalAddress + tcpListenerLocalPort */
idx += snmp_ip_port_to_oid(&pcb->local_ip, pcb->local_port, &test_oid[idx]);
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, idx, NULL);
pcb = pcb->next;
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* fill in object properties */
return tcp_ListenerTable_get_cell_value_core(column, value);
} else {
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
}
static const struct snmp_scalar_node tcp_RtoAlgorithm = SNMP_SCALAR_CREATE_NODE_READONLY(1, SNMP_ASN1_TYPE_INTEGER, tcp_get_value);
static const struct snmp_scalar_node tcp_RtoMin = SNMP_SCALAR_CREATE_NODE_READONLY(2, SNMP_ASN1_TYPE_INTEGER, tcp_get_value);
static const struct snmp_scalar_node tcp_RtoMax = SNMP_SCALAR_CREATE_NODE_READONLY(3, SNMP_ASN1_TYPE_INTEGER, tcp_get_value);
static const struct snmp_scalar_node tcp_MaxConn = SNMP_SCALAR_CREATE_NODE_READONLY(4, SNMP_ASN1_TYPE_INTEGER, tcp_get_value);
static const struct snmp_scalar_node tcp_ActiveOpens = SNMP_SCALAR_CREATE_NODE_READONLY(5, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
static const struct snmp_scalar_node tcp_PassiveOpens = SNMP_SCALAR_CREATE_NODE_READONLY(6, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
static const struct snmp_scalar_node tcp_AttemptFails = SNMP_SCALAR_CREATE_NODE_READONLY(7, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
static const struct snmp_scalar_node tcp_EstabResets = SNMP_SCALAR_CREATE_NODE_READONLY(8, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
static const struct snmp_scalar_node tcp_CurrEstab = SNMP_SCALAR_CREATE_NODE_READONLY(9, SNMP_ASN1_TYPE_GAUGE, tcp_get_value);
static const struct snmp_scalar_node tcp_InSegs = SNMP_SCALAR_CREATE_NODE_READONLY(10, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
static const struct snmp_scalar_node tcp_OutSegs = SNMP_SCALAR_CREATE_NODE_READONLY(11, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
static const struct snmp_scalar_node tcp_RetransSegs = SNMP_SCALAR_CREATE_NODE_READONLY(12, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
static const struct snmp_scalar_node tcp_InErrs = SNMP_SCALAR_CREATE_NODE_READONLY(14, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
static const struct snmp_scalar_node tcp_OutRsts = SNMP_SCALAR_CREATE_NODE_READONLY(15, SNMP_ASN1_TYPE_COUNTER, tcp_get_value);
#if LWIP_HAVE_INT64
static const struct snmp_scalar_node tcp_HCInSegs = SNMP_SCALAR_CREATE_NODE_READONLY(17, SNMP_ASN1_TYPE_COUNTER64, tcp_get_value);
static const struct snmp_scalar_node tcp_HCOutSegs = SNMP_SCALAR_CREATE_NODE_READONLY(18, SNMP_ASN1_TYPE_COUNTER64, tcp_get_value);
#endif
#if LWIP_IPV4
static const struct snmp_table_simple_col_def tcp_ConnTable_columns[] = {
{ 1, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* tcpConnState */
{ 2, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* tcpConnLocalAddress */
{ 3, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* tcpConnLocalPort */
{ 4, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* tcpConnRemAddress */
{ 5, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 } /* tcpConnRemPort */
};
static const struct snmp_table_simple_node tcp_ConnTable = SNMP_TABLE_CREATE_SIMPLE(13, tcp_ConnTable_columns, tcp_ConnTable_get_cell_value, tcp_ConnTable_get_next_cell_instance_and_value);
#endif /* LWIP_IPV4 */
static const struct snmp_table_simple_col_def tcp_ConnectionTable_columns[] = {
/* all items except tcpConnectionState and tcpConnectionProcess are declared as not-accessible */
{ 7, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 }, /* tcpConnectionState */
{ 8, SNMP_ASN1_TYPE_UNSIGNED32, SNMP_VARIANT_VALUE_TYPE_U32 } /* tcpConnectionProcess */
};
static const struct snmp_table_simple_node tcp_ConnectionTable = SNMP_TABLE_CREATE_SIMPLE(19, tcp_ConnectionTable_columns, tcp_ConnectionTable_get_cell_value, tcp_ConnectionTable_get_next_cell_instance_and_value);
static const struct snmp_table_simple_col_def tcp_ListenerTable_columns[] = {
/* all items except tcpListenerProcess are declared as not-accessible */
{ 4, SNMP_ASN1_TYPE_UNSIGNED32, SNMP_VARIANT_VALUE_TYPE_U32 } /* tcpListenerProcess */
};
static const struct snmp_table_simple_node tcp_ListenerTable = SNMP_TABLE_CREATE_SIMPLE(20, tcp_ListenerTable_columns, tcp_ListenerTable_get_cell_value, tcp_ListenerTable_get_next_cell_instance_and_value);
/* the following nodes access variables in LWIP stack from SNMP worker thread and must therefore be synced to LWIP (TCPIP) thread */
CREATE_LWIP_SYNC_NODE( 1, tcp_RtoAlgorithm)
CREATE_LWIP_SYNC_NODE( 2, tcp_RtoMin)
CREATE_LWIP_SYNC_NODE( 3, tcp_RtoMax)
CREATE_LWIP_SYNC_NODE( 4, tcp_MaxConn)
CREATE_LWIP_SYNC_NODE( 5, tcp_ActiveOpens)
CREATE_LWIP_SYNC_NODE( 6, tcp_PassiveOpens)
CREATE_LWIP_SYNC_NODE( 7, tcp_AttemptFails)
CREATE_LWIP_SYNC_NODE( 8, tcp_EstabResets)
CREATE_LWIP_SYNC_NODE( 9, tcp_CurrEstab)
CREATE_LWIP_SYNC_NODE(10, tcp_InSegs)
CREATE_LWIP_SYNC_NODE(11, tcp_OutSegs)
CREATE_LWIP_SYNC_NODE(12, tcp_RetransSegs)
#if LWIP_IPV4
CREATE_LWIP_SYNC_NODE(13, tcp_ConnTable)
#endif /* LWIP_IPV4 */
CREATE_LWIP_SYNC_NODE(14, tcp_InErrs)
CREATE_LWIP_SYNC_NODE(15, tcp_OutRsts)
#if LWIP_HAVE_INT64
CREATE_LWIP_SYNC_NODE(17, tcp_HCInSegs)
CREATE_LWIP_SYNC_NODE(18, tcp_HCOutSegs)
#endif
CREATE_LWIP_SYNC_NODE(19, tcp_ConnectionTable)
CREATE_LWIP_SYNC_NODE(20, tcp_ListenerTable)
static const struct snmp_node *const tcp_nodes[] = {
&SYNC_NODE_NAME(tcp_RtoAlgorithm).node.node,
&SYNC_NODE_NAME(tcp_RtoMin).node.node,
&SYNC_NODE_NAME(tcp_RtoMax).node.node,
&SYNC_NODE_NAME(tcp_MaxConn).node.node,
&SYNC_NODE_NAME(tcp_ActiveOpens).node.node,
&SYNC_NODE_NAME(tcp_PassiveOpens).node.node,
&SYNC_NODE_NAME(tcp_AttemptFails).node.node,
&SYNC_NODE_NAME(tcp_EstabResets).node.node,
&SYNC_NODE_NAME(tcp_CurrEstab).node.node,
&SYNC_NODE_NAME(tcp_InSegs).node.node,
&SYNC_NODE_NAME(tcp_OutSegs).node.node,
&SYNC_NODE_NAME(tcp_RetransSegs).node.node,
#if LWIP_IPV4
&SYNC_NODE_NAME(tcp_ConnTable).node.node,
#endif /* LWIP_IPV4 */
&SYNC_NODE_NAME(tcp_InErrs).node.node,
&SYNC_NODE_NAME(tcp_OutRsts).node.node,
&SYNC_NODE_NAME(tcp_HCInSegs).node.node,
#if LWIP_HAVE_INT64
&SYNC_NODE_NAME(tcp_HCOutSegs).node.node,
&SYNC_NODE_NAME(tcp_ConnectionTable).node.node,
#endif
&SYNC_NODE_NAME(tcp_ListenerTable).node.node
};
const struct snmp_tree_node snmp_mib2_tcp_root = SNMP_CREATE_TREE_NODE(6, tcp_nodes);
#endif /* LWIP_SNMP && SNMP_LWIP_MIB2 && LWIP_TCP */

View File

@ -0,0 +1,372 @@
/**
* @file
* Management Information Base II (RFC1213) UDP objects and functions.
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
* Christiaan Simons <christiaan.simons@axon.tv>
*/
#include "lwip/snmp.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_mib2.h"
#include "lwip/apps/snmp_table.h"
#include "lwip/apps/snmp_scalar.h"
#include "lwip/udp.h"
#include "lwip/stats.h"
#include <string.h>
#if LWIP_SNMP && SNMP_LWIP_MIB2 && LWIP_UDP
#if SNMP_USE_NETCONN
#define SYNC_NODE_NAME(node_name) node_name ## _synced
#define CREATE_LWIP_SYNC_NODE(oid, node_name) \
static const struct snmp_threadsync_node node_name ## _synced = SNMP_CREATE_THREAD_SYNC_NODE(oid, &node_name.node, &snmp_mib2_lwip_locks);
#else
#define SYNC_NODE_NAME(node_name) node_name
#define CREATE_LWIP_SYNC_NODE(oid, node_name)
#endif
/* --- udp .1.3.6.1.2.1.7 ----------------------------------------------------- */
static s16_t
udp_get_value(struct snmp_node_instance *instance, void *value)
{
u32_t *uint_ptr = (u32_t *)value;
switch (instance->node->oid) {
case 1: /* udpInDatagrams */
*uint_ptr = STATS_GET(mib2.udpindatagrams);
return sizeof(*uint_ptr);
case 2: /* udpNoPorts */
*uint_ptr = STATS_GET(mib2.udpnoports);
return sizeof(*uint_ptr);
case 3: /* udpInErrors */
*uint_ptr = STATS_GET(mib2.udpinerrors);
return sizeof(*uint_ptr);
case 4: /* udpOutDatagrams */
*uint_ptr = STATS_GET(mib2.udpoutdatagrams);
return sizeof(*uint_ptr);
#if LWIP_HAVE_INT64
case 8: { /* udpHCInDatagrams */
/* use the 32 bit counter for now... */
u64_t val64 = STATS_GET(mib2.udpindatagrams);
*((u64_t *)value) = val64;
}
return sizeof(u64_t);
case 9: { /* udpHCOutDatagrams */
/* use the 32 bit counter for now... */
u64_t val64 = STATS_GET(mib2.udpoutdatagrams);
*((u64_t *)value) = val64;
}
return sizeof(u64_t);
#endif
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("udp_get_value(): unknown id: %"S32_F"\n", instance->node->oid));
break;
}
return 0;
}
/* --- udpEndpointTable --- */
static snmp_err_t
udp_endpointTable_get_cell_value_core(const u32_t *column, union snmp_variant_value *value)
{
/* all items except udpEndpointProcess are declared as not-accessible */
switch (*column) {
case 8: /* udpEndpointProcess */
value->u32 = 0; /* not supported */
break;
default:
return SNMP_ERR_NOSUCHINSTANCE;
}
return SNMP_ERR_NOERROR;
}
static snmp_err_t
udp_endpointTable_get_cell_value(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, union snmp_variant_value *value, u32_t *value_len)
{
ip_addr_t local_ip, remote_ip;
u16_t local_port, remote_port;
struct udp_pcb *pcb;
u8_t idx = 0;
LWIP_UNUSED_ARG(value_len);
/* udpEndpointLocalAddressType + udpEndpointLocalAddress + udpEndpointLocalPort */
idx += snmp_oid_to_ip_port(&row_oid[idx], row_oid_len - idx, &local_ip, &local_port);
if (idx == 0) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* udpEndpointRemoteAddressType + udpEndpointRemoteAddress + udpEndpointRemotePort */
idx += snmp_oid_to_ip_port(&row_oid[idx], row_oid_len - idx, &remote_ip, &remote_port);
if (idx == 0) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* udpEndpointInstance */
if (row_oid_len < (idx + 1)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
if (row_oid[idx] != 0) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* find udp_pcb with requested ip and port*/
pcb = udp_pcbs;
while (pcb != NULL) {
if (ip_addr_cmp(&local_ip, &pcb->local_ip) &&
(local_port == pcb->local_port) &&
ip_addr_cmp(&remote_ip, &pcb->remote_ip) &&
(remote_port == pcb->remote_port)) {
/* fill in object properties */
return udp_endpointTable_get_cell_value_core(column, value);
}
pcb = pcb->next;
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
udp_endpointTable_get_next_cell_instance_and_value(const u32_t *column, struct snmp_obj_id *row_oid, union snmp_variant_value *value, u32_t *value_len)
{
struct udp_pcb *pcb;
struct snmp_next_oid_state state;
/* 1x udpEndpointLocalAddressType + 1x OID len + 16x udpEndpointLocalAddress + 1x udpEndpointLocalPort +
* 1x udpEndpointRemoteAddressType + 1x OID len + 16x udpEndpointRemoteAddress + 1x udpEndpointRemotePort +
* 1x udpEndpointInstance = 39
*/
u32_t result_temp[39];
LWIP_UNUSED_ARG(value_len);
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(result_temp));
/* iterate over all possible OIDs to find the next one */
pcb = udp_pcbs;
while (pcb != NULL) {
u32_t test_oid[LWIP_ARRAYSIZE(result_temp)];
u8_t idx = 0;
/* udpEndpointLocalAddressType + udpEndpointLocalAddress + udpEndpointLocalPort */
idx += snmp_ip_port_to_oid(&pcb->local_ip, pcb->local_port, &test_oid[idx]);
/* udpEndpointRemoteAddressType + udpEndpointRemoteAddress + udpEndpointRemotePort */
idx += snmp_ip_port_to_oid(&pcb->remote_ip, pcb->remote_port, &test_oid[idx]);
test_oid[idx] = 0; /* udpEndpointInstance */
idx++;
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, idx, NULL);
pcb = pcb->next;
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* fill in object properties */
return udp_endpointTable_get_cell_value_core(column, value);
} else {
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
}
/* --- udpTable --- */
#if LWIP_IPV4
/* list of allowed value ranges for incoming OID */
static const struct snmp_oid_range udp_Table_oid_ranges[] = {
{ 0, 0xff }, /* IP A */
{ 0, 0xff }, /* IP B */
{ 0, 0xff }, /* IP C */
{ 0, 0xff }, /* IP D */
{ 1, 0xffff } /* Port */
};
static snmp_err_t
udp_Table_get_cell_value_core(struct udp_pcb *pcb, const u32_t *column, union snmp_variant_value *value, u32_t *value_len)
{
LWIP_UNUSED_ARG(value_len);
switch (*column) {
case 1: /* udpLocalAddress */
/* set reference to PCB local IP and return a generic node that copies IP4 addresses */
value->u32 = ip_2_ip4(&pcb->local_ip)->addr;
break;
case 2: /* udpLocalPort */
/* set reference to PCB local port and return a generic node that copies u16_t values */
value->u32 = pcb->local_port;
break;
default:
return SNMP_ERR_NOSUCHINSTANCE;
}
return SNMP_ERR_NOERROR;
}
static snmp_err_t
udp_Table_get_cell_value(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, union snmp_variant_value *value, u32_t *value_len)
{
ip4_addr_t ip;
u16_t port;
struct udp_pcb *pcb;
/* check if incoming OID length and if values are in plausible range */
if (!snmp_oid_in_range(row_oid, row_oid_len, udp_Table_oid_ranges, LWIP_ARRAYSIZE(udp_Table_oid_ranges))) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* get IP and port from incoming OID */
snmp_oid_to_ip4(&row_oid[0], &ip); /* we know it succeeds because of oid_in_range check above */
port = (u16_t)row_oid[4];
/* find udp_pcb with requested ip and port*/
pcb = udp_pcbs;
while (pcb != NULL) {
if (IP_IS_V4_VAL(pcb->local_ip)) {
if (ip4_addr_cmp(&ip, ip_2_ip4(&pcb->local_ip)) && (port == pcb->local_port)) {
/* fill in object properties */
return udp_Table_get_cell_value_core(pcb, column, value, value_len);
}
}
pcb = pcb->next;
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static snmp_err_t
udp_Table_get_next_cell_instance_and_value(const u32_t *column, struct snmp_obj_id *row_oid, union snmp_variant_value *value, u32_t *value_len)
{
struct udp_pcb *pcb;
struct snmp_next_oid_state state;
u32_t result_temp[LWIP_ARRAYSIZE(udp_Table_oid_ranges)];
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(udp_Table_oid_ranges));
/* iterate over all possible OIDs to find the next one */
pcb = udp_pcbs;
while (pcb != NULL) {
u32_t test_oid[LWIP_ARRAYSIZE(udp_Table_oid_ranges)];
if (IP_IS_V4_VAL(pcb->local_ip)) {
snmp_ip4_to_oid(ip_2_ip4(&pcb->local_ip), &test_oid[0]);
test_oid[4] = pcb->local_port;
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, LWIP_ARRAYSIZE(udp_Table_oid_ranges), pcb);
}
pcb = pcb->next;
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* fill in object properties */
return udp_Table_get_cell_value_core((struct udp_pcb *)state.reference, column, value, value_len);
} else {
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
}
#endif /* LWIP_IPV4 */
static const struct snmp_scalar_node udp_inDatagrams = SNMP_SCALAR_CREATE_NODE_READONLY(1, SNMP_ASN1_TYPE_COUNTER, udp_get_value);
static const struct snmp_scalar_node udp_noPorts = SNMP_SCALAR_CREATE_NODE_READONLY(2, SNMP_ASN1_TYPE_COUNTER, udp_get_value);
static const struct snmp_scalar_node udp_inErrors = SNMP_SCALAR_CREATE_NODE_READONLY(3, SNMP_ASN1_TYPE_COUNTER, udp_get_value);
static const struct snmp_scalar_node udp_outDatagrams = SNMP_SCALAR_CREATE_NODE_READONLY(4, SNMP_ASN1_TYPE_COUNTER, udp_get_value);
#if LWIP_HAVE_INT64
static const struct snmp_scalar_node udp_HCInDatagrams = SNMP_SCALAR_CREATE_NODE_READONLY(8, SNMP_ASN1_TYPE_COUNTER64, udp_get_value);
static const struct snmp_scalar_node udp_HCOutDatagrams = SNMP_SCALAR_CREATE_NODE_READONLY(9, SNMP_ASN1_TYPE_COUNTER64, udp_get_value);
#endif
#if LWIP_IPV4
static const struct snmp_table_simple_col_def udp_Table_columns[] = {
{ 1, SNMP_ASN1_TYPE_IPADDR, SNMP_VARIANT_VALUE_TYPE_U32 }, /* udpLocalAddress */
{ 2, SNMP_ASN1_TYPE_INTEGER, SNMP_VARIANT_VALUE_TYPE_U32 } /* udpLocalPort */
};
static const struct snmp_table_simple_node udp_Table = SNMP_TABLE_CREATE_SIMPLE(5, udp_Table_columns, udp_Table_get_cell_value, udp_Table_get_next_cell_instance_and_value);
#endif /* LWIP_IPV4 */
static const struct snmp_table_simple_col_def udp_endpointTable_columns[] = {
/* all items except udpEndpointProcess are declared as not-accessible */
{ 8, SNMP_ASN1_TYPE_UNSIGNED32, SNMP_VARIANT_VALUE_TYPE_U32 } /* udpEndpointProcess */
};
static const struct snmp_table_simple_node udp_endpointTable = SNMP_TABLE_CREATE_SIMPLE(7, udp_endpointTable_columns, udp_endpointTable_get_cell_value, udp_endpointTable_get_next_cell_instance_and_value);
/* the following nodes access variables in LWIP stack from SNMP worker thread and must therefore be synced to LWIP (TCPIP) thread */
CREATE_LWIP_SYNC_NODE(1, udp_inDatagrams)
CREATE_LWIP_SYNC_NODE(2, udp_noPorts)
CREATE_LWIP_SYNC_NODE(3, udp_inErrors)
CREATE_LWIP_SYNC_NODE(4, udp_outDatagrams)
#if LWIP_IPV4
CREATE_LWIP_SYNC_NODE(5, udp_Table)
#endif /* LWIP_IPV4 */
CREATE_LWIP_SYNC_NODE(7, udp_endpointTable)
#if LWIP_HAVE_INT64
CREATE_LWIP_SYNC_NODE(8, udp_HCInDatagrams)
CREATE_LWIP_SYNC_NODE(9, udp_HCOutDatagrams)
#endif
static const struct snmp_node *const udp_nodes[] = {
&SYNC_NODE_NAME(udp_inDatagrams).node.node,
&SYNC_NODE_NAME(udp_noPorts).node.node,
&SYNC_NODE_NAME(udp_inErrors).node.node,
&SYNC_NODE_NAME(udp_outDatagrams).node.node,
#if LWIP_IPV4
&SYNC_NODE_NAME(udp_Table).node.node,
#endif /* LWIP_IPV4 */
&SYNC_NODE_NAME(udp_endpointTable).node.node
#if LWIP_HAVE_INT64
,
&SYNC_NODE_NAME(udp_HCInDatagrams).node.node,
&SYNC_NODE_NAME(udp_HCOutDatagrams).node.node
#endif
};
const struct snmp_tree_node snmp_mib2_udp_root = SNMP_CREATE_TREE_NODE(7, udp_nodes);
#endif /* LWIP_SNMP && SNMP_LWIP_MIB2 && LWIP_UDP */

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,185 @@
/**
* @file
* SNMP Agent message handling structures (internal API, do not use in client code).
*/
/*
* Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
* Copyright (c) 2016 Elias Oenal.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Christiaan Simons <christiaan.simons@axon.tv>
* Martin Hentschel <info@cl-soft.de>
* Elias Oenal <lwip@eliasoenal.com>
*/
#ifndef LWIP_HDR_APPS_SNMP_MSG_H
#define LWIP_HDR_APPS_SNMP_MSG_H
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "snmp_pbuf_stream.h"
#include "lwip/ip_addr.h"
#include "lwip/err.h"
#if LWIP_SNMP_V3
#include "snmpv3_priv.h"
#endif
#ifdef __cplusplus
extern "C" {
#endif
/* version defines used in PDU */
#define SNMP_VERSION_1 0
#define SNMP_VERSION_2c 1
#define SNMP_VERSION_3 3
struct snmp_varbind_enumerator {
struct snmp_pbuf_stream pbuf_stream;
u16_t varbind_count;
};
typedef enum {
SNMP_VB_ENUMERATOR_ERR_OK = 0,
SNMP_VB_ENUMERATOR_ERR_EOVB = 1,
SNMP_VB_ENUMERATOR_ERR_ASN1ERROR = 2,
SNMP_VB_ENUMERATOR_ERR_INVALIDLENGTH = 3
} snmp_vb_enumerator_err_t;
void snmp_vb_enumerator_init(struct snmp_varbind_enumerator *enumerator, struct pbuf *p, u16_t offset, u16_t length);
snmp_vb_enumerator_err_t snmp_vb_enumerator_get_next(struct snmp_varbind_enumerator *enumerator, struct snmp_varbind *varbind);
struct snmp_request {
/* Communication handle */
void *handle;
/* source IP address */
const ip_addr_t *source_ip;
/* source UDP port */
u16_t source_port;
/* incoming snmp version */
u8_t version;
/* community name (zero terminated) */
u8_t community[SNMP_MAX_COMMUNITY_STR_LEN + 1];
/* community string length (exclusive zero term) */
u16_t community_strlen;
/* request type */
u8_t request_type;
/* request ID */
s32_t request_id;
/* error status */
s32_t error_status;
/* error index */
s32_t error_index;
/* non-repeaters (getBulkRequest (SNMPv2c)) */
s32_t non_repeaters;
/* max-repetitions (getBulkRequest (SNMPv2c)) */
s32_t max_repetitions;
/* Usually response-pdu (2). When snmpv3 errors are detected report-pdu(8) */
u8_t request_out_type;
#if LWIP_SNMP_V3
s32_t msg_id;
s32_t msg_max_size;
u8_t msg_flags;
s32_t msg_security_model;
u8_t msg_authoritative_engine_id[SNMP_V3_MAX_ENGINE_ID_LENGTH];
u8_t msg_authoritative_engine_id_len;
s32_t msg_authoritative_engine_boots;
s32_t msg_authoritative_engine_time;
u8_t msg_user_name[SNMP_V3_MAX_USER_LENGTH];
u8_t msg_user_name_len;
u8_t msg_authentication_parameters[SNMP_V3_MAX_AUTH_PARAM_LENGTH];
u8_t msg_authentication_parameters_len;
u8_t msg_privacy_parameters[SNMP_V3_MAX_PRIV_PARAM_LENGTH];
u8_t msg_privacy_parameters_len;
u8_t context_engine_id[SNMP_V3_MAX_ENGINE_ID_LENGTH];
u8_t context_engine_id_len;
u8_t context_name[SNMP_V3_MAX_ENGINE_ID_LENGTH];
u8_t context_name_len;
#endif
struct pbuf *inbound_pbuf;
struct snmp_varbind_enumerator inbound_varbind_enumerator;
u16_t inbound_varbind_offset;
u16_t inbound_varbind_len;
u16_t inbound_padding_len;
struct pbuf *outbound_pbuf;
struct snmp_pbuf_stream outbound_pbuf_stream;
u16_t outbound_pdu_offset;
u16_t outbound_error_status_offset;
u16_t outbound_error_index_offset;
u16_t outbound_varbind_offset;
#if LWIP_SNMP_V3
u16_t outbound_msg_global_data_offset;
u16_t outbound_msg_global_data_end;
u16_t outbound_msg_security_parameters_str_offset;
u16_t outbound_msg_security_parameters_seq_offset;
u16_t outbound_msg_security_parameters_end;
u16_t outbound_msg_authentication_parameters_offset;
u16_t outbound_scoped_pdu_seq_offset;
u16_t outbound_scoped_pdu_string_offset;
#endif
u8_t value_buffer[SNMP_MAX_VALUE_SIZE];
};
/** A helper struct keeping length information about varbinds */
struct snmp_varbind_len {
u8_t vb_len_len;
u16_t vb_value_len;
u8_t oid_len_len;
u16_t oid_value_len;
u8_t value_len_len;
u16_t value_value_len;
};
/** Agent community string */
extern const char *snmp_community;
/** Agent community string for write access */
extern const char *snmp_community_write;
/** handle for sending traps */
extern void *snmp_traps_handle;
void snmp_receive(void *handle, struct pbuf *p, const ip_addr_t *source_ip, u16_t port);
err_t snmp_sendto(void *handle, struct pbuf *p, const ip_addr_t *dst, u16_t port);
u8_t snmp_get_local_ip_for_dst(void *handle, const ip_addr_t *dst, ip_addr_t *result);
err_t snmp_varbind_length(struct snmp_varbind *varbind, struct snmp_varbind_len *len);
err_t snmp_append_outbound_varbind(struct snmp_pbuf_stream *pbuf_stream, struct snmp_varbind *varbind);
#ifdef __cplusplus
}
#endif
#endif /* LWIP_SNMP */
#endif /* LWIP_HDR_APPS_SNMP_MSG_H */

View File

@ -0,0 +1,123 @@
/**
* @file
* SNMP netconn frontend.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP && SNMP_USE_NETCONN
#include <string.h>
#include "lwip/api.h"
#include "lwip/ip.h"
#include "lwip/udp.h"
#include "snmp_msg.h"
#include "lwip/sys.h"
#include "lwip/prot/iana.h"
/** SNMP netconn API worker thread */
static void
snmp_netconn_thread(void *arg)
{
struct netconn *conn;
struct netbuf *buf;
err_t err;
LWIP_UNUSED_ARG(arg);
/* Bind to SNMP port with default IP address */
#if LWIP_IPV6
conn = netconn_new(NETCONN_UDP_IPV6);
netconn_bind(conn, IP6_ADDR_ANY, LWIP_IANA_PORT_SNMP);
#else /* LWIP_IPV6 */
conn = netconn_new(NETCONN_UDP);
netconn_bind(conn, IP4_ADDR_ANY, LWIP_IANA_PORT_SNMP);
#endif /* LWIP_IPV6 */
LWIP_ERROR("snmp_netconn: invalid conn", (conn != NULL), return;);
snmp_traps_handle = conn;
do {
err = netconn_recv(conn, &buf);
if (err == ERR_OK) {
snmp_receive(conn, buf->p, &buf->addr, buf->port);
}
if (buf != NULL) {
netbuf_delete(buf);
}
} while (1);
}
err_t
snmp_sendto(void *handle, struct pbuf *p, const ip_addr_t *dst, u16_t port)
{
err_t result;
struct netbuf buf;
memset(&buf, 0, sizeof(buf));
buf.p = p;
result = netconn_sendto((struct netconn *)handle, &buf, dst, port);
return result;
}
u8_t
snmp_get_local_ip_for_dst(void *handle, const ip_addr_t *dst, ip_addr_t *result)
{
struct netconn *conn = (struct netconn *)handle;
struct netif *dst_if;
const ip_addr_t *dst_ip;
LWIP_UNUSED_ARG(conn); /* unused in case of IPV4 only configuration */
ip_route_get_local_ip(&conn->pcb.udp->local_ip, dst, dst_if, dst_ip);
if ((dst_if != NULL) && (dst_ip != NULL)) {
ip_addr_copy(*result, *dst_ip);
return 1;
} else {
return 0;
}
}
/**
* Starts SNMP Agent.
*/
void
snmp_init(void)
{
LWIP_ASSERT_CORE_LOCKED();
sys_thread_new("snmp_netconn", snmp_netconn_thread, NULL, SNMP_STACK_SIZE, SNMP_THREAD_PRIO);
}
#endif /* LWIP_SNMP && SNMP_USE_NETCONN */

View File

@ -0,0 +1,156 @@
/**
* @file
* SNMP pbuf stream wrapper implementation (internal API, do not use in client code).
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Martin Hentschel <info@cl-soft.de>
*
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
#include "snmp_pbuf_stream.h"
#include "lwip/def.h"
#include <string.h>
err_t
snmp_pbuf_stream_init(struct snmp_pbuf_stream *pbuf_stream, struct pbuf *p, u16_t offset, u16_t length)
{
pbuf_stream->offset = offset;
pbuf_stream->length = length;
pbuf_stream->pbuf = p;
return ERR_OK;
}
err_t
snmp_pbuf_stream_read(struct snmp_pbuf_stream *pbuf_stream, u8_t *data)
{
if (pbuf_stream->length == 0) {
return ERR_BUF;
}
if (pbuf_copy_partial(pbuf_stream->pbuf, data, 1, pbuf_stream->offset) == 0) {
return ERR_BUF;
}
pbuf_stream->offset++;
pbuf_stream->length--;
return ERR_OK;
}
err_t
snmp_pbuf_stream_write(struct snmp_pbuf_stream *pbuf_stream, u8_t data)
{
return snmp_pbuf_stream_writebuf(pbuf_stream, &data, 1);
}
err_t
snmp_pbuf_stream_writebuf(struct snmp_pbuf_stream *pbuf_stream, const void *buf, u16_t buf_len)
{
if (pbuf_stream->length < buf_len) {
return ERR_BUF;
}
if (pbuf_take_at(pbuf_stream->pbuf, buf, buf_len, pbuf_stream->offset) != ERR_OK) {
return ERR_BUF;
}
pbuf_stream->offset += buf_len;
pbuf_stream->length -= buf_len;
return ERR_OK;
}
err_t
snmp_pbuf_stream_writeto(struct snmp_pbuf_stream *pbuf_stream, struct snmp_pbuf_stream *target_pbuf_stream, u16_t len)
{
if ((pbuf_stream == NULL) || (target_pbuf_stream == NULL)) {
return ERR_ARG;
}
if ((len > pbuf_stream->length) || (len > target_pbuf_stream->length)) {
return ERR_ARG;
}
if (len == 0) {
len = LWIP_MIN(pbuf_stream->length, target_pbuf_stream->length);
}
while (len > 0) {
u16_t chunk_len;
err_t err;
u16_t target_offset;
struct pbuf *pbuf = pbuf_skip(pbuf_stream->pbuf, pbuf_stream->offset, &target_offset);
if ((pbuf == NULL) || (pbuf->len == 0)) {
return ERR_BUF;
}
chunk_len = LWIP_MIN(len, pbuf->len);
err = snmp_pbuf_stream_writebuf(target_pbuf_stream, &((u8_t *)pbuf->payload)[target_offset], chunk_len);
if (err != ERR_OK) {
return err;
}
pbuf_stream->offset += chunk_len;
pbuf_stream->length -= chunk_len;
len -= chunk_len;
}
return ERR_OK;
}
err_t
snmp_pbuf_stream_seek(struct snmp_pbuf_stream *pbuf_stream, s32_t offset)
{
if ((offset < 0) || (offset > pbuf_stream->length)) {
/* we cannot seek backwards or forward behind stream end */
return ERR_ARG;
}
pbuf_stream->offset += (u16_t)offset;
pbuf_stream->length -= (u16_t)offset;
return ERR_OK;
}
err_t
snmp_pbuf_stream_seek_abs(struct snmp_pbuf_stream *pbuf_stream, u32_t offset)
{
s32_t rel_offset = offset - pbuf_stream->offset;
return snmp_pbuf_stream_seek(pbuf_stream, rel_offset);
}
#endif /* LWIP_SNMP */

View File

@ -0,0 +1,72 @@
/**
* @file
* SNMP pbuf stream wrapper (internal API, do not use in client code).
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Martin Hentschel <info@cl-soft.de>
*
*/
#ifndef LWIP_HDR_APPS_SNMP_PBUF_STREAM_H
#define LWIP_HDR_APPS_SNMP_PBUF_STREAM_H
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP
#include "lwip/err.h"
#include "lwip/pbuf.h"
#ifdef __cplusplus
extern "C" {
#endif
struct snmp_pbuf_stream {
struct pbuf *pbuf;
u16_t offset;
u16_t length;
};
err_t snmp_pbuf_stream_init(struct snmp_pbuf_stream *pbuf_stream, struct pbuf *p, u16_t offset, u16_t length);
err_t snmp_pbuf_stream_read(struct snmp_pbuf_stream *pbuf_stream, u8_t *data);
err_t snmp_pbuf_stream_write(struct snmp_pbuf_stream *pbuf_stream, u8_t data);
err_t snmp_pbuf_stream_writebuf(struct snmp_pbuf_stream *pbuf_stream, const void *buf, u16_t buf_len);
err_t snmp_pbuf_stream_writeto(struct snmp_pbuf_stream *pbuf_stream, struct snmp_pbuf_stream *target_pbuf_stream, u16_t len);
err_t snmp_pbuf_stream_seek(struct snmp_pbuf_stream *pbuf_stream, s32_t offset);
err_t snmp_pbuf_stream_seek_abs(struct snmp_pbuf_stream *pbuf_stream, u32_t offset);
#ifdef __cplusplus
}
#endif
#endif /* LWIP_SNMP */
#endif /* LWIP_HDR_APPS_SNMP_PBUF_STREAM_H */

View File

@ -0,0 +1,103 @@
/**
* @file
* SNMP RAW API frontend.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
*/
#include "lwip/apps/snmp_opts.h"
#include "lwip/ip_addr.h"
#if LWIP_SNMP && SNMP_USE_RAW
#include "lwip/udp.h"
#include "lwip/ip.h"
#include "lwip/prot/iana.h"
#include "snmp_msg.h"
/* lwIP UDP receive callback function */
static void
snmp_recv(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
{
LWIP_UNUSED_ARG(arg);
snmp_receive(pcb, p, addr, port);
pbuf_free(p);
}
err_t
snmp_sendto(void *handle, struct pbuf *p, const ip_addr_t *dst, u16_t port)
{
return udp_sendto((struct udp_pcb *)handle, p, dst, port);
}
u8_t
snmp_get_local_ip_for_dst(void *handle, const ip_addr_t *dst, ip_addr_t *result)
{
struct udp_pcb *udp_pcb = (struct udp_pcb *)handle;
struct netif *dst_if;
const ip_addr_t *dst_ip;
LWIP_UNUSED_ARG(udp_pcb); /* unused in case of IPV4 only configuration */
ip_route_get_local_ip(&udp_pcb->local_ip, dst, dst_if, dst_ip);
if ((dst_if != NULL) && (dst_ip != NULL)) {
ip_addr_copy(*result, *dst_ip);
return 1;
} else {
return 0;
}
}
/**
* @ingroup snmp_core
* Starts SNMP Agent.
* Allocates UDP pcb and binds it to IP_ANY_TYPE port 161.
*/
void
snmp_init(void)
{
err_t err;
struct udp_pcb *snmp_pcb = udp_new_ip_type(IPADDR_TYPE_ANY);
LWIP_ERROR("snmp_raw: no PCB", (snmp_pcb != NULL), return;);
LWIP_ASSERT_CORE_LOCKED();
snmp_traps_handle = snmp_pcb;
udp_recv(snmp_pcb, snmp_recv, NULL);
err = udp_bind(snmp_pcb, IP_ANY_TYPE, LWIP_IANA_PORT_SNMP);
LWIP_ERROR("snmp_raw: Unable to bind PCB", (err == ERR_OK), return;);
}
#endif /* LWIP_SNMP && SNMP_USE_RAW */

View File

@ -0,0 +1,232 @@
/**
* @file
* SNMP scalar node support implementation.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Martin Hentschel <info@cl-soft.de>
*
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
#include "lwip/apps/snmp_scalar.h"
#include "lwip/apps/snmp_core.h"
static s16_t snmp_scalar_array_get_value(struct snmp_node_instance *instance, void *value);
static snmp_err_t snmp_scalar_array_set_test(struct snmp_node_instance *instance, u16_t value_len, void *value);
static snmp_err_t snmp_scalar_array_set_value(struct snmp_node_instance *instance, u16_t value_len, void *value);
snmp_err_t
snmp_scalar_get_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
const struct snmp_scalar_node *scalar_node = (const struct snmp_scalar_node *)(const void *)instance->node;
LWIP_UNUSED_ARG(root_oid);
LWIP_UNUSED_ARG(root_oid_len);
/* scalar only has one dedicated instance: .0 */
if ((instance->instance_oid.len != 1) || (instance->instance_oid.id[0] != 0)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
instance->access = scalar_node->access;
instance->asn1_type = scalar_node->asn1_type;
instance->get_value = scalar_node->get_value;
instance->set_test = scalar_node->set_test;
instance->set_value = scalar_node->set_value;
return SNMP_ERR_NOERROR;
}
snmp_err_t
snmp_scalar_get_next_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
/* because our only instance is .0 we can only return a next instance if no instance oid is passed */
if (instance->instance_oid.len == 0) {
instance->instance_oid.len = 1;
instance->instance_oid.id[0] = 0;
return snmp_scalar_get_instance(root_oid, root_oid_len, instance);
}
return SNMP_ERR_NOSUCHINSTANCE;
}
snmp_err_t
snmp_scalar_array_get_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
LWIP_UNUSED_ARG(root_oid);
LWIP_UNUSED_ARG(root_oid_len);
if ((instance->instance_oid.len == 2) && (instance->instance_oid.id[1] == 0)) {
const struct snmp_scalar_array_node *array_node = (const struct snmp_scalar_array_node *)(const void *)instance->node;
const struct snmp_scalar_array_node_def *array_node_def = array_node->array_nodes;
u32_t i = 0;
while (i < array_node->array_node_count) {
if (array_node_def->oid == instance->instance_oid.id[0]) {
break;
}
array_node_def++;
i++;
}
if (i < array_node->array_node_count) {
instance->access = array_node_def->access;
instance->asn1_type = array_node_def->asn1_type;
instance->get_value = snmp_scalar_array_get_value;
instance->set_test = snmp_scalar_array_set_test;
instance->set_value = snmp_scalar_array_set_value;
instance->reference.const_ptr = array_node_def;
return SNMP_ERR_NOERROR;
}
}
return SNMP_ERR_NOSUCHINSTANCE;
}
snmp_err_t
snmp_scalar_array_get_next_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
const struct snmp_scalar_array_node *array_node = (const struct snmp_scalar_array_node *)(const void *)instance->node;
const struct snmp_scalar_array_node_def *array_node_def = array_node->array_nodes;
const struct snmp_scalar_array_node_def *result = NULL;
LWIP_UNUSED_ARG(root_oid);
LWIP_UNUSED_ARG(root_oid_len);
if ((instance->instance_oid.len == 0) && (array_node->array_node_count > 0)) {
/* return node with lowest OID */
u16_t i = 0;
result = array_node_def;
array_node_def++;
for (i = 1; i < array_node->array_node_count; i++) {
if (array_node_def->oid < result->oid) {
result = array_node_def;
}
array_node_def++;
}
} else if (instance->instance_oid.len >= 1) {
if (instance->instance_oid.len == 1) {
/* if we have the requested OID we return its instance, otherwise we search for the next available */
u16_t i = 0;
while (i < array_node->array_node_count) {
if (array_node_def->oid == instance->instance_oid.id[0]) {
result = array_node_def;
break;
}
array_node_def++;
i++;
}
}
if (result == NULL) {
u32_t oid_dist = 0xFFFFFFFFUL;
u16_t i = 0;
array_node_def = array_node->array_nodes; /* may be already at the end when if case before was executed without result -> reinitialize to start */
while (i < array_node->array_node_count) {
if ((array_node_def->oid > instance->instance_oid.id[0]) &&
((u32_t)(array_node_def->oid - instance->instance_oid.id[0]) < oid_dist)) {
result = array_node_def;
oid_dist = array_node_def->oid - instance->instance_oid.id[0];
}
array_node_def++;
i++;
}
}
}
if (result == NULL) {
/* nothing to return */
return SNMP_ERR_NOSUCHINSTANCE;
}
instance->instance_oid.len = 2;
instance->instance_oid.id[0] = result->oid;
instance->instance_oid.id[1] = 0;
instance->access = result->access;
instance->asn1_type = result->asn1_type;
instance->get_value = snmp_scalar_array_get_value;
instance->set_test = snmp_scalar_array_set_test;
instance->set_value = snmp_scalar_array_set_value;
instance->reference.const_ptr = result;
return SNMP_ERR_NOERROR;
}
static s16_t
snmp_scalar_array_get_value(struct snmp_node_instance *instance, void *value)
{
s16_t result = -1;
const struct snmp_scalar_array_node *array_node = (const struct snmp_scalar_array_node *)(const void *)instance->node;
const struct snmp_scalar_array_node_def *array_node_def = (const struct snmp_scalar_array_node_def *)instance->reference.const_ptr;
if (array_node->get_value != NULL) {
result = array_node->get_value(array_node_def, value);
}
return result;
}
static snmp_err_t
snmp_scalar_array_set_test(struct snmp_node_instance *instance, u16_t value_len, void *value)
{
snmp_err_t result = SNMP_ERR_NOTWRITABLE;
const struct snmp_scalar_array_node *array_node = (const struct snmp_scalar_array_node *)(const void *)instance->node;
const struct snmp_scalar_array_node_def *array_node_def = (const struct snmp_scalar_array_node_def *)instance->reference.const_ptr;
if (array_node->set_test != NULL) {
result = array_node->set_test(array_node_def, value_len, value);
}
return result;
}
static snmp_err_t
snmp_scalar_array_set_value(struct snmp_node_instance *instance, u16_t value_len, void *value)
{
snmp_err_t result = SNMP_ERR_NOTWRITABLE;
const struct snmp_scalar_array_node *array_node = (const struct snmp_scalar_array_node *)(const void *)instance->node;
const struct snmp_scalar_array_node_def *array_node_def = (const struct snmp_scalar_array_node_def *)instance->reference.const_ptr;
if (array_node->set_value != NULL) {
result = array_node->set_value(array_node_def, value_len, value);
}
return result;
}
#endif /* LWIP_SNMP */

View File

@ -0,0 +1,90 @@
/*
Generated by LwipMibCompiler
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP && LWIP_SNMP_V3 /* don't build if not configured for use in lwipopts.h */
#include "lwip/apps/snmp_snmpv2_framework.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_scalar.h"
#include "lwip/apps/snmp_table.h"
#include "lwip/apps/snmpv3.h"
#include "snmpv3_priv.h"
#include "lwip/sys.h"
#include <string.h>
const struct snmp_obj_id usmNoAuthProtocol = { 10, { 1, 3, 6, 1, 6, 3, 10, 1, 1, 1 } };
const struct snmp_obj_id usmHMACMD5AuthProtocol = { 10, { 1, 3, 6, 1, 6, 3, 10, 1, 1, 2 } };
const struct snmp_obj_id usmHMACSHAAuthProtocol = { 10, { 1, 3, 6, 1, 6, 3, 10, 1, 1, 3 } };
/* .4 sha-224
* .5 sha-256
* .6 sha-384
* .7 sha-512
*/
const struct snmp_obj_id usmNoPrivProtocol = { 10, { 1, 3, 6, 1, 6, 3, 10, 1, 2, 1 } };
const struct snmp_obj_id usmDESPrivProtocol = { 10, { 1, 3, 6, 1, 6, 3, 10, 1, 2, 2 } };
/* .3 3des-ede */
const struct snmp_obj_id usmAESPrivProtocol = { 10, { 1, 3, 6, 1, 6, 3, 10, 1, 2, 4 } };
/* .5 unknown
* .6 unknown
* .7 unknown
*/
/* TODO: where should this value come from? */
#define SNMP_FRAMEWORKMIB_SNMPENGINEMAXMESSAGESIZE 1500
/* --- snmpFrameworkMIBObjects 1.3.6.1.6.3.10.2 ----------------------------------------------------- */
static s16_t snmpengine_scalars_get_value(const struct snmp_scalar_array_node_def *node, void *value)
{
const char *engineid;
u8_t engineid_len;
switch (node->oid) {
case 1: /* snmpEngineID */
snmpv3_get_engine_id(&engineid, &engineid_len);
MEMCPY(value, engineid, engineid_len);
return engineid_len;
case 2: /* snmpEngineBoots */
*(s32_t *)value = snmpv3_get_engine_boots_internal();
return sizeof(s32_t);
case 3: /* snmpEngineTime */
*(s32_t *)value = snmpv3_get_engine_time_internal();
return sizeof(s32_t);
case 4: /* snmpEngineMaxMessageSize */
*(s32_t *)value = SNMP_FRAMEWORKMIB_SNMPENGINEMAXMESSAGESIZE;
return sizeof(s32_t);
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("snmpengine_scalars_get_value(): unknown id: %"S32_F"\n", node->oid));
return 0;
}
}
static const struct snmp_scalar_array_node_def snmpengine_scalars_nodes[] = {
{1, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpEngineID */
{2, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpEngineBoots */
{3, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpEngineTime */
{4, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY}, /* snmpEngineMaxMessageSize */
};
static const struct snmp_scalar_array_node snmpengine_scalars = SNMP_SCALAR_CREATE_ARRAY_NODE(1, snmpengine_scalars_nodes, snmpengine_scalars_get_value, NULL, NULL);
static const struct snmp_node *const snmpframeworkmibobjects_subnodes[] = {
&snmpengine_scalars.node.node
};
static const struct snmp_tree_node snmpframeworkmibobjects_treenode = SNMP_CREATE_TREE_NODE(2, snmpframeworkmibobjects_subnodes);
/* --- snmpFrameworkMIB ----------------------------------------------------- */
static const struct snmp_node *const snmpframeworkmib_subnodes[] = {
&snmpframeworkmibobjects_treenode.node
};
static const struct snmp_tree_node snmpframeworkmib_root = SNMP_CREATE_TREE_NODE(10, snmpframeworkmib_subnodes);
static const u32_t snmpframeworkmib_base_oid[] = {1, 3, 6, 1, 6, 3, 10};
const struct snmp_mib snmpframeworkmib = {snmpframeworkmib_base_oid, LWIP_ARRAYSIZE(snmpframeworkmib_base_oid), &snmpframeworkmib_root.node};
/* --- snmpFrameworkMIB ----------------------------------------------------- */
#endif /* LWIP_SNMP */

View File

@ -0,0 +1,410 @@
/*
Generated by LwipMibCompiler
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP && LWIP_SNMP_V3
#include "lwip/apps/snmp_snmpv2_usm.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_scalar.h"
#include "lwip/apps/snmp_table.h"
#include "lwip/apps/snmpv3.h"
#include "snmpv3_priv.h"
#include "lwip/apps/snmp_snmpv2_framework.h"
#include <string.h>
/* --- usmUser 1.3.6.1.6.3.15.1.2 ----------------------------------------------------- */
static const struct snmp_oid_range usmUserTable_oid_ranges[] = {
{ 0, 0xff }, { 0, 0xff }, { 0, 0xff }, { 0, 0xff },
{ 0, 0xff }, { 0, 0xff }, { 0, 0xff }, { 0, 0xff },
{ 0, 0xff }, { 0, 0xff }, { 0, 0xff }, { 0, 0xff },
{ 0, 0xff }, { 0, 0xff }, { 0, 0xff }, { 0, 0xff },
{ 0, 0xff }, { 0, 0xff }, { 0, 0xff }, { 0, 0xff },
{ 0, 0xff }, { 0, 0xff }, { 0, 0xff }, { 0, 0xff },
{ 0, 0xff }, { 0, 0xff }, { 0, 0xff }, { 0, 0xff },
{ 0, 0xff }, { 0, 0xff }, { 0, 0xff }, { 0, 0xff }
};
static void snmp_engineid_to_oid(const char *engineid, u32_t *oid, u32_t len)
{
u8_t i;
for (i = 0; i < len; i++) {
oid[i] = engineid[i];
}
}
static void snmp_oid_to_name(char *name, const u32_t *oid, size_t len)
{
u8_t i;
for (i = 0; i < len; i++) {
name[i] = (char)oid[i];
}
}
static void snmp_name_to_oid(const char *name, u32_t *oid, size_t len)
{
u8_t i;
for (i = 0; i < len; i++) {
oid[i] = name[i];
}
}
static const struct snmp_obj_id *snmp_auth_algo_to_oid(snmpv3_auth_algo_t algo)
{
if (algo == SNMP_V3_AUTH_ALGO_MD5) {
return &usmHMACMD5AuthProtocol;
} else if (algo == SNMP_V3_AUTH_ALGO_SHA) {
return &usmHMACMD5AuthProtocol;
}
return &usmNoAuthProtocol;
}
static const struct snmp_obj_id *snmp_priv_algo_to_oid(snmpv3_priv_algo_t algo)
{
if (algo == SNMP_V3_PRIV_ALGO_DES) {
return &usmDESPrivProtocol;
} else if (algo == SNMP_V3_PRIV_ALGO_AES) {
return &usmAESPrivProtocol;
}
return &usmNoPrivProtocol;
}
char username[32];
static snmp_err_t usmusertable_get_instance(const u32_t *column, const u32_t *row_oid, u8_t row_oid_len, struct snmp_node_instance *cell_instance)
{
const char *engineid;
u8_t eid_len;
u32_t engineid_oid[SNMP_V3_MAX_ENGINE_ID_LENGTH];
u8_t name_len;
u8_t engineid_len;
u8_t name_start;
u8_t engineid_start;
LWIP_UNUSED_ARG(column);
snmpv3_get_engine_id(&engineid, &eid_len);
engineid_len = (u8_t)row_oid[0];
engineid_start = 1;
if (engineid_len != eid_len) {
/* EngineID length does not match! */
return SNMP_ERR_NOSUCHINSTANCE;
}
if (engineid_len > row_oid_len) {
/* row OID doesn't contain enough data according to engineid_len.*/
return SNMP_ERR_NOSUCHINSTANCE;
}
/* check if incoming OID length and if values are in plausible range */
if (!snmp_oid_in_range(&row_oid[engineid_start], engineid_len, usmUserTable_oid_ranges, engineid_len)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
snmp_engineid_to_oid(engineid, engineid_oid, engineid_len);
/* Verify EngineID */
if (snmp_oid_equal(&row_oid[engineid_start], engineid_len, engineid_oid, engineid_len)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
name_len = (u8_t)row_oid[engineid_start + engineid_len];
name_start = engineid_start + engineid_len + 1;
if (name_len > SNMP_V3_MAX_USER_LENGTH) {
/* specified name is too long */
return SNMP_ERR_NOSUCHINSTANCE;
}
if (1 + engineid_len + 1 + name_len != row_oid_len) {
/* Length of EngineID and name does not match row oid length. (+2 for length fields)*/
return SNMP_ERR_NOSUCHINSTANCE;
}
/* check if incoming OID length and if values are in plausible range */
if (!snmp_oid_in_range(&row_oid[name_start], name_len, usmUserTable_oid_ranges, name_len)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* Verify if user exists */
memset(username, 0, sizeof(username));
snmp_oid_to_name(username, &row_oid[name_start], name_len);
if (snmpv3_get_user(username, NULL, NULL, NULL, NULL) != ERR_OK) {
return SNMP_ERR_NOSUCHINSTANCE;
}
/* Save name in reference pointer to make it easier to handle later on */
cell_instance->reference.ptr = username;
cell_instance->reference_len = name_len;
/* user was found */
return SNMP_ERR_NOERROR;
}
/*
* valid oid options
* <oid>
* <oid>.<EngineID length>
* <oid>.<EngineID length>.<partial EngineID>
* <oid>.<EngineID length>.<EngineID>
* <oid>.<EngineID length>.<EngineID>.<UserName length>
* <oid>.<EngineID length>.<EngineID>.<UserName length>.<partial UserName>
* <oid>.<EngineID length>.<EngineID>.<UserName length>.<UserName>
*
*/
static snmp_err_t usmusertable_get_next_instance(const u32_t *column, struct snmp_obj_id *row_oid, struct snmp_node_instance *cell_instance)
{
const char *engineid;
u8_t eid_len;
u32_t engineid_oid[SNMP_V3_MAX_ENGINE_ID_LENGTH];
u8_t name_len;
u8_t engineid_len;
u8_t name_start;
u8_t engineid_start = 1;
u8_t i;
struct snmp_next_oid_state state;
u32_t result_temp[LWIP_ARRAYSIZE(usmUserTable_oid_ranges)];
LWIP_UNUSED_ARG(column);
snmpv3_get_engine_id(&engineid, &eid_len);
/* If EngineID might be given */
if (row_oid->len > 0) {
engineid_len = (u8_t)row_oid->id[0];
engineid_start = 1;
if (engineid_len != eid_len) {
/* EngineID length does not match! */
return SNMP_ERR_NOSUCHINSTANCE;
}
if (engineid_len > row_oid->len) {
/* Verify partial EngineID */
snmp_engineid_to_oid(engineid, engineid_oid, row_oid->len - 1);
if (!snmp_oid_equal(&row_oid->id[engineid_start], row_oid->len - 1, engineid_oid, row_oid->len - 1)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
} else {
/* Verify complete EngineID */
snmp_engineid_to_oid(engineid, engineid_oid, engineid_len);
if (!snmp_oid_equal(&row_oid->id[engineid_start], engineid_len, engineid_oid, engineid_len)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
}
/* At this point, the given EngineID (partially) matches the local EngineID.*/
/* If name might also be given */
if (row_oid->len > engineid_start + engineid_len) {
name_len = (u8_t)row_oid->id[engineid_start + engineid_len];
name_start = engineid_start + engineid_len + 1;
if (name_len > SNMP_V3_MAX_USER_LENGTH) {
/* specified name is too long, max length is 32 according to mib file.*/
return SNMP_ERR_NOSUCHINSTANCE;
}
if (row_oid->len < engineid_len + name_len + 2) {
/* Partial name given according to oid.*/
u8_t tmplen = row_oid->len - engineid_len - 2;
if (!snmp_oid_in_range(&row_oid->id[name_start], tmplen, usmUserTable_oid_ranges, tmplen)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
} else {
/* Full name given according to oid. Also test for too much data.*/
u8_t tmplen = row_oid->len - engineid_len - 2;
if (!snmp_oid_in_range(&row_oid->id[name_start], name_len, usmUserTable_oid_ranges, tmplen)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
}
/* At this point the EngineID and (partial) UserName match the local EngineID and UserName.*/
}
}
/* init struct to search next oid */
snmp_next_oid_init(&state, row_oid->id, row_oid->len, result_temp, LWIP_ARRAYSIZE(usmUserTable_oid_ranges));
for (i = 0; i < snmpv3_get_amount_of_users(); i++) {
u32_t test_oid[LWIP_ARRAYSIZE(usmUserTable_oid_ranges)];
test_oid[0] = eid_len;
snmp_engineid_to_oid(engineid, &test_oid[1], eid_len);
snmpv3_get_username(username, i);
test_oid[1 + eid_len] = strlen(username);
snmp_name_to_oid(username, &test_oid[2 + eid_len], strlen(username));
/* check generated OID: is it a candidate for the next one? */
snmp_next_oid_check(&state, test_oid, (u8_t)(1 + eid_len + 1 + strlen(username)), LWIP_PTR_NUMERIC_CAST(void *, i));
}
/* did we find a next one? */
if (state.status == SNMP_NEXT_OID_STATUS_SUCCESS) {
snmp_oid_assign(row_oid, state.next_oid, state.next_oid_len);
/* store username for subsequent operations (get/test/set) */
memset(username, 0, sizeof(username));
snmpv3_get_username(username, LWIP_PTR_NUMERIC_CAST(u8_t, state.reference));
cell_instance->reference.ptr = username;
cell_instance->reference_len = strlen(username);
return SNMP_ERR_NOERROR;
}
/* not found */
return SNMP_ERR_NOSUCHINSTANCE;
}
static s16_t usmusertable_get_value(struct snmp_node_instance *cell_instance, void *value)
{
snmpv3_user_storagetype_t storage_type;
switch (SNMP_TABLE_GET_COLUMN_FROM_OID(cell_instance->instance_oid.id)) {
case 3: /* usmUserSecurityName */
MEMCPY(value, cell_instance->reference.ptr, cell_instance->reference_len);
return (s16_t)cell_instance->reference_len;
case 4: /* usmUserCloneFrom */
MEMCPY(value, snmp_zero_dot_zero.id, snmp_zero_dot_zero.len * sizeof(u32_t));
return snmp_zero_dot_zero.len * sizeof(u32_t);
case 5: { /* usmUserAuthProtocol */
const struct snmp_obj_id *auth_algo;
snmpv3_auth_algo_t auth_algo_val;
snmpv3_get_user((const char *)cell_instance->reference.ptr, &auth_algo_val, NULL, NULL, NULL);
auth_algo = snmp_auth_algo_to_oid(auth_algo_val);
MEMCPY(value, auth_algo->id, auth_algo->len * sizeof(u32_t));
return auth_algo->len * sizeof(u32_t);
}
case 6: /* usmUserAuthKeyChange */
return 0;
case 7: /* usmUserOwnAuthKeyChange */
return 0;
case 8: { /* usmUserPrivProtocol */
const struct snmp_obj_id *priv_algo;
snmpv3_priv_algo_t priv_algo_val;
snmpv3_get_user((const char *)cell_instance->reference.ptr, NULL, NULL, &priv_algo_val, NULL);
priv_algo = snmp_priv_algo_to_oid(priv_algo_val);
MEMCPY(value, priv_algo->id, priv_algo->len * sizeof(u32_t));
return priv_algo->len * sizeof(u32_t);
}
case 9: /* usmUserPrivKeyChange */
return 0;
case 10: /* usmUserOwnPrivKeyChange */
return 0;
case 11: /* usmUserPublic */
/* TODO: Implement usmUserPublic */
return 0;
case 12: /* usmUserStorageType */
snmpv3_get_user_storagetype((const char *)cell_instance->reference.ptr, &storage_type);
*(s32_t *)value = storage_type;
return sizeof(s32_t);
case 13: /* usmUserStatus */
*(s32_t *)value = 1; /* active */
return sizeof(s32_t);
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("usmusertable_get_value(): unknown id: %"S32_F"\n", SNMP_TABLE_GET_COLUMN_FROM_OID(cell_instance->instance_oid.id)));
return 0;
}
}
/* --- usmMIBObjects 1.3.6.1.6.3.15.1 ----------------------------------------------------- */
static s16_t usmstats_scalars_get_value(const struct snmp_scalar_array_node_def *node, void *value)
{
u32_t *uint_ptr = (u32_t *)value;
switch (node->oid) {
case 1: /* usmStatsUnsupportedSecLevels */
*uint_ptr = snmp_stats.unsupportedseclevels;
break;
case 2: /* usmStatsNotInTimeWindows */
*uint_ptr = snmp_stats.notintimewindows;
break;
case 3: /* usmStatsUnknownUserNames */
*uint_ptr = snmp_stats.unknownusernames;
break;
case 4: /* usmStatsUnknownEngineIDs */
*uint_ptr = snmp_stats.unknownengineids;
break;
case 5: /* usmStatsWrongDigests */
*uint_ptr = snmp_stats.wrongdigests;
break;
case 6: /* usmStatsDecryptionErrors */
*uint_ptr = snmp_stats.decryptionerrors;
break;
default:
LWIP_DEBUGF(SNMP_MIB_DEBUG, ("usmstats_scalars_get_value(): unknown id: %"S32_F"\n", node->oid));
return 0;
}
return sizeof(*uint_ptr);
}
/* --- snmpUsmMIB ----------------------------------------------------- */
/* --- usmUser 1.3.6.1.6.3.15.1.2 ----------------------------------------------------- */
static const struct snmp_table_col_def usmusertable_columns[] = {
{3, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserSecurityName */
{4, SNMP_ASN1_TYPE_OBJECT_ID, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserCloneFrom */
{5, SNMP_ASN1_TYPE_OBJECT_ID, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserAuthProtocol */
{6, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserAuthKeyChange */
{7, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserOwnAuthKeyChange */
{8, SNMP_ASN1_TYPE_OBJECT_ID, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserPrivProtocol */
{9, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserPrivKeyChange */
{10, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserOwnPrivKeyChange */
{11, SNMP_ASN1_TYPE_OCTET_STRING, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserPublic */
{12, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserStorageType */
{13, SNMP_ASN1_TYPE_INTEGER, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmUserStatus */
};
static const struct snmp_table_node usmusertable = SNMP_TABLE_CREATE(2, usmusertable_columns, usmusertable_get_instance, usmusertable_get_next_instance, usmusertable_get_value, NULL, NULL);
static const struct snmp_node *const usmuser_subnodes[] = {
&usmusertable.node.node
};
static const struct snmp_tree_node usmuser_treenode = SNMP_CREATE_TREE_NODE(2, usmuser_subnodes);
/* --- usmMIBObjects 1.3.6.1.6.3.15.1 ----------------------------------------------------- */
static const struct snmp_scalar_array_node_def usmstats_scalars_nodes[] = {
{1, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmStatsUnsupportedSecLevels */
{2, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmStatsNotInTimeWindows */
{3, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmStatsUnknownUserNames */
{4, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmStatsUnknownEngineIDs */
{5, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmStatsWrongDigests */
{6, SNMP_ASN1_TYPE_COUNTER, SNMP_NODE_INSTANCE_READ_ONLY}, /* usmStatsDecryptionErrors */
};
static const struct snmp_scalar_array_node usmstats_scalars = SNMP_SCALAR_CREATE_ARRAY_NODE(1, usmstats_scalars_nodes, usmstats_scalars_get_value, NULL, NULL);
static const struct snmp_node *const usmmibobjects_subnodes[] = {
&usmstats_scalars.node.node,
&usmuser_treenode.node
};
static const struct snmp_tree_node usmmibobjects_treenode = SNMP_CREATE_TREE_NODE(1, usmmibobjects_subnodes);
/* --- snmpUsmMIB ----------------------------------------------------- */
static const struct snmp_node *const snmpusmmib_subnodes[] = {
&usmmibobjects_treenode.node
};
static const struct snmp_tree_node snmpusmmib_root = SNMP_CREATE_TREE_NODE(15, snmpusmmib_subnodes);
static const u32_t snmpusmmib_base_oid[] = {1, 3, 6, 1, 6, 3, 15};
const struct snmp_mib snmpusmmib = {snmpusmmib_base_oid, LWIP_ARRAYSIZE(snmpusmmib_base_oid), &snmpusmmib_root.node};
#endif /* LWIP_SNMP */

View File

@ -0,0 +1,342 @@
/**
* @file
* SNMP table support implementation.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Martin Hentschel <info@cl-soft.de>
*
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
#include "lwip/apps/snmp_core.h"
#include "lwip/apps/snmp_table.h"
#include <string.h>
snmp_err_t snmp_table_get_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
snmp_err_t ret = SNMP_ERR_NOSUCHINSTANCE;
const struct snmp_table_node *table_node = (const struct snmp_table_node *)(const void *)instance->node;
LWIP_UNUSED_ARG(root_oid);
LWIP_UNUSED_ARG(root_oid_len);
/* check min. length (fixed row entry definition, column, row instance oid with at least one entry */
/* fixed row entry always has oid 1 */
if ((instance->instance_oid.len >= 3) && (instance->instance_oid.id[0] == 1)) {
/* search column */
const struct snmp_table_col_def *col_def = table_node->columns;
u16_t i = table_node->column_count;
while (i > 0) {
if (col_def->index == instance->instance_oid.id[1]) {
break;
}
col_def++;
i--;
}
if (i > 0) {
/* everything may be overwritten by get_cell_instance_method() in order to implement special handling for single columns/cells */
instance->asn1_type = col_def->asn1_type;
instance->access = col_def->access;
instance->get_value = table_node->get_value;
instance->set_test = table_node->set_test;
instance->set_value = table_node->set_value;
ret = table_node->get_cell_instance(
&(instance->instance_oid.id[1]),
&(instance->instance_oid.id[2]),
instance->instance_oid.len - 2,
instance);
}
}
return ret;
}
snmp_err_t snmp_table_get_next_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
const struct snmp_table_node *table_node = (const struct snmp_table_node *)(const void *)instance->node;
const struct snmp_table_col_def *col_def;
struct snmp_obj_id row_oid;
u32_t column = 0;
snmp_err_t result;
LWIP_UNUSED_ARG(root_oid);
LWIP_UNUSED_ARG(root_oid_len);
/* check that first part of id is 0 or 1, referencing fixed row entry */
if ((instance->instance_oid.len > 0) && (instance->instance_oid.id[0] > 1)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
if (instance->instance_oid.len > 1) {
column = instance->instance_oid.id[1];
}
if (instance->instance_oid.len > 2) {
snmp_oid_assign(&row_oid, &(instance->instance_oid.id[2]), instance->instance_oid.len - 2);
} else {
row_oid.len = 0;
}
instance->get_value = table_node->get_value;
instance->set_test = table_node->set_test;
instance->set_value = table_node->set_value;
/* resolve column and value */
do {
u16_t i;
const struct snmp_table_col_def *next_col_def = NULL;
col_def = table_node->columns;
for (i = 0; i < table_node->column_count; i++) {
if (col_def->index == column) {
next_col_def = col_def;
break;
} else if ((col_def->index > column) && ((next_col_def == NULL) || (col_def->index < next_col_def->index))) {
next_col_def = col_def;
}
col_def++;
}
if (next_col_def == NULL) {
/* no further column found */
return SNMP_ERR_NOSUCHINSTANCE;
}
instance->asn1_type = next_col_def->asn1_type;
instance->access = next_col_def->access;
result = table_node->get_next_cell_instance(
&next_col_def->index,
&row_oid,
instance);
if (result == SNMP_ERR_NOERROR) {
col_def = next_col_def;
break;
}
row_oid.len = 0; /* reset row_oid because we switch to next column and start with the first entry there */
column = next_col_def->index + 1;
} while (1);
/* build resulting oid */
instance->instance_oid.len = 2;
instance->instance_oid.id[0] = 1;
instance->instance_oid.id[1] = col_def->index;
snmp_oid_append(&instance->instance_oid, row_oid.id, row_oid.len);
return SNMP_ERR_NOERROR;
}
snmp_err_t snmp_table_simple_get_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
snmp_err_t ret = SNMP_ERR_NOSUCHINSTANCE;
const struct snmp_table_simple_node *table_node = (const struct snmp_table_simple_node *)(const void *)instance->node;
LWIP_UNUSED_ARG(root_oid);
LWIP_UNUSED_ARG(root_oid_len);
/* check min. length (fixed row entry definition, column, row instance oid with at least one entry */
/* fixed row entry always has oid 1 */
if ((instance->instance_oid.len >= 3) && (instance->instance_oid.id[0] == 1)) {
ret = table_node->get_cell_value(
&(instance->instance_oid.id[1]),
&(instance->instance_oid.id[2]),
instance->instance_oid.len - 2,
&instance->reference,
&instance->reference_len);
if (ret == SNMP_ERR_NOERROR) {
/* search column */
const struct snmp_table_simple_col_def *col_def = table_node->columns;
u32_t i = table_node->column_count;
while (i > 0) {
if (col_def->index == instance->instance_oid.id[1]) {
break;
}
col_def++;
i--;
}
if (i > 0) {
instance->asn1_type = col_def->asn1_type;
instance->access = SNMP_NODE_INSTANCE_READ_ONLY;
instance->set_test = NULL;
instance->set_value = NULL;
switch (col_def->data_type) {
case SNMP_VARIANT_VALUE_TYPE_U32:
instance->get_value = snmp_table_extract_value_from_u32ref;
break;
case SNMP_VARIANT_VALUE_TYPE_S32:
instance->get_value = snmp_table_extract_value_from_s32ref;
break;
case SNMP_VARIANT_VALUE_TYPE_PTR: /* fall through */
case SNMP_VARIANT_VALUE_TYPE_CONST_PTR:
instance->get_value = snmp_table_extract_value_from_refconstptr;
break;
default:
LWIP_DEBUGF(SNMP_DEBUG, ("snmp_table_simple_get_instance(): unknown column data_type: %d\n", col_def->data_type));
return SNMP_ERR_GENERROR;
}
ret = SNMP_ERR_NOERROR;
} else {
ret = SNMP_ERR_NOSUCHINSTANCE;
}
}
}
return ret;
}
snmp_err_t snmp_table_simple_get_next_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
const struct snmp_table_simple_node *table_node = (const struct snmp_table_simple_node *)(const void *)instance->node;
const struct snmp_table_simple_col_def *col_def;
struct snmp_obj_id row_oid;
u32_t column = 0;
snmp_err_t result;
LWIP_UNUSED_ARG(root_oid);
LWIP_UNUSED_ARG(root_oid_len);
/* check that first part of id is 0 or 1, referencing fixed row entry */
if ((instance->instance_oid.len > 0) && (instance->instance_oid.id[0] > 1)) {
return SNMP_ERR_NOSUCHINSTANCE;
}
if (instance->instance_oid.len > 1) {
column = instance->instance_oid.id[1];
}
if (instance->instance_oid.len > 2) {
snmp_oid_assign(&row_oid, &(instance->instance_oid.id[2]), instance->instance_oid.len - 2);
} else {
row_oid.len = 0;
}
/* resolve column and value */
do {
u32_t i;
const struct snmp_table_simple_col_def *next_col_def = NULL;
col_def = table_node->columns;
for (i = 0; i < table_node->column_count; i++) {
if (col_def->index == column) {
next_col_def = col_def;
break;
} else if ((col_def->index > column) && ((next_col_def == NULL) ||
(col_def->index < next_col_def->index))) {
next_col_def = col_def;
}
col_def++;
}
if (next_col_def == NULL) {
/* no further column found */
return SNMP_ERR_NOSUCHINSTANCE;
}
result = table_node->get_next_cell_instance_and_value(
&next_col_def->index,
&row_oid,
&instance->reference,
&instance->reference_len);
if (result == SNMP_ERR_NOERROR) {
col_def = next_col_def;
break;
}
row_oid.len = 0; /* reset row_oid because we switch to next column and start with the first entry there */
column = next_col_def->index + 1;
} while (1);
instance->asn1_type = col_def->asn1_type;
instance->access = SNMP_NODE_INSTANCE_READ_ONLY;
instance->set_test = NULL;
instance->set_value = NULL;
switch (col_def->data_type) {
case SNMP_VARIANT_VALUE_TYPE_U32:
instance->get_value = snmp_table_extract_value_from_u32ref;
break;
case SNMP_VARIANT_VALUE_TYPE_S32:
instance->get_value = snmp_table_extract_value_from_s32ref;
break;
case SNMP_VARIANT_VALUE_TYPE_PTR: /* fall through */
case SNMP_VARIANT_VALUE_TYPE_CONST_PTR:
instance->get_value = snmp_table_extract_value_from_refconstptr;
break;
default:
LWIP_DEBUGF(SNMP_DEBUG, ("snmp_table_simple_get_instance(): unknown column data_type: %d\n", col_def->data_type));
return SNMP_ERR_GENERROR;
}
/* build resulting oid */
instance->instance_oid.len = 2;
instance->instance_oid.id[0] = 1;
instance->instance_oid.id[1] = col_def->index;
snmp_oid_append(&instance->instance_oid, row_oid.id, row_oid.len);
return SNMP_ERR_NOERROR;
}
s16_t
snmp_table_extract_value_from_s32ref(struct snmp_node_instance *instance, void *value)
{
s32_t *dst = (s32_t *)value;
*dst = instance->reference.s32;
return sizeof(*dst);
}
s16_t
snmp_table_extract_value_from_u32ref(struct snmp_node_instance *instance, void *value)
{
u32_t *dst = (u32_t *)value;
*dst = instance->reference.u32;
return sizeof(*dst);
}
s16_t
snmp_table_extract_value_from_refconstptr(struct snmp_node_instance *instance, void *value)
{
MEMCPY(value, instance->reference.const_ptr, instance->reference_len);
return (u16_t)instance->reference_len;
}
#endif /* LWIP_SNMP */

View File

@ -0,0 +1,231 @@
/**
* @file
* SNMP thread synchronization implementation.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Dirk Ziegelmeier <dziegel@gmx.de>
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP && (NO_SYS == 0) /* don't build if not configured for use in lwipopts.h */
#include "lwip/apps/snmp_threadsync.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/sys.h"
#include <string.h>
static void
call_synced_function(struct threadsync_data *call_data, snmp_threadsync_called_fn fn)
{
sys_mutex_lock(&call_data->threadsync_node->instance->sem_usage_mutex);
call_data->threadsync_node->instance->sync_fn(fn, call_data);
sys_sem_wait(&call_data->threadsync_node->instance->sem);
sys_mutex_unlock(&call_data->threadsync_node->instance->sem_usage_mutex);
}
static void
threadsync_get_value_synced(void *ctx)
{
struct threadsync_data *call_data = (struct threadsync_data *)ctx;
if (call_data->proxy_instance.get_value != NULL) {
call_data->retval.s16 = call_data->proxy_instance.get_value(&call_data->proxy_instance, call_data->arg1.value);
} else {
call_data->retval.s16 = -1;
}
sys_sem_signal(&call_data->threadsync_node->instance->sem);
}
static s16_t
threadsync_get_value(struct snmp_node_instance *instance, void *value)
{
struct threadsync_data *call_data = (struct threadsync_data *)instance->reference.ptr;
call_data->arg1.value = value;
call_synced_function(call_data, threadsync_get_value_synced);
return call_data->retval.s16;
}
static void
threadsync_set_test_synced(void *ctx)
{
struct threadsync_data *call_data = (struct threadsync_data *)ctx;
if (call_data->proxy_instance.set_test != NULL) {
call_data->retval.err = call_data->proxy_instance.set_test(&call_data->proxy_instance, call_data->arg2.len, call_data->arg1.value);
} else {
call_data->retval.err = SNMP_ERR_NOTWRITABLE;
}
sys_sem_signal(&call_data->threadsync_node->instance->sem);
}
static snmp_err_t
threadsync_set_test(struct snmp_node_instance *instance, u16_t len, void *value)
{
struct threadsync_data *call_data = (struct threadsync_data *)instance->reference.ptr;
call_data->arg1.value = value;
call_data->arg2.len = len;
call_synced_function(call_data, threadsync_set_test_synced);
return call_data->retval.err;
}
static void
threadsync_set_value_synced(void *ctx)
{
struct threadsync_data *call_data = (struct threadsync_data *)ctx;
if (call_data->proxy_instance.set_value != NULL) {
call_data->retval.err = call_data->proxy_instance.set_value(&call_data->proxy_instance, call_data->arg2.len, call_data->arg1.value);
} else {
call_data->retval.err = SNMP_ERR_NOTWRITABLE;
}
sys_sem_signal(&call_data->threadsync_node->instance->sem);
}
static snmp_err_t
threadsync_set_value(struct snmp_node_instance *instance, u16_t len, void *value)
{
struct threadsync_data *call_data = (struct threadsync_data *)instance->reference.ptr;
call_data->arg1.value = value;
call_data->arg2.len = len;
call_synced_function(call_data, threadsync_set_value_synced);
return call_data->retval.err;
}
static void
threadsync_release_instance_synced(void *ctx)
{
struct threadsync_data *call_data = (struct threadsync_data *)ctx;
call_data->proxy_instance.release_instance(&call_data->proxy_instance);
sys_sem_signal(&call_data->threadsync_node->instance->sem);
}
static void
threadsync_release_instance(struct snmp_node_instance *instance)
{
struct threadsync_data *call_data = (struct threadsync_data *)instance->reference.ptr;
if (call_data->proxy_instance.release_instance != NULL) {
call_synced_function(call_data, threadsync_release_instance_synced);
}
}
static void
get_instance_synced(void *ctx)
{
struct threadsync_data *call_data = (struct threadsync_data *)ctx;
const struct snmp_leaf_node *leaf = (const struct snmp_leaf_node *)(const void *)call_data->proxy_instance.node;
call_data->retval.err = leaf->get_instance(call_data->arg1.root_oid, call_data->arg2.root_oid_len, &call_data->proxy_instance);
sys_sem_signal(&call_data->threadsync_node->instance->sem);
}
static void
get_next_instance_synced(void *ctx)
{
struct threadsync_data *call_data = (struct threadsync_data *)ctx;
const struct snmp_leaf_node *leaf = (const struct snmp_leaf_node *)(const void *)call_data->proxy_instance.node;
call_data->retval.err = leaf->get_next_instance(call_data->arg1.root_oid, call_data->arg2.root_oid_len, &call_data->proxy_instance);
sys_sem_signal(&call_data->threadsync_node->instance->sem);
}
static snmp_err_t
do_sync(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance, snmp_threadsync_called_fn fn)
{
const struct snmp_threadsync_node *threadsync_node = (const struct snmp_threadsync_node *)(const void *)instance->node;
struct threadsync_data *call_data = &threadsync_node->instance->data;
if (threadsync_node->node.node.oid != threadsync_node->target->node.oid) {
LWIP_DEBUGF(SNMP_DEBUG, ("Sync node OID does not match target node OID"));
return SNMP_ERR_NOSUCHINSTANCE;
}
memset(&call_data->proxy_instance, 0, sizeof(call_data->proxy_instance));
instance->reference.ptr = call_data;
snmp_oid_assign(&call_data->proxy_instance.instance_oid, instance->instance_oid.id, instance->instance_oid.len);
call_data->proxy_instance.node = &threadsync_node->target->node;
call_data->threadsync_node = threadsync_node;
call_data->arg1.root_oid = root_oid;
call_data->arg2.root_oid_len = root_oid_len;
call_synced_function(call_data, fn);
if (call_data->retval.err == SNMP_ERR_NOERROR) {
instance->access = call_data->proxy_instance.access;
instance->asn1_type = call_data->proxy_instance.asn1_type;
instance->release_instance = threadsync_release_instance;
instance->get_value = (call_data->proxy_instance.get_value != NULL) ? threadsync_get_value : NULL;
instance->set_value = (call_data->proxy_instance.set_value != NULL) ? threadsync_set_value : NULL;
instance->set_test = (call_data->proxy_instance.set_test != NULL) ? threadsync_set_test : NULL;
snmp_oid_assign(&instance->instance_oid, call_data->proxy_instance.instance_oid.id, call_data->proxy_instance.instance_oid.len);
}
return call_data->retval.err;
}
snmp_err_t
snmp_threadsync_get_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
return do_sync(root_oid, root_oid_len, instance, get_instance_synced);
}
snmp_err_t
snmp_threadsync_get_next_instance(const u32_t *root_oid, u8_t root_oid_len, struct snmp_node_instance *instance)
{
return do_sync(root_oid, root_oid_len, instance, get_next_instance_synced);
}
/** Initializes thread synchronization instance */
void snmp_threadsync_init(struct snmp_threadsync_instance *instance, snmp_threadsync_synchronizer_fn sync_fn)
{
err_t err = sys_mutex_new(&instance->sem_usage_mutex);
LWIP_ASSERT("Failed to set up mutex", err == ERR_OK);
err = sys_sem_new(&instance->sem, 0);
LWIP_UNUSED_ARG(err); /* in case of LWIP_NOASSERT */
LWIP_ASSERT("Failed to set up semaphore", err == ERR_OK);
instance->sync_fn = sync_fn;
}
#endif /* LWIP_SNMP */

View File

@ -0,0 +1,458 @@
/**
* @file
* SNMPv1 traps implementation.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Martin Hentschel
* Christiaan Simons <christiaan.simons@axon.tv>
*
*/
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
#include <string.h>
#include "lwip/snmp.h"
#include "lwip/sys.h"
#include "lwip/apps/snmp.h"
#include "lwip/apps/snmp_core.h"
#include "lwip/prot/iana.h"
#include "snmp_msg.h"
#include "snmp_asn1.h"
#include "snmp_core_priv.h"
struct snmp_msg_trap {
/* source enterprise ID (sysObjectID) */
const struct snmp_obj_id *enterprise;
/* source IP address, raw network order format */
ip_addr_t sip;
/* generic trap code */
u32_t gen_trap;
/* specific trap code */
u32_t spc_trap;
/* timestamp */
u32_t ts;
/* snmp_version */
u32_t snmp_version;
/* output trap lengths used in ASN encoding */
/* encoding pdu length */
u16_t pdulen;
/* encoding community length */
u16_t comlen;
/* encoding sequence length */
u16_t seqlen;
/* encoding varbinds sequence length */
u16_t vbseqlen;
};
static u16_t snmp_trap_varbind_sum(struct snmp_msg_trap *trap, struct snmp_varbind *varbinds);
static u16_t snmp_trap_header_sum(struct snmp_msg_trap *trap, u16_t vb_len);
static err_t snmp_trap_header_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream);
static err_t snmp_trap_varbind_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream, struct snmp_varbind *varbinds);
#define BUILD_EXEC(code) \
if ((code) != ERR_OK) { \
LWIP_DEBUGF(SNMP_DEBUG, ("SNMP error during creation of outbound trap frame!")); \
return ERR_ARG; \
}
/** Agent community string for sending traps */
extern const char *snmp_community_trap;
void *snmp_traps_handle;
struct snmp_trap_dst {
/* destination IP address in network order */
ip_addr_t dip;
/* set to 0 when disabled, >0 when enabled */
u8_t enable;
};
static struct snmp_trap_dst trap_dst[SNMP_TRAP_DESTINATIONS];
static u8_t snmp_auth_traps_enabled = 0;
/**
* @ingroup snmp_traps
* Sets enable switch for this trap destination.
* @param dst_idx index in 0 .. SNMP_TRAP_DESTINATIONS-1
* @param enable switch if 0 destination is disabled >0 enabled.
*/
void
snmp_trap_dst_enable(u8_t dst_idx, u8_t enable)
{
LWIP_ASSERT_CORE_LOCKED();
if (dst_idx < SNMP_TRAP_DESTINATIONS) {
trap_dst[dst_idx].enable = enable;
}
}
/**
* @ingroup snmp_traps
* Sets IPv4 address for this trap destination.
* @param dst_idx index in 0 .. SNMP_TRAP_DESTINATIONS-1
* @param dst IPv4 address in host order.
*/
void
snmp_trap_dst_ip_set(u8_t dst_idx, const ip_addr_t *dst)
{
LWIP_ASSERT_CORE_LOCKED();
if (dst_idx < SNMP_TRAP_DESTINATIONS) {
ip_addr_set(&trap_dst[dst_idx].dip, dst);
}
}
/**
* @ingroup snmp_traps
* Enable/disable authentication traps
*/
void
snmp_set_auth_traps_enabled(u8_t enable)
{
snmp_auth_traps_enabled = enable;
}
/**
* @ingroup snmp_traps
* Get authentication traps enabled state
*/
u8_t
snmp_get_auth_traps_enabled(void)
{
return snmp_auth_traps_enabled;
}
/**
* @ingroup snmp_traps
* Sends a generic or enterprise specific trap message.
*
* @param eoid points to enterprise object identifier
* @param generic_trap is the trap code
* @param specific_trap used for enterprise traps when generic_trap == 6
* @param varbinds linked list of varbinds to be sent
* @return ERR_OK when success, ERR_MEM if we're out of memory
*
* @note the use of the enterprise identifier field
* is per RFC1215.
* Use .iso.org.dod.internet.mgmt.mib-2.snmp for generic traps
* and .iso.org.dod.internet.private.enterprises.yourenterprise
* (sysObjectID) for specific traps.
*/
err_t
snmp_send_trap(const struct snmp_obj_id *eoid, s32_t generic_trap, s32_t specific_trap, struct snmp_varbind *varbinds)
{
struct snmp_msg_trap trap_msg;
struct snmp_trap_dst *td;
struct pbuf *p;
u16_t i, tot_len;
err_t err = ERR_OK;
LWIP_ASSERT_CORE_LOCKED();
trap_msg.snmp_version = 0;
for (i = 0, td = &trap_dst[0]; i < SNMP_TRAP_DESTINATIONS; i++, td++) {
if ((td->enable != 0) && !ip_addr_isany(&td->dip)) {
/* lookup current source address for this dst */
if (snmp_get_local_ip_for_dst(snmp_traps_handle, &td->dip, &trap_msg.sip)) {
if (eoid == NULL) {
trap_msg.enterprise = snmp_get_device_enterprise_oid();
} else {
trap_msg.enterprise = eoid;
}
trap_msg.gen_trap = generic_trap;
if (generic_trap == SNMP_GENTRAP_ENTERPRISE_SPECIFIC) {
trap_msg.spc_trap = specific_trap;
} else {
trap_msg.spc_trap = 0;
}
MIB2_COPY_SYSUPTIME_TO(&trap_msg.ts);
/* pass 0, calculate length fields */
tot_len = snmp_trap_varbind_sum(&trap_msg, varbinds);
tot_len = snmp_trap_header_sum(&trap_msg, tot_len);
/* allocate pbuf(s) */
p = pbuf_alloc(PBUF_TRANSPORT, tot_len, PBUF_RAM);
if (p != NULL) {
struct snmp_pbuf_stream pbuf_stream;
snmp_pbuf_stream_init(&pbuf_stream, p, 0, tot_len);
/* pass 1, encode packet into the pbuf(s) */
snmp_trap_header_enc(&trap_msg, &pbuf_stream);
snmp_trap_varbind_enc(&trap_msg, &pbuf_stream, varbinds);
snmp_stats.outtraps++;
snmp_stats.outpkts++;
/** send to the TRAP destination */
snmp_sendto(snmp_traps_handle, p, &td->dip, LWIP_IANA_PORT_SNMP_TRAP);
pbuf_free(p);
} else {
err = ERR_MEM;
}
} else {
/* routing error */
err = ERR_RTE;
}
}
}
return err;
}
/**
* @ingroup snmp_traps
* Send generic SNMP trap
*/
err_t
snmp_send_trap_generic(s32_t generic_trap)
{
static const struct snmp_obj_id oid = { 7, { 1, 3, 6, 1, 2, 1, 11 } };
return snmp_send_trap(&oid, generic_trap, 0, NULL);
}
/**
* @ingroup snmp_traps
* Send specific SNMP trap with variable bindings
*/
err_t
snmp_send_trap_specific(s32_t specific_trap, struct snmp_varbind *varbinds)
{
return snmp_send_trap(NULL, SNMP_GENTRAP_ENTERPRISE_SPECIFIC, specific_trap, varbinds);
}
/**
* @ingroup snmp_traps
* Send coldstart trap
*/
void
snmp_coldstart_trap(void)
{
snmp_send_trap_generic(SNMP_GENTRAP_COLDSTART);
}
/**
* @ingroup snmp_traps
* Send authentication failure trap (used internally by agent)
*/
void
snmp_authfail_trap(void)
{
if (snmp_auth_traps_enabled != 0) {
snmp_send_trap_generic(SNMP_GENTRAP_AUTH_FAILURE);
}
}
static u16_t
snmp_trap_varbind_sum(struct snmp_msg_trap *trap, struct snmp_varbind *varbinds)
{
struct snmp_varbind *varbind;
u16_t tot_len;
u8_t tot_len_len;
tot_len = 0;
varbind = varbinds;
while (varbind != NULL) {
struct snmp_varbind_len len;
if (snmp_varbind_length(varbind, &len) == ERR_OK) {
tot_len += 1 + len.vb_len_len + len.vb_value_len;
}
varbind = varbind->next;
}
trap->vbseqlen = tot_len;
snmp_asn1_enc_length_cnt(trap->vbseqlen, &tot_len_len);
tot_len += 1 + tot_len_len;
return tot_len;
}
/**
* Sums trap header field lengths from tail to head and
* returns trap_header_lengths for second encoding pass.
*
* @param trap Trap message
* @param vb_len varbind-list length
* @return the required length for encoding the trap header
*/
static u16_t
snmp_trap_header_sum(struct snmp_msg_trap *trap, u16_t vb_len)
{
u16_t tot_len;
u16_t len;
u8_t lenlen;
tot_len = vb_len;
snmp_asn1_enc_u32t_cnt(trap->ts, &len);
snmp_asn1_enc_length_cnt(len, &lenlen);
tot_len += 1 + len + lenlen;
snmp_asn1_enc_s32t_cnt(trap->spc_trap, &len);
snmp_asn1_enc_length_cnt(len, &lenlen);
tot_len += 1 + len + lenlen;
snmp_asn1_enc_s32t_cnt(trap->gen_trap, &len);
snmp_asn1_enc_length_cnt(len, &lenlen);
tot_len += 1 + len + lenlen;
if (IP_IS_V6_VAL(trap->sip)) {
#if LWIP_IPV6
len = sizeof(ip_2_ip6(&trap->sip)->addr);
#endif
} else {
#if LWIP_IPV4
len = sizeof(ip_2_ip4(&trap->sip)->addr);
#endif
}
snmp_asn1_enc_length_cnt(len, &lenlen);
tot_len += 1 + len + lenlen;
snmp_asn1_enc_oid_cnt(trap->enterprise->id, trap->enterprise->len, &len);
snmp_asn1_enc_length_cnt(len, &lenlen);
tot_len += 1 + len + lenlen;
trap->pdulen = tot_len;
snmp_asn1_enc_length_cnt(trap->pdulen, &lenlen);
tot_len += 1 + lenlen;
trap->comlen = (u16_t)LWIP_MIN(strlen(snmp_community_trap), 0xFFFF);
snmp_asn1_enc_length_cnt(trap->comlen, &lenlen);
tot_len += 1 + lenlen + trap->comlen;
snmp_asn1_enc_s32t_cnt(trap->snmp_version, &len);
snmp_asn1_enc_length_cnt(len, &lenlen);
tot_len += 1 + len + lenlen;
trap->seqlen = tot_len;
snmp_asn1_enc_length_cnt(trap->seqlen, &lenlen);
tot_len += 1 + lenlen;
return tot_len;
}
static err_t
snmp_trap_varbind_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream, struct snmp_varbind *varbinds)
{
struct snmp_asn1_tlv tlv;
struct snmp_varbind *varbind;
varbind = varbinds;
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 0, trap->vbseqlen);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
while (varbind != NULL) {
BUILD_EXEC( snmp_append_outbound_varbind(pbuf_stream, varbind) );
varbind = varbind->next;
}
return ERR_OK;
}
/**
* Encodes trap header from head to tail.
*/
static err_t
snmp_trap_header_enc(struct snmp_msg_trap *trap, struct snmp_pbuf_stream *pbuf_stream)
{
struct snmp_asn1_tlv tlv;
/* 'Message' sequence */
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_SEQUENCE, 0, trap->seqlen);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
/* version */
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
snmp_asn1_enc_s32t_cnt(trap->snmp_version, &tlv.value_len);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, trap->snmp_version) );
/* community */
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OCTET_STRING, 0, trap->comlen);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
BUILD_EXEC( snmp_asn1_enc_raw(pbuf_stream, (const u8_t *)snmp_community_trap, trap->comlen) );
/* 'PDU' sequence */
SNMP_ASN1_SET_TLV_PARAMS(tlv, (SNMP_ASN1_CLASS_CONTEXT | SNMP_ASN1_CONTENTTYPE_CONSTRUCTED | SNMP_ASN1_CONTEXT_PDU_TRAP), 0, trap->pdulen);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
/* object ID */
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_OBJECT_ID, 0, 0);
snmp_asn1_enc_oid_cnt(trap->enterprise->id, trap->enterprise->len, &tlv.value_len);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
BUILD_EXEC( snmp_asn1_enc_oid(pbuf_stream, trap->enterprise->id, trap->enterprise->len) );
/* IP addr */
if (IP_IS_V6_VAL(trap->sip)) {
#if LWIP_IPV6
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_IPADDR, 0, sizeof(ip_2_ip6(&trap->sip)->addr));
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
BUILD_EXEC( snmp_asn1_enc_raw(pbuf_stream, (const u8_t *)&ip_2_ip6(&trap->sip)->addr, sizeof(ip_2_ip6(&trap->sip)->addr)) );
#endif
} else {
#if LWIP_IPV4
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_IPADDR, 0, sizeof(ip_2_ip4(&trap->sip)->addr));
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
BUILD_EXEC( snmp_asn1_enc_raw(pbuf_stream, (const u8_t *)&ip_2_ip4(&trap->sip)->addr, sizeof(ip_2_ip4(&trap->sip)->addr)) );
#endif
}
/* trap length */
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
snmp_asn1_enc_s32t_cnt(trap->gen_trap, &tlv.value_len);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, trap->gen_trap) );
/* specific trap */
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_INTEGER, 0, 0);
snmp_asn1_enc_s32t_cnt(trap->spc_trap, &tlv.value_len);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, trap->spc_trap) );
/* timestamp */
SNMP_ASN1_SET_TLV_PARAMS(tlv, SNMP_ASN1_TYPE_TIMETICKS, 0, 0);
snmp_asn1_enc_s32t_cnt(trap->ts, &tlv.value_len);
BUILD_EXEC( snmp_ans1_enc_tlv(pbuf_stream, &tlv) );
BUILD_EXEC( snmp_asn1_enc_s32t(pbuf_stream, tlv.value_len, trap->ts) );
return ERR_OK;
}
#endif /* LWIP_SNMP */

View File

@ -0,0 +1,136 @@
/**
* @file
* Additional SNMPv3 functionality RFC3414 and RFC3826.
*/
/*
* Copyright (c) 2016 Elias Oenal.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Elias Oenal <lwip@eliasoenal.com>
*/
#include "snmpv3_priv.h"
#include "lwip/apps/snmpv3.h"
#include "lwip/sys.h"
#include <string.h>
#if LWIP_SNMP && LWIP_SNMP_V3
#ifdef LWIP_SNMPV3_INCLUDE_ENGINE
#include LWIP_SNMPV3_INCLUDE_ENGINE
#endif
#define SNMP_MAX_TIME_BOOT 2147483647UL
/** Call this if engine has been changed. Has to reset boots, see below */
void
snmpv3_engine_id_changed(void)
{
snmpv3_set_engine_boots(0);
}
/** According to RFC3414 2.2.2.
*
* The number of times that the SNMP engine has
* (re-)initialized itself since snmpEngineID
* was last configured.
*/
s32_t
snmpv3_get_engine_boots_internal(void)
{
if (snmpv3_get_engine_boots() == 0 ||
snmpv3_get_engine_boots() < SNMP_MAX_TIME_BOOT) {
return snmpv3_get_engine_boots();
}
snmpv3_set_engine_boots(SNMP_MAX_TIME_BOOT);
return snmpv3_get_engine_boots();
}
/** RFC3414 2.2.2.
*
* Once the timer reaches 2147483647 it gets reset to zero and the
* engine boot ups get incremented.
*/
s32_t
snmpv3_get_engine_time_internal(void)
{
if (snmpv3_get_engine_time() >= SNMP_MAX_TIME_BOOT) {
snmpv3_reset_engine_time();
if (snmpv3_get_engine_boots() < SNMP_MAX_TIME_BOOT - 1) {
snmpv3_set_engine_boots(snmpv3_get_engine_boots() + 1);
} else {
snmpv3_set_engine_boots(SNMP_MAX_TIME_BOOT);
}
}
return snmpv3_get_engine_time();
}
#if LWIP_SNMP_V3_CRYPTO
/* This function ignores the byte order suggestion in RFC3414
* since it simply doesn't influence the effectiveness of an IV.
*
* Implementing RFC3826 priv param algorithm if LWIP_RAND is available.
*
* @todo: This is a potential thread safety issue.
*/
err_t
snmpv3_build_priv_param(u8_t *priv_param)
{
#ifdef LWIP_RAND /* Based on RFC3826 */
static u8_t init;
static u32_t priv1, priv2;
/* Lazy initialisation */
if (init == 0) {
init = 1;
priv1 = LWIP_RAND();
priv2 = LWIP_RAND();
}
SMEMCPY(&priv_param[0], &priv1, sizeof(priv1));
SMEMCPY(&priv_param[4], &priv2, sizeof(priv2));
/* Emulate 64bit increment */
priv1++;
if (!priv1) { /* Overflow */
priv2++;
}
#else /* Based on RFC3414 */
static u32_t ctr;
u32_t boots = snmpv3_get_engine_boots_internal();
SMEMCPY(&priv_param[0], &boots, 4);
SMEMCPY(&priv_param[4], &ctr, 4);
ctr++;
#endif
return ERR_OK;
}
#endif /* LWIP_SNMP_V3_CRYPTO */
#endif

View File

@ -0,0 +1,342 @@
/**
* @file
* SNMPv3 crypto/auth functions implemented for ARM mbedtls.
*/
/*
* Copyright (c) 2016 Elias Oenal and Dirk Ziegelmeier.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Elias Oenal <lwip@eliasoenal.com>
* Dirk Ziegelmeier <dirk@ziegelmeier.net>
*/
#include "lwip/apps/snmpv3.h"
#include "snmpv3_priv.h"
#include "lwip/arch.h"
#include "snmp_msg.h"
#include "lwip/sys.h"
#include <string.h>
#if LWIP_SNMP && LWIP_SNMP_V3 && LWIP_SNMP_V3_MBEDTLS
#include "mbedtls/md.h"
#include "mbedtls/cipher.h"
#include "mbedtls/md5.h"
#include "mbedtls/sha1.h"
err_t
snmpv3_auth(struct snmp_pbuf_stream *stream, u16_t length,
const u8_t *key, snmpv3_auth_algo_t algo, u8_t *hmac_out)
{
u32_t i;
u8_t key_len;
const mbedtls_md_info_t *md_info;
mbedtls_md_context_t ctx;
struct snmp_pbuf_stream read_stream;
snmp_pbuf_stream_init(&read_stream, stream->pbuf, stream->offset, stream->length);
if (algo == SNMP_V3_AUTH_ALGO_MD5) {
md_info = mbedtls_md_info_from_type(MBEDTLS_MD_MD5);
key_len = SNMP_V3_MD5_LEN;
} else if (algo == SNMP_V3_AUTH_ALGO_SHA) {
md_info = mbedtls_md_info_from_type(MBEDTLS_MD_SHA1);
key_len = SNMP_V3_SHA_LEN;
} else {
return ERR_ARG;
}
mbedtls_md_init(&ctx);
if (mbedtls_md_setup(&ctx, md_info, 1) != 0) {
return ERR_ARG;
}
if (mbedtls_md_hmac_starts(&ctx, key, key_len) != 0) {
goto free_md;
}
for (i = 0; i < length; i++) {
u8_t byte;
if (snmp_pbuf_stream_read(&read_stream, &byte)) {
goto free_md;
}
if (mbedtls_md_hmac_update(&ctx, &byte, 1) != 0) {
goto free_md;
}
}
if (mbedtls_md_hmac_finish(&ctx, hmac_out) != 0) {
goto free_md;
}
mbedtls_md_free(&ctx);
return ERR_OK;
free_md:
mbedtls_md_free(&ctx);
return ERR_ARG;
}
#if LWIP_SNMP_V3_CRYPTO
err_t
snmpv3_crypt(struct snmp_pbuf_stream *stream, u16_t length,
const u8_t *key, const u8_t *priv_param, const u32_t engine_boots,
const u32_t engine_time, snmpv3_priv_algo_t algo, snmpv3_priv_mode_t mode)
{
size_t i;
mbedtls_cipher_context_t ctx;
const mbedtls_cipher_info_t *cipher_info;
struct snmp_pbuf_stream read_stream;
struct snmp_pbuf_stream write_stream;
snmp_pbuf_stream_init(&read_stream, stream->pbuf, stream->offset, stream->length);
snmp_pbuf_stream_init(&write_stream, stream->pbuf, stream->offset, stream->length);
mbedtls_cipher_init(&ctx);
if (algo == SNMP_V3_PRIV_ALGO_DES) {
u8_t iv_local[8];
u8_t out_bytes[8];
size_t out_len;
/* RFC 3414 mandates padding for DES */
if ((length & 0x07) != 0) {
return ERR_ARG;
}
cipher_info = mbedtls_cipher_info_from_type(MBEDTLS_CIPHER_DES_CBC);
if (mbedtls_cipher_setup(&ctx, cipher_info) != 0) {
return ERR_ARG;
}
if (mbedtls_cipher_set_padding_mode(&ctx, MBEDTLS_PADDING_NONE) != 0) {
return ERR_ARG;
}
if (mbedtls_cipher_setkey(&ctx, key, 8 * 8, (mode == SNMP_V3_PRIV_MODE_ENCRYPT) ? MBEDTLS_ENCRYPT : MBEDTLS_DECRYPT) != 0) {
goto error;
}
/* Prepare IV */
for (i = 0; i < LWIP_ARRAYSIZE(iv_local); i++) {
iv_local[i] = priv_param[i] ^ key[i + 8];
}
if (mbedtls_cipher_set_iv(&ctx, iv_local, LWIP_ARRAYSIZE(iv_local)) != 0) {
goto error;
}
for (i = 0; i < length; i += 8) {
size_t j;
u8_t in_bytes[8];
out_len = LWIP_ARRAYSIZE(out_bytes) ;
for (j = 0; j < LWIP_ARRAYSIZE(in_bytes); j++) {
if (snmp_pbuf_stream_read(&read_stream, &in_bytes[j]) != ERR_OK) {
goto error;
}
}
if (mbedtls_cipher_update(&ctx, in_bytes, LWIP_ARRAYSIZE(in_bytes), out_bytes, &out_len) != 0) {
goto error;
}
if (snmp_pbuf_stream_writebuf(&write_stream, out_bytes, (u16_t)out_len) != ERR_OK) {
goto error;
}
}
out_len = LWIP_ARRAYSIZE(out_bytes);
if (mbedtls_cipher_finish(&ctx, out_bytes, &out_len) != 0) {
goto error;
}
if (snmp_pbuf_stream_writebuf(&write_stream, out_bytes, (u16_t)out_len) != ERR_OK) {
goto error;
}
} else if (algo == SNMP_V3_PRIV_ALGO_AES) {
u8_t iv_local[16];
cipher_info = mbedtls_cipher_info_from_type(MBEDTLS_CIPHER_AES_128_CFB128);
if (mbedtls_cipher_setup(&ctx, cipher_info) != 0) {
return ERR_ARG;
}
if (mbedtls_cipher_setkey(&ctx, key, 16 * 8, (mode == SNMP_V3_PRIV_MODE_ENCRYPT) ? MBEDTLS_ENCRYPT : MBEDTLS_DECRYPT) != 0) {
goto error;
}
/*
* IV is the big endian concatenation of boots,
* uptime and priv param - see RFC3826.
*/
iv_local[0 + 0] = (engine_boots >> 24) & 0xFF;
iv_local[0 + 1] = (engine_boots >> 16) & 0xFF;
iv_local[0 + 2] = (engine_boots >> 8) & 0xFF;
iv_local[0 + 3] = (engine_boots >> 0) & 0xFF;
iv_local[4 + 0] = (engine_time >> 24) & 0xFF;
iv_local[4 + 1] = (engine_time >> 16) & 0xFF;
iv_local[4 + 2] = (engine_time >> 8) & 0xFF;
iv_local[4 + 3] = (engine_time >> 0) & 0xFF;
SMEMCPY(iv_local + 8, priv_param, 8);
if (mbedtls_cipher_set_iv(&ctx, iv_local, LWIP_ARRAYSIZE(iv_local)) != 0) {
goto error;
}
for (i = 0; i < length; i++) {
u8_t in_byte;
u8_t out_byte;
size_t out_len = sizeof(out_byte);
if (snmp_pbuf_stream_read(&read_stream, &in_byte) != ERR_OK) {
goto error;
}
if (mbedtls_cipher_update(&ctx, &in_byte, sizeof(in_byte), &out_byte, &out_len) != 0) {
goto error;
}
if (snmp_pbuf_stream_write(&write_stream, out_byte) != ERR_OK) {
goto error;
}
}
} else {
return ERR_ARG;
}
mbedtls_cipher_free(&ctx);
return ERR_OK;
error:
mbedtls_cipher_free(&ctx);
return ERR_OK;
}
#endif /* LWIP_SNMP_V3_CRYPTO */
/* A.2.1. Password to Key Sample Code for MD5 */
void
snmpv3_password_to_key_md5(
const u8_t *password, /* IN */
size_t passwordlen, /* IN */
const u8_t *engineID, /* IN - pointer to snmpEngineID */
u8_t engineLength,/* IN - length of snmpEngineID */
u8_t *key) /* OUT - pointer to caller 16-octet buffer */
{
mbedtls_md5_context MD;
u8_t *cp, password_buf[64];
u32_t password_index = 0;
u8_t i;
u32_t count = 0;
mbedtls_md5_init(&MD); /* initialize MD5 */
mbedtls_md5_starts(&MD);
/**********************************************/
/* Use while loop until we've done 1 Megabyte */
/**********************************************/
while (count < 1048576) {
cp = password_buf;
for (i = 0; i < 64; i++) {
/*************************************************/
/* Take the next octet of the password, wrapping */
/* to the beginning of the password as necessary.*/
/*************************************************/
*cp++ = password[password_index++ % passwordlen];
}
mbedtls_md5_update(&MD, password_buf, 64);
count += 64;
}
mbedtls_md5_finish(&MD, key); /* tell MD5 we're done */
/*****************************************************/
/* Now localize the key with the engineID and pass */
/* through MD5 to produce final key */
/* May want to ensure that engineLength <= 32, */
/* otherwise need to use a buffer larger than 64 */
/*****************************************************/
SMEMCPY(password_buf, key, 16);
MEMCPY(password_buf + 16, engineID, engineLength);
SMEMCPY(password_buf + 16 + engineLength, key, 16);
mbedtls_md5_starts(&MD);
mbedtls_md5_update(&MD, password_buf, 32 + engineLength);
mbedtls_md5_finish(&MD, key);
mbedtls_md5_free(&MD);
return;
}
/* A.2.2. Password to Key Sample Code for SHA */
void
snmpv3_password_to_key_sha(
const u8_t *password, /* IN */
size_t passwordlen, /* IN */
const u8_t *engineID, /* IN - pointer to snmpEngineID */
u8_t engineLength,/* IN - length of snmpEngineID */
u8_t *key) /* OUT - pointer to caller 20-octet buffer */
{
mbedtls_sha1_context SH;
u8_t *cp, password_buf[72];
u32_t password_index = 0;
u8_t i;
u32_t count = 0;
mbedtls_sha1_init(&SH); /* initialize SHA */
mbedtls_sha1_starts(&SH);
/**********************************************/
/* Use while loop until we've done 1 Megabyte */
/**********************************************/
while (count < 1048576) {
cp = password_buf;
for (i = 0; i < 64; i++) {
/*************************************************/
/* Take the next octet of the password, wrapping */
/* to the beginning of the password as necessary.*/
/*************************************************/
*cp++ = password[password_index++ % passwordlen];
}
mbedtls_sha1_update(&SH, password_buf, 64);
count += 64;
}
mbedtls_sha1_finish(&SH, key); /* tell SHA we're done */
/*****************************************************/
/* Now localize the key with the engineID and pass */
/* through SHA to produce final key */
/* May want to ensure that engineLength <= 32, */
/* otherwise need to use a buffer larger than 72 */
/*****************************************************/
SMEMCPY(password_buf, key, 20);
MEMCPY(password_buf + 20, engineID, engineLength);
SMEMCPY(password_buf + 20 + engineLength, key, 20);
mbedtls_sha1_starts(&SH);
mbedtls_sha1_update(&SH, password_buf, 40 + engineLength);
mbedtls_sha1_finish(&SH, key);
mbedtls_sha1_free(&SH);
return;
}
#endif /* LWIP_SNMP && LWIP_SNMP_V3 && LWIP_SNMP_V3_MBEDTLS */

View File

@ -0,0 +1,69 @@
/**
* @file
* Additional SNMPv3 functionality RFC3414 and RFC3826 (internal API, do not use in client code).
*/
/*
* Copyright (c) 2016 Elias Oenal.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* Author: Elias Oenal <lwip@eliasoenal.com>
*/
#ifndef LWIP_HDR_APPS_SNMP_V3_PRIV_H
#define LWIP_HDR_APPS_SNMP_V3_PRIV_H
#include "lwip/apps/snmp_opts.h"
#if LWIP_SNMP && LWIP_SNMP_V3
#include "lwip/apps/snmpv3.h"
#include "snmp_pbuf_stream.h"
/* According to RFC 3411 */
#define SNMP_V3_MAX_ENGINE_ID_LENGTH 32
#define SNMP_V3_MAX_USER_LENGTH 32
#define SNMP_V3_MAX_AUTH_PARAM_LENGTH 12
#define SNMP_V3_MAX_PRIV_PARAM_LENGTH 8
#define SNMP_V3_MD5_LEN 16
#define SNMP_V3_SHA_LEN 20
typedef enum {
SNMP_V3_PRIV_MODE_DECRYPT = 0,
SNMP_V3_PRIV_MODE_ENCRYPT = 1
} snmpv3_priv_mode_t;
s32_t snmpv3_get_engine_boots_internal(void);
err_t snmpv3_auth(struct snmp_pbuf_stream *stream, u16_t length, const u8_t *key, snmpv3_auth_algo_t algo, u8_t *hmac_out);
err_t snmpv3_crypt(struct snmp_pbuf_stream *stream, u16_t length, const u8_t *key,
const u8_t *priv_param, const u32_t engine_boots, const u32_t engine_time, snmpv3_priv_algo_t algo, snmpv3_priv_mode_t mode);
err_t snmpv3_build_priv_param(u8_t *priv_param);
void snmpv3_enginetime_timer(void *arg);
#endif
#endif /* LWIP_HDR_APPS_SNMP_V3_PRIV_H */

View File

@ -0,0 +1,869 @@
/**
* @file
* SNTP client module
*/
/*
* Copyright (c) 2007-2009 Frédéric Bernon, Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Frédéric Bernon, Simon Goldschmidt
*/
/**
* @defgroup sntp SNTP
* @ingroup apps
*
* This is simple "SNTP" client for the lwIP raw API.
* It is a minimal implementation of SNTPv4 as specified in RFC 4330.
*
* For a list of some public NTP servers, see this link:
* http://support.ntp.org/bin/view/Servers/NTPPoolServers
*
* @todo:
* - complete SNTP_CHECK_RESPONSE checks 3 and 4
*/
#include "lwip/apps/sntp.h"
#include "lwip/opt.h"
#include "lwip/timeouts.h"
#include "lwip/udp.h"
#include "lwip/dns.h"
#include "lwip/ip_addr.h"
#include "lwip/pbuf.h"
#include "lwip/dhcp.h"
#include <string.h>
#include <time.h>
#if LWIP_UDP
/* Handle support for more than one server via SNTP_MAX_SERVERS */
#if SNTP_MAX_SERVERS > 1
#define SNTP_SUPPORT_MULTIPLE_SERVERS 1
#else /* NTP_MAX_SERVERS > 1 */
#define SNTP_SUPPORT_MULTIPLE_SERVERS 0
#endif /* NTP_MAX_SERVERS > 1 */
#ifndef SNTP_SUPPRESS_DELAY_CHECK
#if SNTP_UPDATE_DELAY < 15000
#error "SNTPv4 RFC 4330 enforces a minimum update time of 15 seconds (define SNTP_SUPPRESS_DELAY_CHECK to disable this error)!"
#endif
#endif
/* the various debug levels for this file */
#define SNTP_DEBUG_TRACE (SNTP_DEBUG | LWIP_DBG_TRACE)
#define SNTP_DEBUG_STATE (SNTP_DEBUG | LWIP_DBG_STATE)
#define SNTP_DEBUG_WARN (SNTP_DEBUG | LWIP_DBG_LEVEL_WARNING)
#define SNTP_DEBUG_WARN_STATE (SNTP_DEBUG | LWIP_DBG_LEVEL_WARNING | LWIP_DBG_STATE)
#define SNTP_DEBUG_SERIOUS (SNTP_DEBUG | LWIP_DBG_LEVEL_SERIOUS)
#define SNTP_ERR_KOD 1
/* SNTP protocol defines */
#define SNTP_MSG_LEN 48
#define SNTP_OFFSET_LI_VN_MODE 0
#define SNTP_LI_MASK 0xC0
#define SNTP_LI_NO_WARNING (0x00 << 6)
#define SNTP_LI_LAST_MINUTE_61_SEC (0x01 << 6)
#define SNTP_LI_LAST_MINUTE_59_SEC (0x02 << 6)
#define SNTP_LI_ALARM_CONDITION (0x03 << 6) /* (clock not synchronized) */
#define SNTP_VERSION_MASK 0x38
#define SNTP_VERSION (4/* NTP Version 4*/<<3)
#define SNTP_MODE_MASK 0x07
#define SNTP_MODE_CLIENT 0x03
#define SNTP_MODE_SERVER 0x04
#define SNTP_MODE_BROADCAST 0x05
#define SNTP_OFFSET_STRATUM 1
#define SNTP_STRATUM_KOD 0x00
#define SNTP_OFFSET_ORIGINATE_TIME 24
#define SNTP_OFFSET_RECEIVE_TIME 32
#define SNTP_OFFSET_TRANSMIT_TIME 40
/* Number of seconds between 1970 and Feb 7, 2036 06:28:16 UTC (epoch 1) */
#define DIFF_SEC_1970_2036 ((u32_t)2085978496L)
/** Convert NTP timestamp fraction to microseconds.
*/
#ifndef SNTP_FRAC_TO_US
# if LWIP_HAVE_INT64
# define SNTP_FRAC_TO_US(f) ((u32_t)(((u64_t)(f) * 1000000UL) >> 32))
# else
# define SNTP_FRAC_TO_US(f) ((u32_t)(f) / 4295)
# endif
#endif /* !SNTP_FRAC_TO_US */
/* Configure behaviour depending on native, microsecond or second precision.
* Treat NTP timestamps as signed two's-complement integers. This way,
* timestamps that have the MSB set simply become negative offsets from
* the epoch (Feb 7, 2036 06:28:16 UTC). Representable dates range from
* 1968 to 2104.
*/
#ifndef SNTP_SET_SYSTEM_TIME_NTP
# ifdef SNTP_SET_SYSTEM_TIME_US
# define SNTP_SET_SYSTEM_TIME_NTP(s, f) \
SNTP_SET_SYSTEM_TIME_US((u32_t)((s) + DIFF_SEC_1970_2036), SNTP_FRAC_TO_US(f))
# else
# define SNTP_SET_SYSTEM_TIME_NTP(s, f) \
SNTP_SET_SYSTEM_TIME((u32_t)((s) + DIFF_SEC_1970_2036))
# endif
#endif /* !SNTP_SET_SYSTEM_TIME_NTP */
/* Get the system time either natively as NTP timestamp or convert from
* Unix time in seconds and microseconds. Take care to avoid overflow if the
* microsecond value is at the maximum of 999999. Also add 0.5 us fudge to
* avoid special values like 0, and to mask round-off errors that would
* otherwise break round-trip conversion identity.
*/
#ifndef SNTP_GET_SYSTEM_TIME_NTP
# define SNTP_GET_SYSTEM_TIME_NTP(s, f) do { \
u32_t sec_, usec_; \
SNTP_GET_SYSTEM_TIME(sec_, usec_); \
(s) = (s32_t)(sec_ - DIFF_SEC_1970_2036); \
(f) = usec_ * 4295 - ((usec_ * 2143) >> 16) + 2147; \
} while (0)
#endif /* !SNTP_GET_SYSTEM_TIME_NTP */
/* Start offset of the timestamps to extract from the SNTP packet */
#define SNTP_OFFSET_TIMESTAMPS \
(SNTP_OFFSET_TRANSMIT_TIME + 8 - sizeof(struct sntp_timestamps))
/* Round-trip delay arithmetic helpers */
#if SNTP_COMP_ROUNDTRIP
# if !LWIP_HAVE_INT64
# error "SNTP round-trip delay compensation requires 64-bit arithmetic"
# endif
# define SNTP_SEC_FRAC_TO_S64(s, f) \
((s64_t)(((u64_t)(s) << 32) | (u32_t)(f)))
# define SNTP_TIMESTAMP_TO_S64(t) \
SNTP_SEC_FRAC_TO_S64(lwip_ntohl((t).sec), lwip_ntohl((t).frac))
#endif /* SNTP_COMP_ROUNDTRIP */
/**
* 64-bit NTP timestamp, in network byte order.
*/
struct sntp_time {
u32_t sec;
u32_t frac;
};
/**
* Timestamps to be extracted from the NTP header.
*/
struct sntp_timestamps {
#if SNTP_COMP_ROUNDTRIP || SNTP_CHECK_RESPONSE >= 2
struct sntp_time orig;
struct sntp_time recv;
#endif
struct sntp_time xmit;
};
/**
* SNTP packet format (without optional fields)
* Timestamps are coded as 64 bits:
* - signed 32 bits seconds since Feb 07, 2036, 06:28:16 UTC (epoch 1)
* - unsigned 32 bits seconds fraction (2^32 = 1 second)
*/
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/bpstruct.h"
#endif
PACK_STRUCT_BEGIN
struct sntp_msg {
PACK_STRUCT_FLD_8(u8_t li_vn_mode);
PACK_STRUCT_FLD_8(u8_t stratum);
PACK_STRUCT_FLD_8(u8_t poll);
PACK_STRUCT_FLD_8(u8_t precision);
PACK_STRUCT_FIELD(u32_t root_delay);
PACK_STRUCT_FIELD(u32_t root_dispersion);
PACK_STRUCT_FIELD(u32_t reference_identifier);
PACK_STRUCT_FIELD(u32_t reference_timestamp[2]);
PACK_STRUCT_FIELD(u32_t originate_timestamp[2]);
PACK_STRUCT_FIELD(u32_t receive_timestamp[2]);
PACK_STRUCT_FIELD(u32_t transmit_timestamp[2]);
} PACK_STRUCT_STRUCT;
PACK_STRUCT_END
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/epstruct.h"
#endif
/* function prototypes */
static void sntp_request(void *arg);
/** The operating mode */
static u8_t sntp_opmode;
/** The UDP pcb used by the SNTP client */
static struct udp_pcb *sntp_pcb;
/** Names/Addresses of servers */
struct sntp_server {
#if SNTP_SERVER_DNS
const char *name;
#endif /* SNTP_SERVER_DNS */
ip_addr_t addr;
#if SNTP_MONITOR_SERVER_REACHABILITY
/** Reachability shift register as described in RFC 5905 */
u8_t reachability;
#endif /* SNTP_MONITOR_SERVER_REACHABILITY */
};
static struct sntp_server sntp_servers[SNTP_MAX_SERVERS];
#if SNTP_GET_SERVERS_FROM_DHCP
static u8_t sntp_set_servers_from_dhcp;
#endif /* SNTP_GET_SERVERS_FROM_DHCP */
#if SNTP_SUPPORT_MULTIPLE_SERVERS
/** The currently used server (initialized to 0) */
static u8_t sntp_current_server;
#else /* SNTP_SUPPORT_MULTIPLE_SERVERS */
#define sntp_current_server 0
#endif /* SNTP_SUPPORT_MULTIPLE_SERVERS */
#if SNTP_RETRY_TIMEOUT_EXP
#define SNTP_RESET_RETRY_TIMEOUT() sntp_retry_timeout = SNTP_RETRY_TIMEOUT
/** Retry time, initialized with SNTP_RETRY_TIMEOUT and doubled with each retry. */
static u32_t sntp_retry_timeout;
#else /* SNTP_RETRY_TIMEOUT_EXP */
#define SNTP_RESET_RETRY_TIMEOUT()
#define sntp_retry_timeout SNTP_RETRY_TIMEOUT
#endif /* SNTP_RETRY_TIMEOUT_EXP */
#if SNTP_CHECK_RESPONSE >= 1
/** Saves the last server address to compare with response */
static ip_addr_t sntp_last_server_address;
#endif /* SNTP_CHECK_RESPONSE >= 1 */
#if SNTP_CHECK_RESPONSE >= 2
/** Saves the last timestamp sent (which is sent back by the server)
* to compare against in response. Stored in network byte order. */
static struct sntp_time sntp_last_timestamp_sent;
#endif /* SNTP_CHECK_RESPONSE >= 2 */
#if defined(LWIP_DEBUG) && !defined(sntp_format_time)
/* Debug print helper. */
static const char *
sntp_format_time(s32_t sec)
{
time_t ut;
ut = (u32_t)((u32_t)sec + DIFF_SEC_1970_2036);
return ctime(&ut);
}
#endif /* LWIP_DEBUG && !sntp_format_time */
/**
* SNTP processing of received timestamp
*/
static void
sntp_process(const struct sntp_timestamps *timestamps)
{
s32_t sec;
u32_t frac;
sec = (s32_t)lwip_ntohl(timestamps->xmit.sec);
frac = lwip_ntohl(timestamps->xmit.frac);
#if SNTP_COMP_ROUNDTRIP
# if SNTP_CHECK_RESPONSE >= 2
if (timestamps->recv.sec != 0 || timestamps->recv.frac != 0)
# endif
{
s32_t dest_sec;
u32_t dest_frac;
u32_t step_sec;
/* Get the destination time stamp, i.e. the current system time */
SNTP_GET_SYSTEM_TIME_NTP(dest_sec, dest_frac);
step_sec = (dest_sec < sec) ? ((u32_t)sec - (u32_t)dest_sec)
: ((u32_t)dest_sec - (u32_t)sec);
/* In order to avoid overflows, skip the compensation if the clock step
* is larger than about 34 years. */
if ((step_sec >> 30) == 0) {
s64_t t1, t2, t3, t4;
t4 = SNTP_SEC_FRAC_TO_S64(dest_sec, dest_frac);
t3 = SNTP_SEC_FRAC_TO_S64(sec, frac);
t1 = SNTP_TIMESTAMP_TO_S64(timestamps->orig);
t2 = SNTP_TIMESTAMP_TO_S64(timestamps->recv);
/* Clock offset calculation according to RFC 4330 */
t4 += ((t2 - t1) + (t3 - t4)) / 2;
sec = (s32_t)((u64_t)t4 >> 32);
frac = (u32_t)((u64_t)t4);
}
}
#endif /* SNTP_COMP_ROUNDTRIP */
SNTP_SET_SYSTEM_TIME_NTP(sec, frac);
LWIP_UNUSED_ARG(frac); /* might be unused if only seconds are set */
LWIP_DEBUGF(SNTP_DEBUG_TRACE, ("sntp_process: %s, %" U32_F " us\n",
sntp_format_time(sec), SNTP_FRAC_TO_US(frac)));
}
/**
* Initialize request struct to be sent to server.
*/
static void
sntp_initialize_request(struct sntp_msg *req)
{
memset(req, 0, SNTP_MSG_LEN);
req->li_vn_mode = SNTP_LI_NO_WARNING | SNTP_VERSION | SNTP_MODE_CLIENT;
#if SNTP_CHECK_RESPONSE >= 2 || SNTP_COMP_ROUNDTRIP
{
s32_t secs;
u32_t sec, frac;
/* Get the transmit timestamp */
SNTP_GET_SYSTEM_TIME_NTP(secs, frac);
sec = lwip_htonl((u32_t)secs);
frac = lwip_htonl(frac);
# if SNTP_CHECK_RESPONSE >= 2
sntp_last_timestamp_sent.sec = sec;
sntp_last_timestamp_sent.frac = frac;
# endif
req->transmit_timestamp[0] = sec;
req->transmit_timestamp[1] = frac;
}
#endif /* SNTP_CHECK_RESPONSE >= 2 || SNTP_COMP_ROUNDTRIP */
}
/**
* Retry: send a new request (and increase retry timeout).
*
* @param arg is unused (only necessary to conform to sys_timeout)
*/
static void
sntp_retry(void *arg)
{
LWIP_UNUSED_ARG(arg);
LWIP_DEBUGF(SNTP_DEBUG_STATE, ("sntp_retry: Next request will be sent in %"U32_F" ms\n",
sntp_retry_timeout));
/* set up a timer to send a retry and increase the retry delay */
sys_timeout(sntp_retry_timeout, sntp_request, NULL);
#if SNTP_RETRY_TIMEOUT_EXP
{
u32_t new_retry_timeout;
/* increase the timeout for next retry */
new_retry_timeout = sntp_retry_timeout << 1;
/* limit to maximum timeout and prevent overflow */
if ((new_retry_timeout <= SNTP_RETRY_TIMEOUT_MAX) &&
(new_retry_timeout > sntp_retry_timeout)) {
sntp_retry_timeout = new_retry_timeout;
}
}
#endif /* SNTP_RETRY_TIMEOUT_EXP */
}
#if SNTP_SUPPORT_MULTIPLE_SERVERS
/**
* If Kiss-of-Death is received (or another packet parsing error),
* try the next server or retry the current server and increase the retry
* timeout if only one server is available.
* (implicitly, SNTP_MAX_SERVERS > 1)
*
* @param arg is unused (only necessary to conform to sys_timeout)
*/
static void
sntp_try_next_server(void *arg)
{
u8_t old_server, i;
LWIP_UNUSED_ARG(arg);
old_server = sntp_current_server;
for (i = 0; i < SNTP_MAX_SERVERS - 1; i++) {
sntp_current_server++;
if (sntp_current_server >= SNTP_MAX_SERVERS) {
sntp_current_server = 0;
}
if (!ip_addr_isany(&sntp_servers[sntp_current_server].addr)
#if SNTP_SERVER_DNS
|| (sntp_servers[sntp_current_server].name != NULL)
#endif
) {
LWIP_DEBUGF(SNTP_DEBUG_STATE, ("sntp_try_next_server: Sending request to server %"U16_F"\n",
(u16_t)sntp_current_server));
/* new server: reset retry timeout */
SNTP_RESET_RETRY_TIMEOUT();
/* instantly send a request to the next server */
sntp_request(NULL);
return;
}
}
/* no other valid server found */
sntp_current_server = old_server;
sntp_retry(NULL);
}
#else /* SNTP_SUPPORT_MULTIPLE_SERVERS */
/* Always retry on error if only one server is supported */
#define sntp_try_next_server sntp_retry
#endif /* SNTP_SUPPORT_MULTIPLE_SERVERS */
/** UDP recv callback for the sntp pcb */
static void
sntp_recv(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
{
struct sntp_timestamps timestamps;
u8_t mode;
u8_t stratum;
err_t err;
LWIP_UNUSED_ARG(arg);
LWIP_UNUSED_ARG(pcb);
err = ERR_ARG;
#if SNTP_CHECK_RESPONSE >= 1
/* check server address and port */
if (((sntp_opmode != SNTP_OPMODE_POLL) || ip_addr_cmp(addr, &sntp_last_server_address)) &&
(port == SNTP_PORT))
#else /* SNTP_CHECK_RESPONSE >= 1 */
LWIP_UNUSED_ARG(addr);
LWIP_UNUSED_ARG(port);
#endif /* SNTP_CHECK_RESPONSE >= 1 */
{
/* process the response */
if (p->tot_len == SNTP_MSG_LEN) {
mode = pbuf_get_at(p, SNTP_OFFSET_LI_VN_MODE) & SNTP_MODE_MASK;
/* if this is a SNTP response... */
if (((sntp_opmode == SNTP_OPMODE_POLL) && (mode == SNTP_MODE_SERVER)) ||
((sntp_opmode == SNTP_OPMODE_LISTENONLY) && (mode == SNTP_MODE_BROADCAST))) {
stratum = pbuf_get_at(p, SNTP_OFFSET_STRATUM);
if (stratum == SNTP_STRATUM_KOD) {
/* Kiss-of-death packet. Use another server or increase UPDATE_DELAY. */
err = SNTP_ERR_KOD;
LWIP_DEBUGF(SNTP_DEBUG_STATE, ("sntp_recv: Received Kiss-of-Death\n"));
} else {
pbuf_copy_partial(p, &timestamps, sizeof(timestamps), SNTP_OFFSET_TIMESTAMPS);
#if SNTP_CHECK_RESPONSE >= 2
/* check originate_timetamp against sntp_last_timestamp_sent */
if (timestamps.orig.sec != sntp_last_timestamp_sent.sec ||
timestamps.orig.frac != sntp_last_timestamp_sent.frac) {
LWIP_DEBUGF(SNTP_DEBUG_WARN,
("sntp_recv: Invalid originate timestamp in response\n"));
} else
#endif /* SNTP_CHECK_RESPONSE >= 2 */
/* @todo: add code for SNTP_CHECK_RESPONSE >= 3 and >= 4 here */
{
/* correct answer */
err = ERR_OK;
}
}
} else {
LWIP_DEBUGF(SNTP_DEBUG_WARN, ("sntp_recv: Invalid mode in response: %"U16_F"\n", (u16_t)mode));
/* wait for correct response */
err = ERR_TIMEOUT;
}
} else {
LWIP_DEBUGF(SNTP_DEBUG_WARN, ("sntp_recv: Invalid packet length: %"U16_F"\n", p->tot_len));
}
}
#if SNTP_CHECK_RESPONSE >= 1
else {
/* packet from wrong remote address or port, wait for correct response */
err = ERR_TIMEOUT;
}
#endif /* SNTP_CHECK_RESPONSE >= 1 */
pbuf_free(p);
if (err == ERR_OK) {
/* correct packet received: process it it */
sntp_process(&timestamps);
#if SNTP_MONITOR_SERVER_REACHABILITY
/* indicate that server responded */
sntp_servers[sntp_current_server].reachability |= 1;
#endif /* SNTP_MONITOR_SERVER_REACHABILITY */
/* Set up timeout for next request (only if poll response was received)*/
if (sntp_opmode == SNTP_OPMODE_POLL) {
u32_t sntp_update_delay;
sys_untimeout(sntp_try_next_server, NULL);
sys_untimeout(sntp_request, NULL);
/* Correct response, reset retry timeout */
SNTP_RESET_RETRY_TIMEOUT();
sntp_update_delay = (u32_t)SNTP_UPDATE_DELAY;
sys_timeout(sntp_update_delay, sntp_request, NULL);
LWIP_DEBUGF(SNTP_DEBUG_STATE, ("sntp_recv: Scheduled next time request: %"U32_F" ms\n",
sntp_update_delay));
}
} else if (err == SNTP_ERR_KOD) {
/* KOD errors are only processed in case of an explicit poll response */
if (sntp_opmode == SNTP_OPMODE_POLL) {
/* Kiss-of-death packet. Use another server or increase UPDATE_DELAY. */
sntp_try_next_server(NULL);
}
} else {
/* ignore any broken packet, poll mode: retry after timeout to avoid flooding */
}
}
/** Actually send an sntp request to a server.
*
* @param server_addr resolved IP address of the SNTP server
*/
static void
sntp_send_request(const ip_addr_t *server_addr)
{
struct pbuf *p;
LWIP_ASSERT("server_addr != NULL", server_addr != NULL);
p = pbuf_alloc(PBUF_TRANSPORT, SNTP_MSG_LEN, PBUF_RAM);
if (p != NULL) {
struct sntp_msg *sntpmsg = (struct sntp_msg *)p->payload;
LWIP_DEBUGF(SNTP_DEBUG_STATE, ("sntp_send_request: Sending request to server\n"));
/* initialize request message */
sntp_initialize_request(sntpmsg);
/* send request */
udp_sendto(sntp_pcb, p, server_addr, SNTP_PORT);
/* free the pbuf after sending it */
pbuf_free(p);
#if SNTP_MONITOR_SERVER_REACHABILITY
/* indicate new packet has been sent */
sntp_servers[sntp_current_server].reachability <<= 1;
#endif /* SNTP_MONITOR_SERVER_REACHABILITY */
/* set up receive timeout: try next server or retry on timeout */
sys_timeout((u32_t)SNTP_RECV_TIMEOUT, sntp_try_next_server, NULL);
#if SNTP_CHECK_RESPONSE >= 1
/* save server address to verify it in sntp_recv */
ip_addr_copy(sntp_last_server_address, *server_addr);
#endif /* SNTP_CHECK_RESPONSE >= 1 */
} else {
LWIP_DEBUGF(SNTP_DEBUG_SERIOUS, ("sntp_send_request: Out of memory, trying again in %"U32_F" ms\n",
(u32_t)SNTP_RETRY_TIMEOUT));
/* out of memory: set up a timer to send a retry */
sys_timeout((u32_t)SNTP_RETRY_TIMEOUT, sntp_request, NULL);
}
}
#if SNTP_SERVER_DNS
/**
* DNS found callback when using DNS names as server address.
*/
static void
sntp_dns_found(const char *hostname, const ip_addr_t *ipaddr, void *arg)
{
LWIP_UNUSED_ARG(hostname);
LWIP_UNUSED_ARG(arg);
if (ipaddr != NULL) {
/* Address resolved, send request */
LWIP_DEBUGF(SNTP_DEBUG_STATE, ("sntp_dns_found: Server address resolved, sending request\n"));
sntp_servers[sntp_current_server].addr = *ipaddr;
sntp_send_request(ipaddr);
} else {
/* DNS resolving failed -> try another server */
LWIP_DEBUGF(SNTP_DEBUG_WARN_STATE, ("sntp_dns_found: Failed to resolve server address resolved, trying next server\n"));
sntp_try_next_server(NULL);
}
}
#endif /* SNTP_SERVER_DNS */
/**
* Send out an sntp request.
*
* @param arg is unused (only necessary to conform to sys_timeout)
*/
static void
sntp_request(void *arg)
{
ip_addr_t sntp_server_address;
err_t err;
LWIP_UNUSED_ARG(arg);
/* initialize SNTP server address */
#if SNTP_SERVER_DNS
if (sntp_servers[sntp_current_server].name) {
/* always resolve the name and rely on dns-internal caching & timeout */
ip_addr_set_zero(&sntp_servers[sntp_current_server].addr);
err = dns_gethostbyname(sntp_servers[sntp_current_server].name, &sntp_server_address,
sntp_dns_found, NULL);
if (err == ERR_INPROGRESS) {
/* DNS request sent, wait for sntp_dns_found being called */
LWIP_DEBUGF(SNTP_DEBUG_STATE, ("sntp_request: Waiting for server address to be resolved.\n"));
return;
} else if (err == ERR_OK) {
sntp_servers[sntp_current_server].addr = sntp_server_address;
}
} else
#endif /* SNTP_SERVER_DNS */
{
sntp_server_address = sntp_servers[sntp_current_server].addr;
err = (ip_addr_isany_val(sntp_server_address)) ? ERR_ARG : ERR_OK;
}
if (err == ERR_OK) {
LWIP_DEBUGF(SNTP_DEBUG_TRACE, ("sntp_request: current server address is %s\n",
ipaddr_ntoa(&sntp_server_address)));
sntp_send_request(&sntp_server_address);
} else {
/* address conversion failed, try another server */
LWIP_DEBUGF(SNTP_DEBUG_WARN_STATE, ("sntp_request: Invalid server address, trying next server.\n"));
sys_timeout((u32_t)SNTP_RETRY_TIMEOUT, sntp_try_next_server, NULL);
}
}
/**
* @ingroup sntp
* Initialize this module.
* Send out request instantly or after SNTP_STARTUP_DELAY(_FUNC).
*/
void
sntp_init(void)
{
/* LWIP_ASSERT_CORE_LOCKED(); is checked by udp_new() */
#ifdef SNTP_SERVER_ADDRESS
#if SNTP_SERVER_DNS
sntp_setservername(0, SNTP_SERVER_ADDRESS);
#else
#error SNTP_SERVER_ADDRESS string not supported SNTP_SERVER_DNS==0
#endif
#endif /* SNTP_SERVER_ADDRESS */
if (sntp_pcb == NULL) {
sntp_pcb = udp_new_ip_type(IPADDR_TYPE_ANY);
LWIP_ASSERT("Failed to allocate udp pcb for sntp client", sntp_pcb != NULL);
if (sntp_pcb != NULL) {
udp_recv(sntp_pcb, sntp_recv, NULL);
if (sntp_opmode == SNTP_OPMODE_POLL) {
SNTP_RESET_RETRY_TIMEOUT();
#if SNTP_STARTUP_DELAY
sys_timeout((u32_t)SNTP_STARTUP_DELAY_FUNC, sntp_request, NULL);
#else
sntp_request(NULL);
#endif
} else if (sntp_opmode == SNTP_OPMODE_LISTENONLY) {
ip_set_option(sntp_pcb, SOF_BROADCAST);
udp_bind(sntp_pcb, IP_ANY_TYPE, SNTP_PORT);
}
}
}
}
/**
* @ingroup sntp
* Stop this module.
*/
void
sntp_stop(void)
{
LWIP_ASSERT_CORE_LOCKED();
if (sntp_pcb != NULL) {
#if SNTP_MONITOR_SERVER_REACHABILITY
u8_t i;
for (i = 0; i < SNTP_MAX_SERVERS; i++) {
sntp_servers[i].reachability = 0;
}
#endif /* SNTP_MONITOR_SERVER_REACHABILITY */
sys_untimeout(sntp_request, NULL);
sys_untimeout(sntp_try_next_server, NULL);
udp_remove(sntp_pcb);
sntp_pcb = NULL;
}
}
/**
* @ingroup sntp
* Get enabled state.
*/
u8_t sntp_enabled(void)
{
return (sntp_pcb != NULL) ? 1 : 0;
}
/**
* @ingroup sntp
* Sets the operating mode.
* @param operating_mode one of the available operating modes
*/
void
sntp_setoperatingmode(u8_t operating_mode)
{
LWIP_ASSERT_CORE_LOCKED();
LWIP_ASSERT("Invalid operating mode", operating_mode <= SNTP_OPMODE_LISTENONLY);
LWIP_ASSERT("Operating mode must not be set while SNTP client is running", sntp_pcb == NULL);
sntp_opmode = operating_mode;
}
/**
* @ingroup sntp
* Gets the operating mode.
*/
u8_t
sntp_getoperatingmode(void)
{
return sntp_opmode;
}
#if SNTP_MONITOR_SERVER_REACHABILITY
/**
* @ingroup sntp
* Gets the server reachability shift register as described in RFC 5905.
*
* @param idx the index of the NTP server
*/
u8_t
sntp_getreachability(u8_t idx)
{
if (idx < SNTP_MAX_SERVERS) {
return sntp_servers[idx].reachability;
}
return 0;
}
#endif /* SNTP_MONITOR_SERVER_REACHABILITY */
#if SNTP_GET_SERVERS_FROM_DHCP
/**
* Config SNTP server handling by IP address, name, or DHCP; clear table
* @param set_servers_from_dhcp enable or disable getting server addresses from dhcp
*/
void
sntp_servermode_dhcp(int set_servers_from_dhcp)
{
u8_t new_mode = set_servers_from_dhcp ? 1 : 0;
LWIP_ASSERT_CORE_LOCKED();
if (sntp_set_servers_from_dhcp != new_mode) {
sntp_set_servers_from_dhcp = new_mode;
}
}
#endif /* SNTP_GET_SERVERS_FROM_DHCP */
/**
* @ingroup sntp
* Initialize one of the NTP servers by IP address
*
* @param idx the index of the NTP server to set must be < SNTP_MAX_SERVERS
* @param server IP address of the NTP server to set
*/
void
sntp_setserver(u8_t idx, const ip_addr_t *server)
{
LWIP_ASSERT_CORE_LOCKED();
if (idx < SNTP_MAX_SERVERS) {
if (server != NULL) {
sntp_servers[idx].addr = (*server);
} else {
ip_addr_set_zero(&sntp_servers[idx].addr);
}
#if SNTP_SERVER_DNS
sntp_servers[idx].name = NULL;
#endif
}
}
#if LWIP_DHCP && SNTP_GET_SERVERS_FROM_DHCP
/**
* Initialize one of the NTP servers by IP address, required by DHCP
*
* @param num the index of the NTP server to set must be < SNTP_MAX_SERVERS
* @param server IP address of the NTP server to set
*/
void
dhcp_set_ntp_servers(u8_t num, const ip4_addr_t *server)
{
LWIP_DEBUGF(SNTP_DEBUG_TRACE, ("sntp: %s %u.%u.%u.%u as NTP server #%u via DHCP\n",
(sntp_set_servers_from_dhcp ? "Got" : "Rejected"),
ip4_addr1(server), ip4_addr2(server), ip4_addr3(server), ip4_addr4(server), num));
if (sntp_set_servers_from_dhcp && num) {
u8_t i;
for (i = 0; (i < num) && (i < SNTP_MAX_SERVERS); i++) {
ip_addr_t addr;
ip_addr_copy_from_ip4(addr, server[i]);
sntp_setserver(i, &addr);
}
for (i = num; i < SNTP_MAX_SERVERS; i++) {
sntp_setserver(i, NULL);
}
}
}
#endif /* LWIP_DHCP && SNTP_GET_SERVERS_FROM_DHCP */
/**
* @ingroup sntp
* Obtain one of the currently configured by IP address (or DHCP) NTP servers
*
* @param idx the index of the NTP server
* @return IP address of the indexed NTP server or "ip_addr_any" if the NTP
* server has not been configured by address (or at all).
*/
const ip_addr_t *
sntp_getserver(u8_t idx)
{
if (idx < SNTP_MAX_SERVERS) {
return &sntp_servers[idx].addr;
}
return IP_ADDR_ANY;
}
#if SNTP_SERVER_DNS
/**
* Initialize one of the NTP servers by name
*
* @param idx the index of the NTP server to set must be < SNTP_MAX_SERVERS
* @param server DNS name of the NTP server to set, to be resolved at contact time
*/
void
sntp_setservername(u8_t idx, const char *server)
{
LWIP_ASSERT_CORE_LOCKED();
if (idx < SNTP_MAX_SERVERS) {
sntp_servers[idx].name = server;
}
}
/**
* Obtain one of the currently configured by name NTP servers.
*
* @param idx the index of the NTP server
* @return IP address of the indexed NTP server or NULL if the NTP
* server has not been configured by name (or at all)
*/
const char *
sntp_getservername(u8_t idx)
{
if (idx < SNTP_MAX_SERVERS) {
return sntp_servers[idx].name;
}
return NULL;
}
#endif /* SNTP_SERVER_DNS */
#endif /* LWIP_UDP */

View File

@ -0,0 +1,435 @@
/**
*
* @file tftp_server.c
*
* @author Logan Gunthorpe <logang@deltatee.com>
* Dirk Ziegelmeier <dziegel@gmx.de>
*
* @brief Trivial File Transfer Protocol (RFC 1350)
*
* Copyright (c) Deltatee Enterprises Ltd. 2013
* All rights reserved.
*
*/
/*
* Redistribution and use in source and binary forms, with or without
* modification,are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
* EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
* TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
* Author: Logan Gunthorpe <logang@deltatee.com>
* Dirk Ziegelmeier <dziegel@gmx.de>
*
*/
/**
* @defgroup tftp TFTP server
* @ingroup apps
*
* This is simple TFTP server for the lwIP raw API.
*/
#include "lwip/apps/tftp_server.h"
#if LWIP_UDP
#include "lwip/udp.h"
#include "lwip/timeouts.h"
#include "lwip/debug.h"
#define TFTP_MAX_PAYLOAD_SIZE 512
#define TFTP_HEADER_LENGTH 4
#define TFTP_RRQ 1
#define TFTP_WRQ 2
#define TFTP_DATA 3
#define TFTP_ACK 4
#define TFTP_ERROR 5
enum tftp_error {
TFTP_ERROR_FILE_NOT_FOUND = 1,
TFTP_ERROR_ACCESS_VIOLATION = 2,
TFTP_ERROR_DISK_FULL = 3,
TFTP_ERROR_ILLEGAL_OPERATION = 4,
TFTP_ERROR_UNKNOWN_TRFR_ID = 5,
TFTP_ERROR_FILE_EXISTS = 6,
TFTP_ERROR_NO_SUCH_USER = 7
};
#include <string.h>
struct tftp_state {
const struct tftp_context *ctx;
void *handle;
struct pbuf *last_data;
struct udp_pcb *upcb;
ip_addr_t addr;
u16_t port;
int timer;
int last_pkt;
u16_t blknum;
u8_t retries;
u8_t mode_write;
};
static struct tftp_state tftp_state;
static void tftp_tmr(void *arg);
static void
close_handle(void)
{
tftp_state.port = 0;
ip_addr_set_any(0, &tftp_state.addr);
if (tftp_state.last_data != NULL) {
pbuf_free(tftp_state.last_data);
tftp_state.last_data = NULL;
}
sys_untimeout(tftp_tmr, NULL);
if (tftp_state.handle) {
tftp_state.ctx->close(tftp_state.handle);
tftp_state.handle = NULL;
LWIP_DEBUGF(TFTP_DEBUG | LWIP_DBG_STATE, ("tftp: closing\n"));
}
}
static void
send_error(const ip_addr_t *addr, u16_t port, enum tftp_error code, const char *str)
{
int str_length = strlen(str);
struct pbuf *p;
u16_t *payload;
p = pbuf_alloc(PBUF_TRANSPORT, (u16_t)(TFTP_HEADER_LENGTH + str_length + 1), PBUF_RAM);
if (p == NULL) {
return;
}
payload = (u16_t *) p->payload;
payload[0] = PP_HTONS(TFTP_ERROR);
payload[1] = lwip_htons(code);
MEMCPY(&payload[2], str, str_length + 1);
udp_sendto(tftp_state.upcb, p, addr, port);
pbuf_free(p);
}
static void
send_ack(u16_t blknum)
{
struct pbuf *p;
u16_t *payload;
p = pbuf_alloc(PBUF_TRANSPORT, TFTP_HEADER_LENGTH, PBUF_RAM);
if (p == NULL) {
return;
}
payload = (u16_t *) p->payload;
payload[0] = PP_HTONS(TFTP_ACK);
payload[1] = lwip_htons(blknum);
udp_sendto(tftp_state.upcb, p, &tftp_state.addr, tftp_state.port);
pbuf_free(p);
}
static void
resend_data(void)
{
struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, tftp_state.last_data->len, PBUF_RAM);
if (p == NULL) {
return;
}
if (pbuf_copy(p, tftp_state.last_data) != ERR_OK) {
pbuf_free(p);
return;
}
udp_sendto(tftp_state.upcb, p, &tftp_state.addr, tftp_state.port);
pbuf_free(p);
}
static void
send_data(void)
{
u16_t *payload;
int ret;
if (tftp_state.last_data != NULL) {
pbuf_free(tftp_state.last_data);
}
tftp_state.last_data = pbuf_alloc(PBUF_TRANSPORT, TFTP_HEADER_LENGTH + TFTP_MAX_PAYLOAD_SIZE, PBUF_RAM);
if (tftp_state.last_data == NULL) {
return;
}
payload = (u16_t *) tftp_state.last_data->payload;
payload[0] = PP_HTONS(TFTP_DATA);
payload[1] = lwip_htons(tftp_state.blknum);
ret = tftp_state.ctx->read(tftp_state.handle, &payload[2], TFTP_MAX_PAYLOAD_SIZE);
if (ret < 0) {
send_error(&tftp_state.addr, tftp_state.port, TFTP_ERROR_ACCESS_VIOLATION, "Error occured while reading the file.");
close_handle();
return;
}
pbuf_realloc(tftp_state.last_data, (u16_t)(TFTP_HEADER_LENGTH + ret));
resend_data();
}
static void
recv(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
{
u16_t *sbuf = (u16_t *) p->payload;
int opcode;
LWIP_UNUSED_ARG(arg);
LWIP_UNUSED_ARG(upcb);
if (((tftp_state.port != 0) && (port != tftp_state.port)) ||
(!ip_addr_isany_val(tftp_state.addr) && !ip_addr_cmp(&tftp_state.addr, addr))) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Only one connection at a time is supported");
pbuf_free(p);
return;
}
opcode = sbuf[0];
tftp_state.last_pkt = tftp_state.timer;
tftp_state.retries = 0;
switch (opcode) {
case PP_HTONS(TFTP_RRQ): /* fall through */
case PP_HTONS(TFTP_WRQ): {
const char tftp_null = 0;
char filename[TFTP_MAX_FILENAME_LEN + 1];
char mode[TFTP_MAX_MODE_LEN + 1];
u16_t filename_end_offset;
u16_t mode_end_offset;
if (tftp_state.handle != NULL) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Only one connection at a time is supported");
break;
}
sys_timeout(TFTP_TIMER_MSECS, tftp_tmr, NULL);
/* find \0 in pbuf -> end of filename string */
filename_end_offset = pbuf_memfind(p, &tftp_null, sizeof(tftp_null), 2);
if ((u16_t)(filename_end_offset - 1) > sizeof(filename)) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Filename too long/not NULL terminated");
break;
}
pbuf_copy_partial(p, filename, filename_end_offset - 1, 2);
/* find \0 in pbuf -> end of mode string */
mode_end_offset = pbuf_memfind(p, &tftp_null, sizeof(tftp_null), filename_end_offset + 1);
if ((u16_t)(mode_end_offset - filename_end_offset) > sizeof(mode)) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Mode too long/not NULL terminated");
break;
}
pbuf_copy_partial(p, mode, mode_end_offset - filename_end_offset, filename_end_offset + 1);
tftp_state.handle = tftp_state.ctx->open(filename, mode, opcode == PP_HTONS(TFTP_WRQ));
tftp_state.blknum = 1;
if (!tftp_state.handle) {
send_error(addr, port, TFTP_ERROR_FILE_NOT_FOUND, "Unable to open requested file.");
break;
}
LWIP_DEBUGF(TFTP_DEBUG | LWIP_DBG_STATE, ("tftp: %s request from ", (opcode == PP_HTONS(TFTP_WRQ)) ? "write" : "read"));
ip_addr_debug_print(TFTP_DEBUG | LWIP_DBG_STATE, addr);
LWIP_DEBUGF(TFTP_DEBUG | LWIP_DBG_STATE, (" for '%s' mode '%s'\n", filename, mode));
ip_addr_copy(tftp_state.addr, *addr);
tftp_state.port = port;
if (opcode == PP_HTONS(TFTP_WRQ)) {
tftp_state.mode_write = 1;
send_ack(0);
} else {
tftp_state.mode_write = 0;
send_data();
}
break;
}
case PP_HTONS(TFTP_DATA): {
int ret;
u16_t blknum;
if (tftp_state.handle == NULL) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "No connection");
break;
}
if (tftp_state.mode_write != 1) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Not a write connection");
break;
}
blknum = lwip_ntohs(sbuf[1]);
if (blknum == tftp_state.blknum) {
pbuf_remove_header(p, TFTP_HEADER_LENGTH);
ret = tftp_state.ctx->write(tftp_state.handle, p);
if (ret < 0) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "error writing file");
close_handle();
} else {
send_ack(blknum);
}
if (p->tot_len < TFTP_MAX_PAYLOAD_SIZE) {
close_handle();
} else {
tftp_state.blknum++;
}
} else if ((u16_t)(blknum + 1) == tftp_state.blknum) {
/* retransmit of previous block, ack again (casting to u16_t to care for overflow) */
send_ack(blknum);
} else {
send_error(addr, port, TFTP_ERROR_UNKNOWN_TRFR_ID, "Wrong block number");
}
break;
}
case PP_HTONS(TFTP_ACK): {
u16_t blknum;
int lastpkt;
if (tftp_state.handle == NULL) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "No connection");
break;
}
if (tftp_state.mode_write != 0) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Not a read connection");
break;
}
blknum = lwip_ntohs(sbuf[1]);
if (blknum != tftp_state.blknum) {
send_error(addr, port, TFTP_ERROR_UNKNOWN_TRFR_ID, "Wrong block number");
break;
}
lastpkt = 0;
if (tftp_state.last_data != NULL) {
lastpkt = tftp_state.last_data->tot_len != (TFTP_MAX_PAYLOAD_SIZE + TFTP_HEADER_LENGTH);
}
if (!lastpkt) {
tftp_state.blknum++;
send_data();
} else {
close_handle();
}
break;
}
default:
send_error(addr, port, TFTP_ERROR_ILLEGAL_OPERATION, "Unknown operation");
break;
}
pbuf_free(p);
}
static void
tftp_tmr(void *arg)
{
LWIP_UNUSED_ARG(arg);
tftp_state.timer++;
if (tftp_state.handle == NULL) {
return;
}
sys_timeout(TFTP_TIMER_MSECS, tftp_tmr, NULL);
if ((tftp_state.timer - tftp_state.last_pkt) > (TFTP_TIMEOUT_MSECS / TFTP_TIMER_MSECS)) {
if ((tftp_state.last_data != NULL) && (tftp_state.retries < TFTP_MAX_RETRIES)) {
LWIP_DEBUGF(TFTP_DEBUG | LWIP_DBG_STATE, ("tftp: timeout, retrying\n"));
resend_data();
tftp_state.retries++;
} else {
LWIP_DEBUGF(TFTP_DEBUG | LWIP_DBG_STATE, ("tftp: timeout\n"));
close_handle();
}
}
}
/** @ingroup tftp
* Initialize TFTP server.
* @param ctx TFTP callback struct
*/
err_t
tftp_init(const struct tftp_context *ctx)
{
err_t ret;
/* LWIP_ASSERT_CORE_LOCKED(); is checked by udp_new() */
struct udp_pcb *pcb = udp_new_ip_type(IPADDR_TYPE_ANY);
if (pcb == NULL) {
return ERR_MEM;
}
ret = udp_bind(pcb, IP_ANY_TYPE, TFTP_PORT);
if (ret != ERR_OK) {
udp_remove(pcb);
return ret;
}
tftp_state.handle = NULL;
tftp_state.port = 0;
tftp_state.ctx = ctx;
tftp_state.timer = 0;
tftp_state.last_data = NULL;
tftp_state.upcb = pcb;
udp_recv(pcb, recv, NULL);
return ERR_OK;
}
/** @ingroup tftp
* Deinitialize ("turn off") TFTP server.
*/
void tftp_cleanup(void)
{
LWIP_ASSERT("Cleanup called on non-initialized TFTP", tftp_state.upcb != NULL);
udp_remove(tftp_state.upcb);
close_handle();
memset(&tftp_state, 0, sizeof(tftp_state));
}
#endif /* LWIP_UDP */

View File

@ -0,0 +1,5 @@
SRC_FILES += sys_arch.c \
tcp_echo_socket_demo.c \
udp_echo.c
include $(KERNEL_ROOT)/compiler.mk

View File

@ -0,0 +1,96 @@
/*
* Copyright (c) 2001-2003 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#ifndef __CC_H__
#define __CC_H__
#include "stdio.h"
#include "main.h"
//typedef unsigned char u8_t;
//typedef signed char s8_t;
//typedef unsigned short u16_t;
//typedef signed short s16_t;
//typedef unsigned long u32_t;
//typedef signed long s32_t;
//typedef u32_t mem_ptr_t;
//typedef int sys_prot_t;
#define U16_F "hu"
#define S16_F "d"
#define X16_F "hx"
#define U32_F "u"
#define S32_F "d"
#define X32_F "x"
#define SZT_F "uz"
/* define compiler specific symbols */
#if defined (__ICCARM__)
#define PACK_STRUCT_BEGIN
#define PACK_STRUCT_STRUCT
#define PACK_STRUCT_END
#define PACK_STRUCT_FIELD(x) x
#define PACK_STRUCT_USE_INCLUDES
#elif defined (__CC_ARM)
#define PACK_STRUCT_BEGIN __packed
#define PACK_STRUCT_STRUCT
#define PACK_STRUCT_END
#define PACK_STRUCT_FIELD(x) x
#elif defined (__GNUC__)
#define PACK_STRUCT_BEGIN
#define PACK_STRUCT_STRUCT __attribute__ ((__packed__))
#define PACK_STRUCT_END
#define PACK_STRUCT_FIELD(x) x
#elif defined (__TASKING__)
#define PACK_STRUCT_BEGIN
#define PACK_STRUCT_STRUCT
#define PACK_STRUCT_END
#define PACK_STRUCT_FIELD(x) x
#endif
#define LWIP_PLATFORM_ASSERT(x) do {printf(x);}while(0)
#endif /* __CC_H__ */

View File

@ -0,0 +1,86 @@
/**
******************************************************************************
* File Name : ethernetif.h
* Description : This file provides initialization code for LWIP
* middleWare.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2018 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
#ifndef __ETHERNETIF_H__
#define __ETHERNETIF_H__
#include "lwip/err.h"
#include "lwip/netif.h"
/* Within 'USER CODE' section, code will be kept by default at each generation */
/* USER CODE BEGIN 0 */
#define NETIF_MTU ( 1500 )
#define NETIF_IN_TASK_STACK_SIZE ( 1024 )
#define NETIF_IN_TASK_PRIORITY ( 3 )
#define NETIF_OUT_TASK_STACK_SIZE ( 1024 )
#define NETIF_OUT_TASK_PRIORITY ( 3 )
/* USER CODE END 0 */
/* Exported functions ------------------------------------------------------- */
err_t ethernetif_init(struct netif *netif);
void ethernetif_input( void *argument );
void ethernetif_output( void *argument );
void ethernetif_update_config(struct netif *netif);
void ethernetif_notify_conn_changed(struct netif *netif);
u32_t sys_jiffies(void);
u32_t sys_now(void);
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
#endif
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

View File

@ -0,0 +1,499 @@
/**
******************************************************************************
* @file lwipopts.h
* @author MCD Application Team
* @version V1.1.0
* @date 31-July-2013
* @brief lwIP Options Configuration.
* This file is based on Utilities\lwip_v1.4.1\src\include\lwip\opt.h
* and contains the lwIP configuration for the STM32F4x7 demonstration.
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
#ifndef __LWIPOPTS_H__
#define __LWIPOPTS_H__
/**
* SYS_LIGHTWEIGHT_PROT==1: if you want inter-task protection for certain
* critical regions during buffer allocation, deallocation and memory
* allocation and deallocation.
*/
#define SYS_LIGHTWEIGHT_PROT 1
/**
* NO_SYS==1: Provides VERY minimal functionality. Otherwise,
* use lwIP facilities.
*/
#define NO_SYS 0
/**
* NO_SYS_NO_TIMERS==1: Drop support for sys_timeout when NO_SYS==1
* Mainly for compatibility to old versions.
*/
#define NO_SYS_NO_TIMERS 0
/* ---------- Memory options ---------- */
/* MEM_ALIGNMENT: should be set to the alignment of the CPU for which
lwIP is compiled. 4 byte alignment -> define MEM_ALIGNMENT to 4, 2
byte alignment -> define MEM_ALIGNMENT to 2. */
#define MEM_ALIGNMENT 4
/* MEM_SIZE: the size of the heap memory. If the application will send
a lot of data that needs to be copied, this should be set high. */
#define MEM_SIZE (25*1024)
/* MEMP_NUM_PBUF: the number of memp struct pbufs. If the application
sends a lot of data out of ROM (or other static memory), this
should be set high. */
#define MEMP_NUM_PBUF 15
/* MEMP_NUM_UDP_PCB: the number of UDP protocol control blocks. One
per active UDP "connection". */
#define MEMP_NUM_UDP_PCB 4
/* MEMP_NUM_TCP_PCB: the number of simulatenously active TCP
connections. */
#define MEMP_NUM_TCP_PCB 4
/* MEMP_NUM_TCP_PCB_LISTEN: the number of listening TCP
connections. */
#define MEMP_NUM_TCP_PCB_LISTEN 2
/* MEMP_NUM_TCP_SEG: the number of simultaneously queued TCP
segments. */
#define MEMP_NUM_TCP_SEG 120
/* MEMP_NUM_SYS_TIMEOUT: the number of simulateously active
timeouts. */
#define MEMP_NUM_SYS_TIMEOUT 6
/* ---------- Pbuf options ---------- */
/* PBUF_POOL_SIZE: the number of buffers in the pbuf pool. */
#define PBUF_POOL_SIZE 20
/* PBUF_POOL_BUFSIZE: the size of each pbuf in the pbuf pool. */
#define PBUF_POOL_BUFSIZE LWIP_MEM_ALIGN_SIZE(TCP_MSS+40+PBUF_LINK_ENCAPSULATION_HLEN+PBUF_LINK_HLEN)
/* ---------- TCP options ---------- */
#define LWIP_TCP 1
#define TCP_TTL 255
/* Controls if TCP should queue segments that arrive out of
order. Define to 0 if your device is low on memory. */
#define TCP_QUEUE_OOSEQ 0
/* TCP Maximum segment size. */
#define TCP_MSS (1500 - 40) /* TCP_MSS = (Ethernet MTU - IP header size - TCP header size) */
/* TCP sender buffer space (bytes). */
#define TCP_SND_BUF (11*TCP_MSS)
/* TCP_SND_QUEUELEN: TCP sender buffer space (pbufs). This must be at least
as much as (2 * TCP_SND_BUF/TCP_MSS) for things to work. */
#define TCP_SND_QUEUELEN (8* TCP_SND_BUF/TCP_MSS)
/* TCP receive window. */
#define TCP_WND 16000//(12*TCP_MSS)
/* ---------- ICMP options ---------- */
#define LWIP_ICMP 1
/* ---------- DHCP options ---------- */
/* Define LWIP_DHCP to 1 if you want DHCP configuration of
interfaces. DHCP is not implemented in lwIP 0.5.1, however, so
turning this on does currently not work. */
#define LWIP_DHCP 0
/* ---------- UDP options ---------- */
#define LWIP_UDP 1
#define UDP_TTL 255
/* ---------- Statistics options ---------- */
#define LWIP_STATS 0
#define LWIP_PROVIDE_ERRNO 1
/* ---------- link callback options ---------- */
/* LWIP_NETIF_LINK_CALLBACK==1: Support a callback function from an interface
* whenever the link changes (i.e., link down)
*/
#define LWIP_NETIF_LINK_CALLBACK 0
/*
--------------------------------------
---------- Checksum options ----------
--------------------------------------
*/
/*
The STM32F4x7 allows computing and verifying the IP, UDP, TCP and ICMP checksums by hardware:
- To use this feature let the following define uncommented.
- To disable it and process by CPU comment the the checksum.
*/
#define CHECKSUM_BY_HARDWARE
#ifdef CHECKSUM_BY_HARDWARE
/* CHECKSUM_GEN_IP==0: Generate checksums by hardware for outgoing IP packets.*/
#define CHECKSUM_GEN_IP 0
/* CHECKSUM_GEN_UDP==0: Generate checksums by hardware for outgoing UDP packets.*/
#define CHECKSUM_GEN_UDP 0
/* CHECKSUM_GEN_TCP==0: Generate checksums by hardware for outgoing TCP packets.*/
#define CHECKSUM_GEN_TCP 0
/* CHECKSUM_CHECK_IP==0: Check checksums by hardware for incoming IP packets.*/
#define CHECKSUM_CHECK_IP 0
/* CHECKSUM_CHECK_UDP==0: Check checksums by hardware for incoming UDP packets.*/
#define CHECKSUM_CHECK_UDP 0
/* CHECKSUM_CHECK_TCP==0: Check checksums by hardware for incoming TCP packets.*/
#define CHECKSUM_CHECK_TCP 0
/* CHECKSUM_CHECK_ICMP==0: Check checksums by hardware for incoming ICMP packets.*/
#define CHECKSUM_GEN_ICMP 0
#else
/* CHECKSUM_GEN_IP==1: Generate checksums in software for outgoing IP packets.*/
#define CHECKSUM_GEN_IP 1
/* CHECKSUM_GEN_UDP==1: Generate checksums in software for outgoing UDP packets.*/
#define CHECKSUM_GEN_UDP 1
/* CHECKSUM_GEN_TCP==1: Generate checksums in software for outgoing TCP packets.*/
#define CHECKSUM_GEN_TCP 1
/* CHECKSUM_CHECK_IP==1: Check checksums in software for incoming IP packets.*/
#define CHECKSUM_CHECK_IP 1
/* CHECKSUM_CHECK_UDP==1: Check checksums in software for incoming UDP packets.*/
#define CHECKSUM_CHECK_UDP 1
/* CHECKSUM_CHECK_TCP==1: Check checksums in software for incoming TCP packets.*/
#define CHECKSUM_CHECK_TCP 1
/* CHECKSUM_CHECK_ICMP==1: Check checksums by hardware for incoming ICMP packets.*/
#define CHECKSUM_GEN_ICMP 1
#endif
/*
----------------------------------------------
---------- Sequential layer options ----------
----------------------------------------------
*/
/**
* LWIP_NETCONN==1: Enable Netconn API (require to use api_lib.c)
*/
#define LWIP_NETCONN 1
/*
------------------------------------
---------- Socket options ----------
------------------------------------
*/
/**
* LWIP_SOCKET==1: Enable Socket API (require to use sockets.c)
*/
#define LWIP_SOCKET 1
/*
---------------------------------
---------- OS options ----------
---------------------------------
*/
#define DEFAULT_UDP_RECVMBOX_SIZE 10
#define DEFAULT_TCP_RECVMBOX_SIZE 10
#define DEFAULT_ACCEPTMBOX_SIZE 10
#define DEFAULT_THREAD_STACKSIZE 1024
#define TCPIP_THREAD_NAME "lwip"
#define TCPIP_THREAD_STACKSIZE 2048
#define TCPIP_MBOX_SIZE 8
#define TCPIP_THREAD_PRIO 3
//#define IPERF_SERVER_THREAD_NAME "iperf_server"
//#define IPERF_SERVER_THREAD_STACKSIZE 1024
//#define IPERF_SERVER_THREAD_PRIO 0
//#define BLOCK_TIME 250
//#define BLOCK_TIME_WAITING_FOR_INPUT ( ( portTickType ) 100 )
/*
----------------------------------------
---------- Lwip Debug options ----------
----------------------------------------
*/
//#define LWIP_DEBUG 1
#endif /* __LWIPOPTS_H__ */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/*
* Copyright (c) 2001-2003 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
//#ifndef __LWIPOPTS_H__
//#define __LWIPOPTS_H__
//#define TCPIP_MBOX_SIZE 6
//#define TCPIP_THREAD_NAME "tcp/ip"
//#define TCPIP_THREAD_STACKSIZE 1500
//#define TCPIP_THREAD_PRIO 3
//#define DEFAULT_THREAD_STACKSIZE 1500
//#define DEFAULT_THREAD_PRIO 3
//#define ETH_PAD_SIZE 2
//#define LWIP_TCPIP_CORE_LOCKING 0 ///TODO Initialize mutex
////#define LWIP_DEBUG
////#define DBG_TYPES_ON 0xff
//#define ETHARP_DEBUG LWIP_DBG_OFF
//#define NETIF_DEBUG LWIP_DBG_ON
//#define PBUF_DEBUG LWIP_DBG_OFF
//#define API_LIB_DEBUG LWIP_DBG_OFF
//#define API_MSG_DEBUG LWIP_DBG_OFF
//#define SOCKETS_DEBUG LWIP_DBG_OFF
//#define ICMP_DEBUG LWIP_DBG_OFF
//#define IGMP_DEBUG LWIP_DBG_OFF
//#define INET_DEBUG LWIP_DBG_OFF
//#define IP_DEBUG LWIP_DBG_OFF
//#define IP_REASS_DEBUG LWIP_DBG_OFF
//#define RAW_DEBUG LWIP_DBG_OFF
//#define MEM_DEBUG LWIP_DBG_OFF
//#define MEMP_DEBUG LWIP_DBG_OFF
//#define SYS_DEBUG LWIP_DBG_OFF
//#define TCP_DEBUG LWIP_DBG_OFF
//#define TCP_INPUT_DEBUG LWIP_DBG_OFF
//#define TCP_FR_DEBUG LWIP_DBG_OFF
//#define TCP_RTO_DEBUG LWIP_DBG_OFF
//#define TCP_CWND_DEBUG LWIP_DBG_OFF
//#define TCP_WND_DEBUG LWIP_DBG_OFF
//#define TCP_OUTPUT_DEBUG LWIP_DBG_OFF
//#define TCP_RST_DEBUG LWIP_DBG_OFF
//#define TCP_QLEN_DEBUG LWIP_DBG_OFF
//#define UDP_DEBUG LWIP_DBG_OFF
//#define TCPIP_DEBUG LWIP_DBG_OFF
//#define PPP_DEBUG LWIP_DBG_OFF
//#define SLIP_DEBUG LWIP_DBG_OFF
//#define DHCP_DEBUG LWIP_DBG_OFF
//#define AUTOIP_DEBUG LWIP_DBG_OFF
//#define SNMP_MSG_DEBUG LWIP_DBG_OFF
//#define SNMP_MIB_DEBUG LWIP_DBG_OFF
//#define DNS_DEBUG LWIP_DBG_OFF
//#define LWIP_NOASSERT 0
///**
// * SYS_LIGHTWEIGHT_PROT==1: if you want inter-task protection for certain
// * critical regions during buffer allocation, deallocation and memory
// * allocation and deallocation.
// */
//#define SYS_LIGHTWEIGHT_PROT 1
///*
// ------------------------------------
// ---------- Memory options ----------
// ------------------------------------
//*/
///**
// * MEM_ALIGNMENT: should be set to the alignment of the CPU
// * 4 byte alignment -> #define MEM_ALIGNMENT 4
// * 2 byte alignment -> #define MEM_ALIGNMENT 2
// */
//#define MEM_ALIGNMENT 4
///** STF (9*1024)
// * MEM_SIZE: the size of the heap memory. If the application will send
// * a lot of data that needs to be copied, this should be set high.
// */
//#define MEM_SIZE (8*1024)
///*
// ------------------------------------------------
// ---------- Internal Memory Pool Sizes ----------
// ------------------------------------------------
//*/
///** STF 40
// * MEMP_NUM_PBUF: the number of memp struct pbufs (used for PBUF_ROM and PBUF_REF).
// * If the application sends a lot of data out of ROM (or other static memory),
// * this should be set high.
// */
//#define MEMP_NUM_PBUF 20
///** STF 8
// * MEMP_NUM_TCP_PCB: the number of simulatenously active TCP connections.
// * (requires the LWIP_TCP option)
// */
//#define MEMP_NUM_TCP_PCB 10
///**
// * MEMP_NUM_TCP_SEG: the number of simultaneously queued TCP segments.
// * (requires the LWIP_TCP option)
// */
//#define MEMP_NUM_TCP_SEG 8
///** STF 3
// * MEMP_NUM_SYS_TIMEOUT: the number of simulateously active timeouts.
// * (requires NO_SYS==0)
// */
//#define MEMP_NUM_SYS_TIMEOUT 5
///**
// * MEMP_NUM_NETBUF: the number of struct netbufs.
// * (only needed if you use the sequential API, like api_lib.c)
// */
//#define MEMP_NUM_NETBUF 4
///** STF 8
// * PBUF_POOL_SIZE: the number of buffers in the pbuf pool.
// */
//#define PBUF_POOL_SIZE 4
///*
// ----------------------------------
// ---------- Pbuf options ----------
// ----------------------------------
//*/
///**
// * PBUF_POOL_BUFSIZE: the size of each pbuf in the pbuf pool. The default is
// * designed to accomodate single full size TCP frame in one pbuf, including
// * TCP_MSS, IP header, and link header.
// */
//#define PBUF_POOL_BUFSIZE 1500
///*
// ---------------------------------
// ---------- TCP options ----------
// ---------------------------------
//*/
///**
// * LWIP_TCP==1: Turn on TCP.
// */
//#define LWIP_TCP 1
///* TCP Maximum segment size. */
//#define TCP_MSS 1500
///* TCP sender buffer space (bytes). */
//#define TCP_SND_BUF 2*TCP_MSS
///**
// * TCP_WND: The size of a TCP window.
// */
//#define TCP_WND 2*TCP_MSS //1500
///**
// * TCP_SYNMAXRTX: Maximum number of retransmissions of SYN segments.
// */
//#define TCP_SYNMAXRTX 4
///*
// ---------------------------------
// ---------- RAW options ----------
// ---------------------------------
//*/
///**
// * LWIP_RAW==1: Enable application layer to hook into the IP layer itself.
// */
//#define LWIP_RAW 0
///*
// ----------------------------------------------
// ---------- Sequential layer options ----------
// ----------------------------------------------
//*/
///**
// * LWIP_NETCONN==1: Enable Netconn API (require to use api_lib.c)
// */
//#define LWIP_NETCONN 1
///*
// ------------------------------------
// ---------- Socket options ----------
// ------------------------------------
//*/
///**
// * LWIP_SOCKET==1: Enable Socket API (require to use sockets.c)
// */
//#define LWIP_SOCKET 1
///*
// ----------------------------------------
// ---------- Statistics options ----------
// ----------------------------------------
//*/
///**
// * LWIP_STATS==1: Enable statistics collection in lwip_stats.
// */
//#define LWIP_STATS 0
///*
// ----------------------------------
// ---------- DHCP options ----------
// ----------------------------------
//*/
///**
// * LWIP_DHCP==1: Enable DHCP module.
// */
//#define LWIP_DHCP 0
///*
// ----------------------------------
// ---------- IGMP options ----------
// ----------------------------------
//*/
///**
// * LWIP_IGMP==1: Turn on IGMP module.
// */
//#define LWIP_IGMP 0
//#define LWIP_PROVIDE_ERRNO 1
////#define LWIP_SNMP 1
//#endif /* __LWIPOPTS_H__ */

View File

@ -0,0 +1,50 @@
/**
******************************************************************************
* @file GPIO/GPIO_EXTI/Inc/main.h
* @author MCD Application Team
* @brief Header for main.c module
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __MAIN_H
#define __MAIN_H
/* Includes ------------------------------------------------------------------*/
// void BSP_Init(void);
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
#endif /* __MAIN_H */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

View File

@ -0,0 +1,38 @@
/*
* Copyright (c) 2001-2003 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#ifndef __PERF_H__
#define __PERF_H__
#define PERF_START /* null definition */
#define PERF_STOP(x) /* null definition */
#endif /* __PERF_H__ */

View File

@ -0,0 +1,450 @@
/*
* Copyright (c) 2017 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt
*
*/
/*
* Copyright (c) 2020 AIIT XUOS Lab
* XiUOS is licensed under Mulan PSL v2.
* You can use this software according to the terms and conditions of the Mulan PSL v2.
* You may obtain a copy of Mulan PSL v2 at:
* http://license.coscl.org.cn/MulanPSL2
* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
* See the Mulan PSL v2 for more details.
*/
/**
* @file sys_arch.c
* @brief In order to adapt to XiUOS, some changes have been made to implement the LwIP interface.
* @version 1.0
* @author AIIT XUOS Lab
* @date 2021-05-29
*/
#include "debug.h"
#include <lwip/opt.h>
#include <lwip/arch.h>
#include "tcpip.h"
#include "lwip/init.h"
#include "lwip/netif.h"
#include "lwip/sio.h"
#include "ethernetif.h"
#if !NO_SYS
#include "sys_arch.h"
#endif
#include <lwip/stats.h>
#include <lwip/debug.h>
#include <lwip/sys.h>
#include "lwip/dhcp.h"
#include <string.h>
#include <xs_ktask.h>
#include <xs_ktick.h>
#include <xs_assign.h>
#include <xs_sem.h>
#include <xs_mutex.h>
#include <xs_ktask.h>
#include <xs_msg.h>
#include <ethernetif.h>
int errno;
x_ticks_t lwip_sys_now;
struct sys_timeouts {
struct sys_timeo *next;
};
struct timeoutlist
{
struct sys_timeouts timeouts;
int32 pid;
};
#define SYS_THREAD_MAX 4
static struct timeoutlist s_timeoutlist[SYS_THREAD_MAX];
static u16_t s_nextthread = 0;
u32_t
sys_jiffies(void)
{
lwip_sys_now = CurrentTicksGain();
return lwip_sys_now;
}
u32_t
sys_now(void)
{
lwip_sys_now = CurrentTicksGain();
return lwip_sys_now;
}
void
sys_init(void)
{
int i;
for(i = 0; i < SYS_THREAD_MAX; i++)
{
s_timeoutlist[i].pid = 0;
s_timeoutlist[i].timeouts.next = NULL;
}
s_nextthread = 0;
}
struct sys_timeouts *sys_arch_timeouts(void)
{
int i;
int32 pid;
struct timeoutlist *tl;
pid = (int32)GetKTaskDescriptor()->id.id;
for(i = 0; i < s_nextthread; i++)
{
tl = &(s_timeoutlist[i]);
if(tl->pid == pid)
{
return &(tl->timeouts);
}
}
return NULL;
}
sys_prot_t sys_arch_protect(void)
{
return CriticalAreaLock();
}
void sys_arch_unprotect(sys_prot_t pval)
{
CriticalAreaUnLock(pval);
}
#if !NO_SYS
err_t
sys_sem_new(sys_sem_t *sem, u8_t count)
{
*sem = KSemaphoreCreate((uint16)count);
#if SYS_STATS
++lwip_stats.sys.sem.used;
if (lwip_stats.sys.sem.max < lwip_stats.sys.sem.used) {
lwip_stats.sys.sem.max = lwip_stats.sys.sem.used;
}
#endif /* SYS_STATS */
if(*sem >= 0)
return ERR_OK;
else
{
#if SYS_STATS
++lwip_stats.sys.sem.err;
#endif /* SYS_STATS */
KPrintf("[sys_arch]:new sem fail!\n");
return ERR_MEM;
}
}
void
sys_sem_free(sys_sem_t *sem)
{
#if SYS_STATS
--lwip_stats.sys.sem.used;
#endif /* SYS_STATS */
KSemaphoreDelete(*sem);
*sem = SYS_SEM_NULL;
}
int sys_sem_valid(sys_sem_t *sem)
{
return (*sem >= SYS_SEM_NULL);
}
void
sys_sem_set_invalid(sys_sem_t *sem)
{
*sem = SYS_SEM_NULL;
}
u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t timeout)
{
x_ticks_t start_tick = 0 ;
int32 wait_time = 0;
if(*sem == SYS_SEM_NULL)
return SYS_ARCH_TIMEOUT;
start_tick = CurrentTicksGain();
if (0 == timeout)
wait_time = WAITING_FOREVER;
else
wait_time = timeout;
if(KSemaphoreObtain(*sem, wait_time) == EOK)
return ((CurrentTicksGain()-start_tick)*MS_PER_SYSTICK_F407);
else
return SYS_ARCH_TIMEOUT;
}
void sys_sem_signal(sys_sem_t *sem)
{
if(KSemaphoreAbandon( *sem ) != EOK)
KPrintf("[sys_arch]:sem signal fail!\n");
}
err_t sys_mutex_new(sys_mutex_t *mutex)
{
*mutex = KMutexCreate();
if(*mutex >= 0)
return ERR_OK;
else
{
KPrintf("[sys_arch]:new mutex fail!\n");
return ERR_MEM;
}
}
void sys_mutex_free(sys_mutex_t *mutex)
{
KMutexDelete(*mutex);
}
void sys_mutex_set_invalid(sys_mutex_t *mutex)
{
*mutex = SYS_MRTEX_NULL;
}
void sys_mutex_lock(sys_mutex_t *mutex)
{
KMutexObtain(*mutex,
WAITING_FOREVER);
}
void sys_mutex_unlock(sys_mutex_t *mutex)
{
KMutexAbandon( *mutex );
}
sys_thread_t sys_thread_new(const char *name, lwip_thread_fn function, void *arg, int stacksize, int prio)
{
sys_thread_t handle = -1;
handle = KTaskCreate(name,
function,
arg,
(uint32)stacksize,
(uint8)prio);
if (handle >= 0)
{
StartupKTask(handle);
return handle;
}
return -ERROR;
}
err_t sys_mbox_new(sys_mbox_t *mbox, int size)
{
*mbox = KCreateMsgQueue(sizeof(void *), size);
#if SYS_STATS
++lwip_stats.sys.mbox.used;
if (lwip_stats.sys.mbox.max < lwip_stats.sys.mbox.used) {
lwip_stats.sys.mbox.max = lwip_stats.sys.mbox.used;
}
#endif /* SYS_STATS */
if(*mbox < 0)
return ERR_MEM;
return ERR_OK;
}
void sys_mbox_free(sys_mbox_t *mbox)
{
KDeleteMsgQueue(*mbox);
}
int sys_mbox_valid(sys_mbox_t *mbox)
{
if (*mbox < SYS_MBOX_NULL)
return 0;
else
return 1;
}
void sys_mbox_set_invalid(sys_mbox_t *mbox)
{
*mbox = SYS_MBOX_NULL;
}
void sys_mbox_post(sys_mbox_t *q, void *msg)
{
while(KMsgQueueSendwait( *q, &msg, sizeof(void *), WAITING_FOREVER) != EOK);
}
err_t sys_mbox_trypost(sys_mbox_t *q, void *msg)
{
if(KMsgQueueSend(*q, &msg, sizeof(void *)) == EOK)
return ERR_OK;
else
return ERR_MEM;
}
err_t sys_mbox_trypost_fromisr(sys_mbox_t *q, void *msg)
{
return sys_mbox_trypost(q, msg);
}
u32_t sys_arch_mbox_fetch(sys_mbox_t *q, void **msg, u32_t timeout)
{
x_ticks_t start_tick = 0 ;
int32 wait_time = 0;
start_tick = CurrentTicksGain();
if (0 == timeout)
wait_time = WAITING_FOREVER;
else
wait_time = timeout;
if(KMsgQueueRecv(*q, &(*msg), sizeof(void *), wait_time) == EOK)
return ((CurrentTicksGain()-start_tick)*MS_PER_SYSTICK_F407);
else{
*msg = NULL;
return SYS_ARCH_TIMEOUT;
}
}
u32_t sys_arch_mbox_tryfetch(sys_mbox_t *q, void **msg)
{
if(KMsgQueueRecv(*q, &(*msg), sizeof(void *), 0) == EOK)
return ERR_OK;
else
return SYS_MBOX_EMPTY;
}
#if LWIP_NETCONN_SEM_PER_THREAD
#error LWIP_NETCONN_SEM_PER_THREAD==1 not supported
#endif /* LWIP_NETCONN_SEM_PER_THREAD */
#endif /* !NO_SYS */
/* Variables Initialization */
struct netif gnetif;
ip4_addr_t ipaddr;
ip4_addr_t netmask;
ip4_addr_t gw;
uint8_t IP_ADDRESS[4];
uint8_t NETMASK_ADDRESS[4];
uint8_t GATEWAY_ADDRESS[4];
void TcpIpInit(void)
{
tcpip_init(NULL, NULL);
/* IP addresses initialization */
/* USER CODE BEGIN 0 */
#if LWIP_DHCP
ip_addr_set_zero_ip4(&ipaddr);
ip_addr_set_zero_ip4(&netmask);
ip_addr_set_zero_ip4(&gw);
#else
#ifdef SET_AS_SERVER
IP4_ADDR(&ipaddr,IP_ADDR0_SERVER,IP_ADDR1_SERVER,IP_ADDR2_SERVER,IP_ADDR3_SERVER);
#else
IP4_ADDR(&ipaddr,IP_ADDR0_ClIENT,IP_ADDR1_ClIENT,IP_ADDR2_ClIENT,IP_ADDR3_ClIENT);
#endif
IP4_ADDR(&netmask,NETMASK_ADDR0,NETMASK_ADDR1,NETMASK_ADDR2,NETMASK_ADDR3);
IP4_ADDR(&gw,GW_ADDR0,GW_ADDR1,GW_ADDR2,GW_ADDR3);
#endif /* USE_DHCP */
/* USER CODE END 0 */
/* Initilialize the LwIP stack without RTOS */
/* add the network interface (IPv4/IPv6) without RTOS */
netif_add(&gnetif, &ipaddr, &netmask, &gw, NULL, &ethernetif_init, &tcpip_input);
/* Registers the default network interface */
netif_set_default(&gnetif);
if (netif_is_link_up(&gnetif))
{
/* When the netif is fully configured this function must be called */
KPrintf("TcpIpInit : netif_set_up\n");
netif_set_up(&gnetif);
}
else
{
/* When the netif link is down this function must be called */
KPrintf("TcpIpInit : netif_set_down\n");
netif_set_down(&gnetif);
}
#if LWIP_DHCP
int err;
/* Creates a new DHCP client for this interface on the first call.
Note: you must call dhcp_fine_tmr() and dhcp_coarse_tmr() at
the predefined regular intervals after starting the client.
You can peek in the netif->dhcp struct for the actual DHCP status.*/
err = dhcp_start(&gnetif);
if(err == ERR_OK)
KPrintf("lwip dhcp init success...\n\n");
else
KPrintf("lwip dhcp init fail...\n\n");
while(ip_addr_cmp(&(gnetif.ip_addr),&ipaddr))
{
vTaskDelay(1);
}
#endif
KPrintf("\n\nIP:%d.%d.%d.%d\n\n", \
((gnetif.ip_addr.addr)&0x000000ff), \
(((gnetif.ip_addr.addr)&0x0000ff00)>>8), \
(((gnetif.ip_addr.addr)&0x00ff0000)>>16), \
((gnetif.ip_addr.addr)&0xff000000)>>24);
}

View File

@ -0,0 +1,102 @@
/*
* Copyright (c) 2017 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt
*
*/
/*
* Copyright (c) 2020 AIIT XUOS Lab
* XiUOS is licensed under Mulan PSL v2.
* You can use this software according to the terms and conditions of the Mulan PSL v2.
* You may obtain a copy of Mulan PSL v2 at:
* http://license.coscl.org.cn/MulanPSL2
* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
* See the Mulan PSL v2 for more details.
*/
/**
* @file sys_arch.h
* @brief In order to adapt to XiUOS, some changes have been made to implement the LwIP interface.
* @version 1.0
* @author AIIT XUOS Lab
* @date 2021-05-29
*/
#include <lwip/opt.h>
#include <lwip/arch.h>
#include "tcpip.h"
#include <xs_base.h>
/* USER CODE BEGIN 0 */
#define SET_AS_SERVER 1 /* define this terminal is udp server or not*/
#define LOCAL_PORT_SERVER 5001
#define TARGET_PORT_CLIENT LOCAL_PORT_SERVER
/*Static IP ADDRESS: IP_ADDR0.IP_ADDR1.IP_ADDR2.IP_ADDR3 */
#define IP_ADDR0_SERVER 192
#define IP_ADDR1_SERVER 168
#define IP_ADDR2_SERVER 0
#define IP_ADDR3_SERVER 166
#define IP_ADDR0_ClIENT 192
#define IP_ADDR1_ClIENT 168
#define IP_ADDR2_ClIENT 0
#define IP_ADDR3_ClIENT 177
/*NETMASK*/
#define NETMASK_ADDR0 255
#define NETMASK_ADDR1 255
#define NETMASK_ADDR2 255
#define NETMASK_ADDR3 0
/*Gateway Address*/
#define GW_ADDR0 192
#define GW_ADDR1 168
#define GW_ADDR2 0
#define GW_ADDR3 1
/* USER CODE END 0 */
#define SYS_MBOX_NULL 0
#define SYS_SEM_NULL 0
#define SYS_MRTEX_NULL SYS_SEM_NULL
typedef int32 sys_sem_t;
typedef int32 sys_mutex_t;
typedef int32 sys_mbox_t;
typedef int32 sys_thread_t;
typedef x_base sys_prot_t;
#define MS_PER_SYSTICK_F407 1000/TICK_PER_SECOND
void TcpIpInit(void);

View File

@ -0,0 +1,68 @@
/*
* Copyright (c) 2020 AIIT XUOS Lab
* XiUOS is licensed under Mulan PSL v2.
* You can use this software according to the terms and conditions of the Mulan PSL v2.
* You may obtain a copy of Mulan PSL v2 at:
* http://license.coscl.org.cn/MulanPSL2
* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
* See the Mulan PSL v2 for more details.
*/
/**
* @file tcp_echo_socket_demo.c
* @brief One UDP demo based on LwIP
* @version 1.0
* @author AIIT XUOS Lab
* @date 2021-05-29
*/
#include <xiuos.h>
#include "udp_echo.h"
#include <connect_ethernet.h>
static void NetStackTaskCreate(void* param);
extern void TcpIpInit(void);
extern char* send_msg;
int UdpEchoSocketDemo(int argc, char *argv[])
{
if (argc == 2)
{
send_msg = argv[1];
}
ETH_BSP_Config();
int32 thr_id = KTaskCreate(
(const char* )"NetStackTaskCreate",
NetStackTaskCreate,
(void* )NULL,
(uint16_t )512,
15);
if(thr_id >= 0)
StartupKTask(thr_id);
else{
KPrintf("NetStackTaskCreate create failed !\n");
return -1;
}
}
#ifndef SEPARATE_COMPILE
SHELL_EXPORT_CMD(SHELL_CMD_PERMISSION(0)|SHELL_CMD_TYPE(SHELL_TYPE_CMD_MAIN),
UdpEchoSocketDemo, UdpEchoSocketDemo, tcp_echo_socket function );
#endif
static void NetStackTaskCreate(void* param)
{
TcpIpInit();
UdpEchoInit();
}

View File

@ -0,0 +1,187 @@
/*
* Copyright (c) 2001-2003 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
/*
* Copyright (c) 2020 AIIT XUOS Lab
* XiUOS is licensed under Mulan PSL v2.
* You can use this software according to the terms and conditions of the Mulan PSL v2.
* You may obtain a copy of Mulan PSL v2 at:
* http://license.coscl.org.cn/MulanPSL2
* THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
* EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
* MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
* See the Mulan PSL v2 for more details.
*/
/**
* @file tcpecho.c
* @brief Add UDP client function
* @version 1.0
* @author AIIT XUOS Lab
* @date 2021-05-29
*/
#include "udp_echo.h"
#include "lwip/opt.h"
#if LWIP_SOCKET
#include <lwip/sockets.h>
#include "lwip/sys.h"
#include "lwip/api.h"
#include <xs_ktask.h>
#define RECV_DATA (1024)
char* send_msg = "\n\nThis one is UDP pkg. Congratulations on you.\n\n";
static void UdpEchoThreadServer(void *arg)
{
KPrintf("UdpEchoThreadServer start.\n");
int sock = -1;
char *recv_data;
struct sockaddr_in udp_addr,seraddr;
int recv_data_len;
socklen_t addrlen;
while(1)
{
recv_data = (char *)malloc(RECV_DATA);
if (recv_data == NULL)
{
KPrintf("No memory\n");
goto __exit;
}
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock < 0)
{
KPrintf("Socket error\n");
goto __exit;
}
udp_addr.sin_family = AF_INET;
udp_addr.sin_addr.s_addr = INADDR_ANY;
udp_addr.sin_port = htons(LOCAL_PORT_SERVER);
memset(&(udp_addr.sin_zero), 0, sizeof(udp_addr.sin_zero));
if (bind(sock, (struct sockaddr *)&udp_addr, sizeof(struct sockaddr)) == -1)
{
KPrintf("Unable to bind\n");
goto __exit;
}
KPrintf("UDP bind sucess, start to receive.\n");
KPrintf("\n\nLocal Port:%d\n\n", LOCAL_PORT_SERVER);
while(1)
{
recv_data_len=recvfrom(sock,recv_data,
RECV_DATA,0,
(struct sockaddr*)&seraddr,
&addrlen);
KPrintf("Receive from : %s\n",inet_ntoa(seraddr.sin_addr));
KPrintf("Recevce data : %s\n\n",recv_data);
sendto(sock,recv_data,
recv_data_len,0,
(struct sockaddr*)&seraddr,
addrlen);
}
__exit:
if (sock >= 0) closesocket(sock);
if (recv_data) free(recv_data);
}
}
static void UdpEchoThreadClient(void *arg)
{
KPrintf("UdpEchoThreadClient start.\n");
MdelayKTask(5000);
int sock_udp_send_once = -1;
sock_udp_send_once = socket(AF_INET, SOCK_DGRAM, 0);
if (sock_udp_send_once < 0)
{
KPrintf("Socket error\n");
goto __exit;
}
struct sockaddr_in udp_sock;
udp_sock.sin_family = AF_INET;
udp_sock.sin_port = htons(TARGET_PORT_CLIENT);
udp_sock.sin_addr.s_addr = PP_HTONL(LWIP_MAKEU32(IP_ADDR0_SERVER,IP_ADDR1_SERVER,IP_ADDR2_SERVER,IP_ADDR3_SERVER));
memset(&(udp_sock.sin_zero), 0, sizeof(udp_sock.sin_zero));
if (connect(sock_udp_send_once, (struct sockaddr *)&udp_sock, sizeof(struct sockaddr)))
{
KPrintf("Unable to connect\n");
goto __exit;
}
KPrintf("UDP connect sucess, start to send.\n");
KPrintf("\n\nTarget Port:%d\n\n", udp_sock.sin_port);
sendto(sock_udp_send_once,send_msg,
strlen(send_msg),0,
(struct sockaddr*)&udp_sock,
sizeof(struct sockaddr));
KPrintf("Send UDP msg: %s ", send_msg);
while (1)
{
KPrintf("Lwip client is running.\n");
MdelayKTask(3000);
}
__exit:
if (sock_udp_send_once >= 0) closesocket(sock_udp_send_once);
return;
}
void
UdpEchoInit(void)
{
#ifdef SET_AS_SERVER
sys_thread_new("UdpEchoThreadServer", UdpEchoThreadServer, NULL, 2048, 4);
#else
sys_thread_new("UdpEchoThreadClient", UdpEchoThreadClient, NULL, 2048, 4);
#endif
}
#endif /* LWIP_NETCONN */

View File

@ -0,0 +1,40 @@
/*
* Copyright (c) 2001-2003 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#ifndef LWIP_TCPECHO_H
#define LWIP_TCPECHO_H
void UdpEchoInit(void);
#endif /* LWIP_TCPECHO_H */

View File

@ -0,0 +1,24 @@
SRC_FILES += altcp_alloc.c \
altcp_tcp.c \
altcp.c \
def.c \
dns.c \
inet_chksum.c \
init.c \
ip.c \
mem.c \
memp.c \
netif.c \
pbuf.c \
raw.c \
stats.c \
sys.c \
tcp_in.c \
tcp_out.c \
tcp.c \
timeouts.c \
udp.c
SRC_DIR += ipv4
include $(KERNEL_ROOT)/compiler.mk

View File

@ -0,0 +1,681 @@
/**
* @file
* @defgroup altcp Application layered TCP Functions
* @ingroup altcp_api
*
* This file contains the common functions for altcp to work.
* For more details see @ref altcp_api.
*/
/**
* @defgroup altcp_api Application layered TCP Introduction
* @ingroup callbackstyle_api
*
* Overview
* --------
* altcp (application layered TCP connection API; to be used from TCPIP thread)
* is an abstraction layer that prevents applications linking hard against the
* @ref tcp.h functions while providing the same functionality. It is used to
* e.g. add SSL/TLS (see LWIP_ALTCP_TLS) or proxy-connect support to an application
* written for the tcp callback API without that application knowing the
* protocol details.
*
* * This interface mimics the tcp callback API to the application while preventing
* direct linking (much like virtual functions).
* * This way, an application can make use of other application layer protocols
* on top of TCP without knowing the details (e.g. TLS, proxy connection).
* * This is achieved by simply including "lwip/altcp.h" instead of "lwip/tcp.h",
* replacing "struct tcp_pcb" with "struct altcp_pcb" and prefixing all functions
* with "altcp_" instead of "tcp_".
*
* With altcp support disabled (LWIP_ALTCP==0), applications written against the
* altcp API can still be compiled but are directly linked against the tcp.h
* callback API and then cannot use layered protocols. To minimize code changes
* in this case, the use of altcp_allocators is strongly suggested.
*
* Usage
* -----
* To make use of this API from an existing tcp raw API application:
* * Include "lwip/altcp.h" instead of "lwip/tcp.h"
* * Replace "struct tcp_pcb" with "struct altcp_pcb"
* * Prefix all called tcp API functions with "altcp_" instead of "tcp_" to link
* against the altcp functions
* * @ref altcp_new (and @ref altcp_new_ip_type/@ref altcp_new_ip6) take
* an @ref altcp_allocator_t as an argument, whereas the original tcp API
* functions take no arguments.
* * An @ref altcp_allocator_t allocator is an object that holds a pointer to an
* allocator object and a corresponding state (e.g. for TLS, the corresponding
* state may hold certificates or keys). This way, the application does not
* even need to know if it uses TLS or pure TCP, this is handled at runtime
* by passing a specific allocator.
* * An application can alternatively bind hard to the altcp_tls API by calling
* @ref altcp_tls_new or @ref altcp_tls_wrap.
* * The TLS layer is not directly implemented by lwIP, but a port to mbedTLS is
* provided.
* * Another altcp layer is proxy-connect to use TLS behind a HTTP proxy (see
* @ref altcp_proxyconnect.h)
*
* altcp_allocator_t
* -----------------
* An altcp allocator is created by the application by combining an allocator
* callback function and a corresponding state, e.g.:\code{.c}
* static const unsigned char cert[] = {0x2D, ... (see mbedTLS doc for how to create this)};
* struct altcp_tls_config * conf = altcp_tls_create_config_client(cert, sizeof(cert));
* altcp_allocator_t tls_allocator = {
* altcp_tls_alloc, conf
* };
* \endcode
*
*
* struct altcp_tls_config
* -----------------------
* The struct altcp_tls_config holds state that is needed to create new TLS client
* or server connections (e.g. certificates and private keys).
*
* It is not defined by lwIP itself but by the TLS port (e.g. altcp_tls to mbedTLS
* adaption). However, the parameters used to create it are defined in @ref
* altcp_tls.h (see @ref altcp_tls_create_config_server_privkey_cert for servers
* and @ref altcp_tls_create_config_client/@ref altcp_tls_create_config_client_2wayauth
* for clients).
*
* For mbedTLS, ensure that certificates can be parsed by 'mbedtls_x509_crt_parse()' and
* private keys can be parsed by 'mbedtls_pk_parse_key()'.
*/
/*
* Copyright (c) 2017 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt <goldsimon@gmx.de>
*
*/
#include "lwip/opt.h"
#if LWIP_ALTCP /* don't build if not configured for use in lwipopts.h */
#include "lwip/altcp.h"
#include "lwip/priv/altcp_priv.h"
#include "lwip/altcp_tcp.h"
#include "lwip/tcp.h"
#include "lwip/mem.h"
#include <string.h>
extern const struct altcp_functions altcp_tcp_functions;
/**
* For altcp layer implementations only: allocate a new struct altcp_pcb from the pool
* and zero the memory
*/
struct altcp_pcb *
altcp_alloc(void)
{
struct altcp_pcb *ret = (struct altcp_pcb *)memp_malloc(MEMP_ALTCP_PCB);
if (ret != NULL) {
memset(ret, 0, sizeof(struct altcp_pcb));
}
return ret;
}
/**
* For altcp layer implementations only: return a struct altcp_pcb to the pool
*/
void
altcp_free(struct altcp_pcb *conn)
{
if (conn) {
if (conn->fns && conn->fns->dealloc) {
conn->fns->dealloc(conn);
}
memp_free(MEMP_ALTCP_PCB, conn);
}
}
/**
* @ingroup altcp
* altcp_new_ip6: @ref altcp_new for IPv6
*/
struct altcp_pcb *
altcp_new_ip6(altcp_allocator_t *allocator)
{
return altcp_new_ip_type(allocator, IPADDR_TYPE_V6);
}
/**
* @ingroup altcp
* altcp_new: @ref altcp_new for IPv4
*/
struct altcp_pcb *
altcp_new(altcp_allocator_t *allocator)
{
return altcp_new_ip_type(allocator, IPADDR_TYPE_V4);
}
/**
* @ingroup altcp
* altcp_new_ip_type: called by applications to allocate a new pcb with the help of an
* allocator function.
*
* @param allocator allocator function and argument
* @param ip_type IP version of the pcb (@ref lwip_ip_addr_type)
* @return a new altcp_pcb or NULL on error
*/
struct altcp_pcb *
altcp_new_ip_type(altcp_allocator_t *allocator, u8_t ip_type)
{
struct altcp_pcb *conn;
if (allocator == NULL) {
/* no allocator given, create a simple TCP connection */
return altcp_tcp_new_ip_type(ip_type);
}
if (allocator->alloc == NULL) {
/* illegal allocator */
return NULL;
}
conn = allocator->alloc(allocator->arg, ip_type);
if (conn == NULL) {
/* allocation failed */
return NULL;
}
return conn;
}
/**
* @ingroup altcp
* @see tcp_arg()
*/
void
altcp_arg(struct altcp_pcb *conn, void *arg)
{
if (conn) {
conn->arg = arg;
}
}
/**
* @ingroup altcp
* @see tcp_accept()
*/
void
altcp_accept(struct altcp_pcb *conn, altcp_accept_fn accept)
{
if (conn != NULL) {
conn->accept = accept;
}
}
/**
* @ingroup altcp
* @see tcp_recv()
*/
void
altcp_recv(struct altcp_pcb *conn, altcp_recv_fn recv)
{
if (conn) {
conn->recv = recv;
}
}
/**
* @ingroup altcp
* @see tcp_sent()
*/
void
altcp_sent(struct altcp_pcb *conn, altcp_sent_fn sent)
{
if (conn) {
conn->sent = sent;
}
}
/**
* @ingroup altcp
* @see tcp_poll()
*/
void
altcp_poll(struct altcp_pcb *conn, altcp_poll_fn poll, u8_t interval)
{
if (conn) {
conn->poll = poll;
conn->pollinterval = interval;
if (conn->fns && conn->fns->set_poll) {
conn->fns->set_poll(conn, interval);
}
}
}
/**
* @ingroup altcp
* @see tcp_err()
*/
void
altcp_err(struct altcp_pcb *conn, altcp_err_fn err)
{
if (conn) {
conn->err = err;
}
}
/* Generic functions calling the "virtual" ones */
/**
* @ingroup altcp
* @see tcp_recved()
*/
void
altcp_recved(struct altcp_pcb *conn, u16_t len)
{
if (conn && conn->fns && conn->fns->recved) {
conn->fns->recved(conn, len);
}
}
/**
* @ingroup altcp
* @see tcp_bind()
*/
err_t
altcp_bind(struct altcp_pcb *conn, const ip_addr_t *ipaddr, u16_t port)
{
if (conn && conn->fns && conn->fns->bind) {
return conn->fns->bind(conn, ipaddr, port);
}
return ERR_VAL;
}
/**
* @ingroup altcp
* @see tcp_connect()
*/
err_t
altcp_connect(struct altcp_pcb *conn, const ip_addr_t *ipaddr, u16_t port, altcp_connected_fn connected)
{
if (conn && conn->fns && conn->fns->connect) {
return conn->fns->connect(conn, ipaddr, port, connected);
}
return ERR_VAL;
}
/**
* @ingroup altcp
* @see tcp_listen_with_backlog_and_err()
*/
struct altcp_pcb *
altcp_listen_with_backlog_and_err(struct altcp_pcb *conn, u8_t backlog, err_t *err)
{
if (conn && conn->fns && conn->fns->listen) {
return conn->fns->listen(conn, backlog, err);
}
return NULL;
}
/**
* @ingroup altcp
* @see tcp_abort()
*/
void
altcp_abort(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->abort) {
conn->fns->abort(conn);
}
}
/**
* @ingroup altcp
* @see tcp_close()
*/
err_t
altcp_close(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->close) {
return conn->fns->close(conn);
}
return ERR_VAL;
}
/**
* @ingroup altcp
* @see tcp_shutdown()
*/
err_t
altcp_shutdown(struct altcp_pcb *conn, int shut_rx, int shut_tx)
{
if (conn && conn->fns && conn->fns->shutdown) {
return conn->fns->shutdown(conn, shut_rx, shut_tx);
}
return ERR_VAL;
}
/**
* @ingroup altcp
* @see tcp_write()
*/
err_t
altcp_write(struct altcp_pcb *conn, const void *dataptr, u16_t len, u8_t apiflags)
{
if (conn && conn->fns && conn->fns->write) {
return conn->fns->write(conn, dataptr, len, apiflags);
}
return ERR_VAL;
}
/**
* @ingroup altcp
* @see tcp_output()
*/
err_t
altcp_output(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->output) {
return conn->fns->output(conn);
}
return ERR_VAL;
}
/**
* @ingroup altcp
* @see tcp_mss()
*/
u16_t
altcp_mss(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->mss) {
return conn->fns->mss(conn);
}
return 0;
}
/**
* @ingroup altcp
* @see tcp_sndbuf()
*/
u16_t
altcp_sndbuf(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->sndbuf) {
return conn->fns->sndbuf(conn);
}
return 0;
}
/**
* @ingroup altcp
* @see tcp_sndqueuelen()
*/
u16_t
altcp_sndqueuelen(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->sndqueuelen) {
return conn->fns->sndqueuelen(conn);
}
return 0;
}
void
altcp_nagle_disable(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->nagle_disable) {
conn->fns->nagle_disable(conn);
}
}
void
altcp_nagle_enable(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->nagle_enable) {
conn->fns->nagle_enable(conn);
}
}
int
altcp_nagle_disabled(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->nagle_disabled) {
return conn->fns->nagle_disabled(conn);
}
return 0;
}
/**
* @ingroup altcp
* @see tcp_setprio()
*/
void
altcp_setprio(struct altcp_pcb *conn, u8_t prio)
{
if (conn && conn->fns && conn->fns->setprio) {
conn->fns->setprio(conn, prio);
}
}
err_t
altcp_get_tcp_addrinfo(struct altcp_pcb *conn, int local, ip_addr_t *addr, u16_t *port)
{
if (conn && conn->fns && conn->fns->addrinfo) {
return conn->fns->addrinfo(conn, local, addr, port);
}
return ERR_VAL;
}
ip_addr_t *
altcp_get_ip(struct altcp_pcb *conn, int local)
{
if (conn && conn->fns && conn->fns->getip) {
return conn->fns->getip(conn, local);
}
return NULL;
}
u16_t
altcp_get_port(struct altcp_pcb *conn, int local)
{
if (conn && conn->fns && conn->fns->getport) {
return conn->fns->getport(conn, local);
}
return 0;
}
#ifdef LWIP_DEBUG
enum tcp_state
altcp_dbg_get_tcp_state(struct altcp_pcb *conn)
{
if (conn && conn->fns && conn->fns->dbg_get_tcp_state) {
return conn->fns->dbg_get_tcp_state(conn);
}
return CLOSED;
}
#endif
/* Default implementations for the "virtual" functions */
void
altcp_default_set_poll(struct altcp_pcb *conn, u8_t interval)
{
if (conn && conn->inner_conn) {
altcp_poll(conn->inner_conn, conn->poll, interval);
}
}
void
altcp_default_recved(struct altcp_pcb *conn, u16_t len)
{
if (conn && conn->inner_conn) {
altcp_recved(conn->inner_conn, len);
}
}
err_t
altcp_default_bind(struct altcp_pcb *conn, const ip_addr_t *ipaddr, u16_t port)
{
if (conn && conn->inner_conn) {
return altcp_bind(conn->inner_conn, ipaddr, port);
}
return ERR_VAL;
}
err_t
altcp_default_shutdown(struct altcp_pcb *conn, int shut_rx, int shut_tx)
{
if (conn) {
if (shut_rx && shut_tx && conn->fns && conn->fns->close) {
/* default shutdown for both sides is close */
return conn->fns->close(conn);
}
if (conn->inner_conn) {
return altcp_shutdown(conn->inner_conn, shut_rx, shut_tx);
}
}
return ERR_VAL;
}
err_t
altcp_default_write(struct altcp_pcb *conn, const void *dataptr, u16_t len, u8_t apiflags)
{
if (conn && conn->inner_conn) {
return altcp_write(conn->inner_conn, dataptr, len, apiflags);
}
return ERR_VAL;
}
err_t
altcp_default_output(struct altcp_pcb *conn)
{
if (conn && conn->inner_conn) {
return altcp_output(conn->inner_conn);
}
return ERR_VAL;
}
u16_t
altcp_default_mss(struct altcp_pcb *conn)
{
if (conn && conn->inner_conn) {
return altcp_mss(conn->inner_conn);
}
return 0;
}
u16_t
altcp_default_sndbuf(struct altcp_pcb *conn)
{
if (conn && conn->inner_conn) {
return altcp_sndbuf(conn->inner_conn);
}
return 0;
}
u16_t
altcp_default_sndqueuelen(struct altcp_pcb *conn)
{
if (conn && conn->inner_conn) {
return altcp_sndqueuelen(conn->inner_conn);
}
return 0;
}
void
altcp_default_nagle_disable(struct altcp_pcb *conn)
{
if (conn && conn->inner_conn) {
altcp_nagle_disable(conn->inner_conn);
}
}
void
altcp_default_nagle_enable(struct altcp_pcb *conn)
{
if (conn && conn->inner_conn) {
altcp_nagle_enable(conn->inner_conn);
}
}
int
altcp_default_nagle_disabled(struct altcp_pcb *conn)
{
if (conn && conn->inner_conn) {
return altcp_nagle_disabled(conn->inner_conn);
}
return 0;
}
void
altcp_default_setprio(struct altcp_pcb *conn, u8_t prio)
{
if (conn && conn->inner_conn) {
altcp_setprio(conn->inner_conn, prio);
}
}
void
altcp_default_dealloc(struct altcp_pcb *conn)
{
LWIP_UNUSED_ARG(conn);
/* nothing to do */
}
err_t
altcp_default_get_tcp_addrinfo(struct altcp_pcb *conn, int local, ip_addr_t *addr, u16_t *port)
{
if (conn && conn->inner_conn) {
return altcp_get_tcp_addrinfo(conn->inner_conn, local, addr, port);
}
return ERR_VAL;
}
ip_addr_t *
altcp_default_get_ip(struct altcp_pcb *conn, int local)
{
if (conn && conn->inner_conn) {
return altcp_get_ip(conn->inner_conn, local);
}
return NULL;
}
u16_t
altcp_default_get_port(struct altcp_pcb *conn, int local)
{
if (conn && conn->inner_conn) {
return altcp_get_port(conn->inner_conn, local);
}
return 0;
}
#ifdef LWIP_DEBUG
enum tcp_state
altcp_default_dbg_get_tcp_state(struct altcp_pcb *conn)
{
if (conn && conn->inner_conn) {
return altcp_dbg_get_tcp_state(conn->inner_conn);
}
return CLOSED;
}
#endif
#endif /* LWIP_ALTCP */

View File

@ -0,0 +1,87 @@
/**
* @file
* Application layered TCP connection API (to be used from TCPIP thread)\n
* This interface mimics the tcp callback API to the application while preventing
* direct linking (much like virtual functions).
* This way, an application can make use of other application layer protocols
* on top of TCP without knowing the details (e.g. TLS, proxy connection).
*
* This file contains allocation implementation that combine several layers.
*/
/*
* Copyright (c) 2017 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt <goldsimon@gmx.de>
*
*/
#include "lwip/opt.h"
#if LWIP_ALTCP /* don't build if not configured for use in lwipopts.h */
#include "lwip/altcp.h"
#include "lwip/altcp_tcp.h"
#include "lwip/altcp_tls.h"
#include "lwip/priv/altcp_priv.h"
#include "lwip/mem.h"
#include <string.h>
#if LWIP_ALTCP_TLS
/** This standard allocator function creates an altcp pcb for
* TLS over TCP */
struct altcp_pcb *
altcp_tls_new(struct altcp_tls_config *config, u8_t ip_type)
{
struct altcp_pcb *inner_conn, *ret;
LWIP_UNUSED_ARG(ip_type);
inner_conn = altcp_tcp_new_ip_type(ip_type);
if (inner_conn == NULL) {
return NULL;
}
ret = altcp_tls_wrap(config, inner_conn);
if (ret == NULL) {
altcp_close(inner_conn);
}
return ret;
}
/** This standard allocator function creates an altcp pcb for
* TLS over TCP */
struct altcp_pcb *
altcp_tls_alloc(void *arg, u8_t ip_type)
{
return altcp_tls_new((struct altcp_tls_config *)arg, ip_type);
}
#endif /* LWIP_ALTCP_TLS */
#endif /* LWIP_ALTCP */

View File

@ -0,0 +1,543 @@
/**
* @file
* Application layered TCP connection API (to be used from TCPIP thread)\n
* This interface mimics the tcp callback API to the application while preventing
* direct linking (much like virtual functions).
* This way, an application can make use of other application layer protocols
* on top of TCP without knowing the details (e.g. TLS, proxy connection).
*
* This file contains the base implementation calling into tcp.
*/
/*
* Copyright (c) 2017 Simon Goldschmidt
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt <goldsimon@gmx.de>
*
*/
#include "lwip/opt.h"
#if LWIP_ALTCP /* don't build if not configured for use in lwipopts.h */
#include "lwip/altcp.h"
#include "lwip/altcp_tcp.h"
#include "lwip/priv/altcp_priv.h"
#include "lwip/tcp.h"
#include "lwip/mem.h"
#include <string.h>
#define ALTCP_TCP_ASSERT_CONN(conn) do { \
LWIP_ASSERT("conn->inner_conn == NULL", (conn)->inner_conn == NULL); \
LWIP_UNUSED_ARG(conn); /* for LWIP_NOASSERT */ } while(0)
#define ALTCP_TCP_ASSERT_CONN_PCB(conn, tpcb) do { \
LWIP_ASSERT("pcb mismatch", (conn)->state == tpcb); \
LWIP_UNUSED_ARG(tpcb); /* for LWIP_NOASSERT */ \
ALTCP_TCP_ASSERT_CONN(conn); } while(0)
/* Variable prototype, the actual declaration is at the end of this file
since it contains pointers to static functions declared here */
extern const struct altcp_functions altcp_tcp_functions;
static void altcp_tcp_setup(struct altcp_pcb *conn, struct tcp_pcb *tpcb);
/* callback functions for TCP */
static err_t
altcp_tcp_accept(void *arg, struct tcp_pcb *new_tpcb, err_t err)
{
struct altcp_pcb *listen_conn = (struct altcp_pcb *)arg;
if (listen_conn && listen_conn->accept) {
/* create a new altcp_conn to pass to the next 'accept' callback */
struct altcp_pcb *new_conn = altcp_alloc();
if (new_conn == NULL) {
return ERR_MEM;
}
altcp_tcp_setup(new_conn, new_tpcb);
return listen_conn->accept(listen_conn->arg, new_conn, err);
}
return ERR_ARG;
}
static err_t
altcp_tcp_connected(void *arg, struct tcp_pcb *tpcb, err_t err)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
if (conn) {
ALTCP_TCP_ASSERT_CONN_PCB(conn, tpcb);
if (conn->connected) {
return conn->connected(conn->arg, conn, err);
}
}
return ERR_OK;
}
static err_t
altcp_tcp_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
if (conn) {
ALTCP_TCP_ASSERT_CONN_PCB(conn, tpcb);
if (conn->recv) {
return conn->recv(conn->arg, conn, p, err);
}
}
if (p != NULL) {
/* prevent memory leaks */
pbuf_free(p);
}
return ERR_OK;
}
static err_t
altcp_tcp_sent(void *arg, struct tcp_pcb *tpcb, u16_t len)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
if (conn) {
ALTCP_TCP_ASSERT_CONN_PCB(conn, tpcb);
if (conn->sent) {
return conn->sent(conn->arg, conn, len);
}
}
return ERR_OK;
}
static err_t
altcp_tcp_poll(void *arg, struct tcp_pcb *tpcb)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
if (conn) {
ALTCP_TCP_ASSERT_CONN_PCB(conn, tpcb);
if (conn->poll) {
return conn->poll(conn->arg, conn);
}
}
return ERR_OK;
}
static void
altcp_tcp_err(void *arg, err_t err)
{
struct altcp_pcb *conn = (struct altcp_pcb *)arg;
if (conn) {
conn->state = NULL; /* already freed */
if (conn->err) {
conn->err(conn->arg, err);
}
altcp_free(conn);
}
}
/* setup functions */
static void
altcp_tcp_remove_callbacks(struct tcp_pcb *tpcb)
{
tcp_arg(tpcb, NULL);
tcp_recv(tpcb, NULL);
tcp_sent(tpcb, NULL);
tcp_err(tpcb, NULL);
tcp_poll(tpcb, NULL, tpcb->pollinterval);
}
static void
altcp_tcp_setup_callbacks(struct altcp_pcb *conn, struct tcp_pcb *tpcb)
{
tcp_arg(tpcb, conn);
tcp_recv(tpcb, altcp_tcp_recv);
tcp_sent(tpcb, altcp_tcp_sent);
tcp_err(tpcb, altcp_tcp_err);
/* tcp_poll is set when interval is set by application */
/* listen is set totally different :-) */
}
static void
altcp_tcp_setup(struct altcp_pcb *conn, struct tcp_pcb *tpcb)
{
altcp_tcp_setup_callbacks(conn, tpcb);
conn->state = tpcb;
conn->fns = &altcp_tcp_functions;
}
struct altcp_pcb *
altcp_tcp_new_ip_type(u8_t ip_type)
{
/* Allocate the tcp pcb first to invoke the priority handling code
if we're out of pcbs */
struct tcp_pcb *tpcb = tcp_new_ip_type(ip_type);
if (tpcb != NULL) {
struct altcp_pcb *ret = altcp_alloc();
if (ret != NULL) {
altcp_tcp_setup(ret, tpcb);
return ret;
} else {
/* altcp_pcb allocation failed -> free the tcp_pcb too */
tcp_close(tpcb);
}
}
return NULL;
}
/** altcp_tcp allocator function fitting to @ref altcp_allocator_t / @ref altcp_new.
*
* arg pointer is not used for TCP.
*/
struct altcp_pcb *
altcp_tcp_alloc(void *arg, u8_t ip_type)
{
LWIP_UNUSED_ARG(arg);
return altcp_tcp_new_ip_type(ip_type);
}
struct altcp_pcb *
altcp_tcp_wrap(struct tcp_pcb *tpcb)
{
if (tpcb != NULL) {
struct altcp_pcb *ret = altcp_alloc();
if (ret != NULL) {
altcp_tcp_setup(ret, tpcb);
return ret;
}
}
return NULL;
}
/* "virtual" functions calling into tcp */
static void
altcp_tcp_set_poll(struct altcp_pcb *conn, u8_t interval)
{
if (conn != NULL) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
tcp_poll(pcb, altcp_tcp_poll, interval);
}
}
static void
altcp_tcp_recved(struct altcp_pcb *conn, u16_t len)
{
if (conn != NULL) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
tcp_recved(pcb, len);
}
}
static err_t
altcp_tcp_bind(struct altcp_pcb *conn, const ip_addr_t *ipaddr, u16_t port)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return ERR_VAL;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
return tcp_bind(pcb, ipaddr, port);
}
static err_t
altcp_tcp_connect(struct altcp_pcb *conn, const ip_addr_t *ipaddr, u16_t port, altcp_connected_fn connected)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return ERR_VAL;
}
ALTCP_TCP_ASSERT_CONN(conn);
conn->connected = connected;
pcb = (struct tcp_pcb *)conn->state;
return tcp_connect(pcb, ipaddr, port, altcp_tcp_connected);
}
static struct altcp_pcb *
altcp_tcp_listen(struct altcp_pcb *conn, u8_t backlog, err_t *err)
{
struct tcp_pcb *pcb;
struct tcp_pcb *lpcb;
if (conn == NULL) {
return NULL;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
lpcb = tcp_listen_with_backlog_and_err(pcb, backlog, err);
if (lpcb != NULL) {
conn->state = lpcb;
tcp_accept(lpcb, altcp_tcp_accept);
return conn;
}
return NULL;
}
static void
altcp_tcp_abort(struct altcp_pcb *conn)
{
if (conn != NULL) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
if (pcb) {
tcp_abort(pcb);
}
}
}
static err_t
altcp_tcp_close(struct altcp_pcb *conn)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return ERR_VAL;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
if (pcb) {
err_t err;
tcp_poll_fn oldpoll = pcb->poll;
altcp_tcp_remove_callbacks(pcb);
err = tcp_close(pcb);
if (err != ERR_OK) {
/* not closed, set up all callbacks again */
altcp_tcp_setup_callbacks(conn, pcb);
/* poll callback is not included in the above */
tcp_poll(pcb, oldpoll, pcb->pollinterval);
return err;
}
conn->state = NULL; /* unsafe to reference pcb after tcp_close(). */
}
altcp_free(conn);
return ERR_OK;
}
static err_t
altcp_tcp_shutdown(struct altcp_pcb *conn, int shut_rx, int shut_tx)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return ERR_VAL;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
return tcp_shutdown(pcb, shut_rx, shut_tx);
}
static err_t
altcp_tcp_write(struct altcp_pcb *conn, const void *dataptr, u16_t len, u8_t apiflags)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return ERR_VAL;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
return tcp_write(pcb, dataptr, len, apiflags);
}
static err_t
altcp_tcp_output(struct altcp_pcb *conn)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return ERR_VAL;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
return tcp_output(pcb);
}
static u16_t
altcp_tcp_mss(struct altcp_pcb *conn)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return 0;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
return tcp_mss(pcb);
}
static u16_t
altcp_tcp_sndbuf(struct altcp_pcb *conn)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return 0;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
return tcp_sndbuf(pcb);
}
static u16_t
altcp_tcp_sndqueuelen(struct altcp_pcb *conn)
{
struct tcp_pcb *pcb;
if (conn == NULL) {
return 0;
}
ALTCP_TCP_ASSERT_CONN(conn);
pcb = (struct tcp_pcb *)conn->state;
return tcp_sndqueuelen(pcb);
}
static void
altcp_tcp_nagle_disable(struct altcp_pcb *conn)
{
if (conn && conn->state) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
tcp_nagle_disable(pcb);
}
}
static void
altcp_tcp_nagle_enable(struct altcp_pcb *conn)
{
if (conn && conn->state) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
tcp_nagle_enable(pcb);
}
}
static int
altcp_tcp_nagle_disabled(struct altcp_pcb *conn)
{
if (conn && conn->state) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
return tcp_nagle_disabled(pcb);
}
return 0;
}
static void
altcp_tcp_setprio(struct altcp_pcb *conn, u8_t prio)
{
if (conn != NULL) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
tcp_setprio(pcb, prio);
}
}
static void
altcp_tcp_dealloc(struct altcp_pcb *conn)
{
LWIP_UNUSED_ARG(conn);
ALTCP_TCP_ASSERT_CONN(conn);
/* no private state to clean up */
}
static err_t
altcp_tcp_get_tcp_addrinfo(struct altcp_pcb *conn, int local, ip_addr_t *addr, u16_t *port)
{
if (conn) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
return tcp_tcp_get_tcp_addrinfo(pcb, local, addr, port);
}
return ERR_VAL;
}
static ip_addr_t *
altcp_tcp_get_ip(struct altcp_pcb *conn, int local)
{
if (conn) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
if (pcb) {
if (local) {
return &pcb->local_ip;
} else {
return &pcb->remote_ip;
}
}
}
return NULL;
}
static u16_t
altcp_tcp_get_port(struct altcp_pcb *conn, int local)
{
if (conn) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
if (pcb) {
if (local) {
return pcb->local_port;
} else {
return pcb->remote_port;
}
}
}
return 0;
}
#ifdef LWIP_DEBUG
static enum tcp_state
altcp_tcp_dbg_get_tcp_state(struct altcp_pcb *conn)
{
if (conn) {
struct tcp_pcb *pcb = (struct tcp_pcb *)conn->state;
ALTCP_TCP_ASSERT_CONN(conn);
if (pcb) {
return pcb->state;
}
}
return CLOSED;
}
#endif
const struct altcp_functions altcp_tcp_functions = {
altcp_tcp_set_poll,
altcp_tcp_recved,
altcp_tcp_bind,
altcp_tcp_connect,
altcp_tcp_listen,
altcp_tcp_abort,
altcp_tcp_close,
altcp_tcp_shutdown,
altcp_tcp_write,
altcp_tcp_output,
altcp_tcp_mss,
altcp_tcp_sndbuf,
altcp_tcp_sndqueuelen,
altcp_tcp_nagle_disable,
altcp_tcp_nagle_enable,
altcp_tcp_nagle_disabled,
altcp_tcp_setprio,
altcp_tcp_dealloc,
altcp_tcp_get_tcp_addrinfo,
altcp_tcp_get_ip,
altcp_tcp_get_port
#ifdef LWIP_DEBUG
, altcp_tcp_dbg_get_tcp_state
#endif
};
#endif /* LWIP_ALTCP */

View File

@ -0,0 +1,240 @@
/**
* @file
* Common functions used throughout the stack.
*
* These are reference implementations of the byte swapping functions.
* Again with the aim of being simple, correct and fully portable.
* Byte swapping is the second thing you would want to optimize. You will
* need to port it to your architecture and in your cc.h:
*
* \#define lwip_htons(x) your_htons
* \#define lwip_htonl(x) your_htonl
*
* Note lwip_ntohs() and lwip_ntohl() are merely references to the htonx counterparts.
*
* If you \#define them to htons() and htonl(), you should
* \#define LWIP_DONT_PROVIDE_BYTEORDER_FUNCTIONS to prevent lwIP from
* defining htonx/ntohx compatibility macros.
* @defgroup sys_nonstandard Non-standard functions
* @ingroup sys_layer
* lwIP provides default implementations for non-standard functions.
* These can be mapped to OS functions to reduce code footprint if desired.
* All defines related to this section must not be placed in lwipopts.h,
* but in arch/cc.h!
* These options cannot be \#defined in lwipopts.h since they are not options
* of lwIP itself, but options of the lwIP port to your system.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Simon Goldschmidt
*
*/
#include "lwip/opt.h"
#include "lwip/def.h"
#include <string.h>
#if BYTE_ORDER == LITTLE_ENDIAN
#if !defined(lwip_htons)
/**
* Convert an u16_t from host- to network byte order.
*
* @param n u16_t in host byte order
* @return n in network byte order
*/
u16_t
lwip_htons(u16_t n)
{
return PP_HTONS(n);
}
#endif /* lwip_htons */
#if !defined(lwip_htonl)
/**
* Convert an u32_t from host- to network byte order.
*
* @param n u32_t in host byte order
* @return n in network byte order
*/
u32_t
lwip_htonl(u32_t n)
{
return PP_HTONL(n);
}
#endif /* lwip_htonl */
#endif /* BYTE_ORDER == LITTLE_ENDIAN */
#ifndef lwip_strnstr
/**
* @ingroup sys_nonstandard
* lwIP default implementation for strnstr() non-standard function.
* This can be \#defined to strnstr() depending on your platform port.
*/
char *
lwip_strnstr(const char *buffer, const char *token, size_t n)
{
const char *p;
size_t tokenlen = strlen(token);
if (tokenlen == 0) {
return LWIP_CONST_CAST(char *, buffer);
}
for (p = buffer; *p && (p + tokenlen <= buffer + n); p++) {
if ((*p == *token) && (strncmp(p, token, tokenlen) == 0)) {
return LWIP_CONST_CAST(char *, p);
}
}
return NULL;
}
#endif
#ifndef lwip_stricmp
/**
* @ingroup sys_nonstandard
* lwIP default implementation for stricmp() non-standard function.
* This can be \#defined to stricmp() depending on your platform port.
*/
int
lwip_stricmp(const char *str1, const char *str2)
{
char c1, c2;
do {
c1 = *str1++;
c2 = *str2++;
if (c1 != c2) {
char c1_upc = c1 | 0x20;
if ((c1_upc >= 'a') && (c1_upc <= 'z')) {
/* characters are not equal an one is in the alphabet range:
downcase both chars and check again */
char c2_upc = c2 | 0x20;
if (c1_upc != c2_upc) {
/* still not equal */
/* don't care for < or > */
return 1;
}
} else {
/* characters are not equal but none is in the alphabet range */
return 1;
}
}
} while (c1 != 0);
return 0;
}
#endif
#ifndef lwip_strnicmp
/**
* @ingroup sys_nonstandard
* lwIP default implementation for strnicmp() non-standard function.
* This can be \#defined to strnicmp() depending on your platform port.
*/
int
lwip_strnicmp(const char *str1, const char *str2, size_t len)
{
char c1, c2;
do {
c1 = *str1++;
c2 = *str2++;
if (c1 != c2) {
char c1_upc = c1 | 0x20;
if ((c1_upc >= 'a') && (c1_upc <= 'z')) {
/* characters are not equal an one is in the alphabet range:
downcase both chars and check again */
char c2_upc = c2 | 0x20;
if (c1_upc != c2_upc) {
/* still not equal */
/* don't care for < or > */
return 1;
}
} else {
/* characters are not equal but none is in the alphabet range */
return 1;
}
}
len--;
} while ((len != 0) && (c1 != 0));
return 0;
}
#endif
#ifndef lwip_itoa
/**
* @ingroup sys_nonstandard
* lwIP default implementation for itoa() non-standard function.
* This can be \#defined to itoa() or snprintf(result, bufsize, "%d", number) depending on your platform port.
*/
void
lwip_itoa(char *result, size_t bufsize, int number)
{
char *res = result;
char *tmp = result + bufsize - 1;
int n = (number >= 0) ? number : -number;
/* handle invalid bufsize */
if (bufsize < 2) {
if (bufsize == 1) {
*result = 0;
}
return;
}
/* First, add sign */
if (number < 0) {
*res++ = '-';
}
/* Then create the string from the end and stop if buffer full,
and ensure output string is zero terminated */
*tmp = 0;
while ((n != 0) && (tmp > res)) {
char val = (char)('0' + (n % 10));
tmp--;
*tmp = val;
n = n / 10;
}
if (n) {
/* buffer is too small */
*result = 0;
return;
}
if (*tmp == 0) {
/* Nothing added? */
*res++ = '0';
*res++ = 0;
return;
}
/* move from temporary buffer to output buffer (sign is not moved) */
memmove(res, tmp, (size_t)((result + bufsize) - tmp));
}
#endif

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,608 @@
/**
* @file
* Internet checksum functions.\n
*
* These are some reference implementations of the checksum algorithm, with the
* aim of being simple, correct and fully portable. Checksumming is the
* first thing you would want to optimize for your platform. If you create
* your own version, link it in and in your cc.h put:
*
* \#define LWIP_CHKSUM your_checksum_routine
*
* Or you can select from the implementations below by defining
* LWIP_CHKSUM_ALGORITHM to 1, 2 or 3.
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#include "lwip/opt.h"
#include "lwip/inet_chksum.h"
#include "lwip/def.h"
#include "lwip/ip_addr.h"
#include <string.h>
#ifndef LWIP_CHKSUM
# define LWIP_CHKSUM lwip_standard_chksum
# ifndef LWIP_CHKSUM_ALGORITHM
# define LWIP_CHKSUM_ALGORITHM 2
# endif
u16_t lwip_standard_chksum(const void *dataptr, int len);
#endif
/* If none set: */
#ifndef LWIP_CHKSUM_ALGORITHM
# define LWIP_CHKSUM_ALGORITHM 0
#endif
#if (LWIP_CHKSUM_ALGORITHM == 1) /* Version #1 */
/**
* lwip checksum
*
* @param dataptr points to start of data to be summed at any boundary
* @param len length of data to be summed
* @return host order (!) lwip checksum (non-inverted Internet sum)
*
* @note accumulator size limits summable length to 64k
* @note host endianess is irrelevant (p3 RFC1071)
*/
u16_t
lwip_standard_chksum(const void *dataptr, int len)
{
u32_t acc;
u16_t src;
const u8_t *octetptr;
acc = 0;
/* dataptr may be at odd or even addresses */
octetptr = (const u8_t *)dataptr;
while (len > 1) {
/* declare first octet as most significant
thus assume network order, ignoring host order */
src = (*octetptr) << 8;
octetptr++;
/* declare second octet as least significant */
src |= (*octetptr);
octetptr++;
acc += src;
len -= 2;
}
if (len > 0) {
/* accumulate remaining octet */
src = (*octetptr) << 8;
acc += src;
}
/* add deferred carry bits */
acc = (acc >> 16) + (acc & 0x0000ffffUL);
if ((acc & 0xffff0000UL) != 0) {
acc = (acc >> 16) + (acc & 0x0000ffffUL);
}
/* This maybe a little confusing: reorder sum using lwip_htons()
instead of lwip_ntohs() since it has a little less call overhead.
The caller must invert bits for Internet sum ! */
return lwip_htons((u16_t)acc);
}
#endif
#if (LWIP_CHKSUM_ALGORITHM == 2) /* Alternative version #2 */
/*
* Curt McDowell
* Broadcom Corp.
* csm@broadcom.com
*
* IP checksum two bytes at a time with support for
* unaligned buffer.
* Works for len up to and including 0x20000.
* by Curt McDowell, Broadcom Corp. 12/08/2005
*
* @param dataptr points to start of data to be summed at any boundary
* @param len length of data to be summed
* @return host order (!) lwip checksum (non-inverted Internet sum)
*/
u16_t
lwip_standard_chksum(const void *dataptr, int len)
{
const u8_t *pb = (const u8_t *)dataptr;
const u16_t *ps;
u16_t t = 0;
u32_t sum = 0;
int odd = ((mem_ptr_t)pb & 1);
/* Get aligned to u16_t */
if (odd && len > 0) {
((u8_t *)&t)[1] = *pb++;
len--;
}
/* Add the bulk of the data */
ps = (const u16_t *)(const void *)pb;
while (len > 1) {
sum += *ps++;
len -= 2;
}
/* Consume left-over byte, if any */
if (len > 0) {
((u8_t *)&t)[0] = *(const u8_t *)ps;
}
/* Add end bytes */
sum += t;
/* Fold 32-bit sum to 16 bits
calling this twice is probably faster than if statements... */
sum = FOLD_U32T(sum);
sum = FOLD_U32T(sum);
/* Swap if alignment was odd */
if (odd) {
sum = SWAP_BYTES_IN_WORD(sum);
}
return (u16_t)sum;
}
#endif
#if (LWIP_CHKSUM_ALGORITHM == 3) /* Alternative version #3 */
/**
* An optimized checksum routine. Basically, it uses loop-unrolling on
* the checksum loop, treating the head and tail bytes specially, whereas
* the inner loop acts on 8 bytes at a time.
*
* @arg start of buffer to be checksummed. May be an odd byte address.
* @len number of bytes in the buffer to be checksummed.
* @return host order (!) lwip checksum (non-inverted Internet sum)
*
* by Curt McDowell, Broadcom Corp. December 8th, 2005
*/
u16_t
lwip_standard_chksum(const void *dataptr, int len)
{
const u8_t *pb = (const u8_t *)dataptr;
const u16_t *ps;
u16_t t = 0;
const u32_t *pl;
u32_t sum = 0, tmp;
/* starts at odd byte address? */
int odd = ((mem_ptr_t)pb & 1);
if (odd && len > 0) {
((u8_t *)&t)[1] = *pb++;
len--;
}
ps = (const u16_t *)(const void *)pb;
if (((mem_ptr_t)ps & 3) && len > 1) {
sum += *ps++;
len -= 2;
}
pl = (const u32_t *)(const void *)ps;
while (len > 7) {
tmp = sum + *pl++; /* ping */
if (tmp < sum) {
tmp++; /* add back carry */
}
sum = tmp + *pl++; /* pong */
if (sum < tmp) {
sum++; /* add back carry */
}
len -= 8;
}
/* make room in upper bits */
sum = FOLD_U32T(sum);
ps = (const u16_t *)pl;
/* 16-bit aligned word remaining? */
while (len > 1) {
sum += *ps++;
len -= 2;
}
/* dangling tail byte remaining? */
if (len > 0) { /* include odd byte */
((u8_t *)&t)[0] = *(const u8_t *)ps;
}
sum += t; /* add end bytes */
/* Fold 32-bit sum to 16 bits
calling this twice is probably faster than if statements... */
sum = FOLD_U32T(sum);
sum = FOLD_U32T(sum);
if (odd) {
sum = SWAP_BYTES_IN_WORD(sum);
}
return (u16_t)sum;
}
#endif
/** Parts of the pseudo checksum which are common to IPv4 and IPv6 */
static u16_t
inet_cksum_pseudo_base(struct pbuf *p, u8_t proto, u16_t proto_len, u32_t acc)
{
struct pbuf *q;
int swapped = 0;
/* iterate through all pbuf in chain */
for (q = p; q != NULL; q = q->next) {
LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): checksumming pbuf %p (has next %p) \n",
(void *)q, (void *)q->next));
acc += LWIP_CHKSUM(q->payload, q->len);
/*LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): unwrapped lwip_chksum()=%"X32_F" \n", acc));*/
/* just executing this next line is probably faster that the if statement needed
to check whether we really need to execute it, and does no harm */
acc = FOLD_U32T(acc);
if (q->len % 2 != 0) {
swapped = !swapped;
acc = SWAP_BYTES_IN_WORD(acc);
}
/*LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): wrapped lwip_chksum()=%"X32_F" \n", acc));*/
}
if (swapped) {
acc = SWAP_BYTES_IN_WORD(acc);
}
acc += (u32_t)lwip_htons((u16_t)proto);
acc += (u32_t)lwip_htons(proto_len);
/* Fold 32-bit sum to 16 bits
calling this twice is probably faster than if statements... */
acc = FOLD_U32T(acc);
acc = FOLD_U32T(acc);
LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): pbuf chain lwip_chksum()=%"X32_F"\n", acc));
return (u16_t)~(acc & 0xffffUL);
}
#if LWIP_IPV4
/* inet_chksum_pseudo:
*
* Calculates the IPv4 pseudo Internet checksum used by TCP and UDP for a pbuf chain.
* IP addresses are expected to be in network byte order.
*
* @param p chain of pbufs over that a checksum should be calculated (ip data part)
* @param src source ip address (used for checksum of pseudo header)
* @param dst destination ip address (used for checksum of pseudo header)
* @param proto ip protocol (used for checksum of pseudo header)
* @param proto_len length of the ip data part (used for checksum of pseudo header)
* @return checksum (as u16_t) to be saved directly in the protocol header
*/
u16_t
inet_chksum_pseudo(struct pbuf *p, u8_t proto, u16_t proto_len,
const ip4_addr_t *src, const ip4_addr_t *dest)
{
u32_t acc;
u32_t addr;
addr = ip4_addr_get_u32(src);
acc = (addr & 0xffffUL);
acc = (u32_t)(acc + ((addr >> 16) & 0xffffUL));
addr = ip4_addr_get_u32(dest);
acc = (u32_t)(acc + (addr & 0xffffUL));
acc = (u32_t)(acc + ((addr >> 16) & 0xffffUL));
/* fold down to 16 bits */
acc = FOLD_U32T(acc);
acc = FOLD_U32T(acc);
return inet_cksum_pseudo_base(p, proto, proto_len, acc);
}
#endif /* LWIP_IPV4 */
#if LWIP_IPV6
/**
* Calculates the checksum with IPv6 pseudo header used by TCP and UDP for a pbuf chain.
* IPv6 addresses are expected to be in network byte order.
*
* @param p chain of pbufs over that a checksum should be calculated (ip data part)
* @param proto ipv6 protocol/next header (used for checksum of pseudo header)
* @param proto_len length of the ipv6 payload (used for checksum of pseudo header)
* @param src source ipv6 address (used for checksum of pseudo header)
* @param dest destination ipv6 address (used for checksum of pseudo header)
* @return checksum (as u16_t) to be saved directly in the protocol header
*/
u16_t
ip6_chksum_pseudo(struct pbuf *p, u8_t proto, u16_t proto_len,
const ip6_addr_t *src, const ip6_addr_t *dest)
{
u32_t acc = 0;
u32_t addr;
u8_t addr_part;
for (addr_part = 0; addr_part < 4; addr_part++) {
addr = src->addr[addr_part];
acc = (u32_t)(acc + (addr & 0xffffUL));
acc = (u32_t)(acc + ((addr >> 16) & 0xffffUL));
addr = dest->addr[addr_part];
acc = (u32_t)(acc + (addr & 0xffffUL));
acc = (u32_t)(acc + ((addr >> 16) & 0xffffUL));
}
/* fold down to 16 bits */
acc = FOLD_U32T(acc);
acc = FOLD_U32T(acc);
return inet_cksum_pseudo_base(p, proto, proto_len, acc);
}
#endif /* LWIP_IPV6 */
/* ip_chksum_pseudo:
*
* Calculates the IPv4 or IPv6 pseudo Internet checksum used by TCP and UDP for a pbuf chain.
* IP addresses are expected to be in network byte order.
*
* @param p chain of pbufs over that a checksum should be calculated (ip data part)
* @param src source ip address (used for checksum of pseudo header)
* @param dst destination ip address (used for checksum of pseudo header)
* @param proto ip protocol (used for checksum of pseudo header)
* @param proto_len length of the ip data part (used for checksum of pseudo header)
* @return checksum (as u16_t) to be saved directly in the protocol header
*/
u16_t
ip_chksum_pseudo(struct pbuf *p, u8_t proto, u16_t proto_len,
const ip_addr_t *src, const ip_addr_t *dest)
{
#if LWIP_IPV6
if (IP_IS_V6(dest)) {
return ip6_chksum_pseudo(p, proto, proto_len, ip_2_ip6(src), ip_2_ip6(dest));
}
#endif /* LWIP_IPV6 */
#if LWIP_IPV4 && LWIP_IPV6
else
#endif /* LWIP_IPV4 && LWIP_IPV6 */
#if LWIP_IPV4
{
return inet_chksum_pseudo(p, proto, proto_len, ip_2_ip4(src), ip_2_ip4(dest));
}
#endif /* LWIP_IPV4 */
}
/** Parts of the pseudo checksum which are common to IPv4 and IPv6 */
static u16_t
inet_cksum_pseudo_partial_base(struct pbuf *p, u8_t proto, u16_t proto_len,
u16_t chksum_len, u32_t acc)
{
struct pbuf *q;
int swapped = 0;
u16_t chklen;
/* iterate through all pbuf in chain */
for (q = p; (q != NULL) && (chksum_len > 0); q = q->next) {
LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): checksumming pbuf %p (has next %p) \n",
(void *)q, (void *)q->next));
chklen = q->len;
if (chklen > chksum_len) {
chklen = chksum_len;
}
acc += LWIP_CHKSUM(q->payload, chklen);
chksum_len = (u16_t)(chksum_len - chklen);
LWIP_ASSERT("delete me", chksum_len < 0x7fff);
/*LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): unwrapped lwip_chksum()=%"X32_F" \n", acc));*/
/* fold the upper bit down */
acc = FOLD_U32T(acc);
if (q->len % 2 != 0) {
swapped = !swapped;
acc = SWAP_BYTES_IN_WORD(acc);
}
/*LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): wrapped lwip_chksum()=%"X32_F" \n", acc));*/
}
if (swapped) {
acc = SWAP_BYTES_IN_WORD(acc);
}
acc += (u32_t)lwip_htons((u16_t)proto);
acc += (u32_t)lwip_htons(proto_len);
/* Fold 32-bit sum to 16 bits
calling this twice is probably faster than if statements... */
acc = FOLD_U32T(acc);
acc = FOLD_U32T(acc);
LWIP_DEBUGF(INET_DEBUG, ("inet_chksum_pseudo(): pbuf chain lwip_chksum()=%"X32_F"\n", acc));
return (u16_t)~(acc & 0xffffUL);
}
#if LWIP_IPV4
/* inet_chksum_pseudo_partial:
*
* Calculates the IPv4 pseudo Internet checksum used by TCP and UDP for a pbuf chain.
* IP addresses are expected to be in network byte order.
*
* @param p chain of pbufs over that a checksum should be calculated (ip data part)
* @param src source ip address (used for checksum of pseudo header)
* @param dst destination ip address (used for checksum of pseudo header)
* @param proto ip protocol (used for checksum of pseudo header)
* @param proto_len length of the ip data part (used for checksum of pseudo header)
* @return checksum (as u16_t) to be saved directly in the protocol header
*/
u16_t
inet_chksum_pseudo_partial(struct pbuf *p, u8_t proto, u16_t proto_len,
u16_t chksum_len, const ip4_addr_t *src, const ip4_addr_t *dest)
{
u32_t acc;
u32_t addr;
addr = ip4_addr_get_u32(src);
acc = (addr & 0xffffUL);
acc = (u32_t)(acc + ((addr >> 16) & 0xffffUL));
addr = ip4_addr_get_u32(dest);
acc = (u32_t)(acc + (addr & 0xffffUL));
acc = (u32_t)(acc + ((addr >> 16) & 0xffffUL));
/* fold down to 16 bits */
acc = FOLD_U32T(acc);
acc = FOLD_U32T(acc);
return inet_cksum_pseudo_partial_base(p, proto, proto_len, chksum_len, acc);
}
#endif /* LWIP_IPV4 */
#if LWIP_IPV6
/**
* Calculates the checksum with IPv6 pseudo header used by TCP and UDP for a pbuf chain.
* IPv6 addresses are expected to be in network byte order. Will only compute for a
* portion of the payload.
*
* @param p chain of pbufs over that a checksum should be calculated (ip data part)
* @param proto ipv6 protocol/next header (used for checksum of pseudo header)
* @param proto_len length of the ipv6 payload (used for checksum of pseudo header)
* @param chksum_len number of payload bytes used to compute chksum
* @param src source ipv6 address (used for checksum of pseudo header)
* @param dest destination ipv6 address (used for checksum of pseudo header)
* @return checksum (as u16_t) to be saved directly in the protocol header
*/
u16_t
ip6_chksum_pseudo_partial(struct pbuf *p, u8_t proto, u16_t proto_len,
u16_t chksum_len, const ip6_addr_t *src, const ip6_addr_t *dest)
{
u32_t acc = 0;
u32_t addr;
u8_t addr_part;
for (addr_part = 0; addr_part < 4; addr_part++) {
addr = src->addr[addr_part];
acc = (u32_t)(acc + (addr & 0xffffUL));
acc = (u32_t)(acc + ((addr >> 16) & 0xffffUL));
addr = dest->addr[addr_part];
acc = (u32_t)(acc + (addr & 0xffffUL));
acc = (u32_t)(acc + ((addr >> 16) & 0xffffUL));
}
/* fold down to 16 bits */
acc = FOLD_U32T(acc);
acc = FOLD_U32T(acc);
return inet_cksum_pseudo_partial_base(p, proto, proto_len, chksum_len, acc);
}
#endif /* LWIP_IPV6 */
/* ip_chksum_pseudo_partial:
*
* Calculates the IPv4 or IPv6 pseudo Internet checksum used by TCP and UDP for a pbuf chain.
*
* @param p chain of pbufs over that a checksum should be calculated (ip data part)
* @param src source ip address (used for checksum of pseudo header)
* @param dst destination ip address (used for checksum of pseudo header)
* @param proto ip protocol (used for checksum of pseudo header)
* @param proto_len length of the ip data part (used for checksum of pseudo header)
* @return checksum (as u16_t) to be saved directly in the protocol header
*/
u16_t
ip_chksum_pseudo_partial(struct pbuf *p, u8_t proto, u16_t proto_len,
u16_t chksum_len, const ip_addr_t *src, const ip_addr_t *dest)
{
#if LWIP_IPV6
if (IP_IS_V6(dest)) {
return ip6_chksum_pseudo_partial(p, proto, proto_len, chksum_len, ip_2_ip6(src), ip_2_ip6(dest));
}
#endif /* LWIP_IPV6 */
#if LWIP_IPV4 && LWIP_IPV6
else
#endif /* LWIP_IPV4 && LWIP_IPV6 */
#if LWIP_IPV4
{
return inet_chksum_pseudo_partial(p, proto, proto_len, chksum_len, ip_2_ip4(src), ip_2_ip4(dest));
}
#endif /* LWIP_IPV4 */
}
/* inet_chksum:
*
* Calculates the Internet checksum over a portion of memory. Used primarily for IP
* and ICMP.
*
* @param dataptr start of the buffer to calculate the checksum (no alignment needed)
* @param len length of the buffer to calculate the checksum
* @return checksum (as u16_t) to be saved directly in the protocol header
*/
u16_t
inet_chksum(const void *dataptr, u16_t len)
{
return (u16_t)~(unsigned int)LWIP_CHKSUM(dataptr, len);
}
/**
* Calculate a checksum over a chain of pbufs (without pseudo-header, much like
* inet_chksum only pbufs are used).
*
* @param p pbuf chain over that the checksum should be calculated
* @return checksum (as u16_t) to be saved directly in the protocol header
*/
u16_t
inet_chksum_pbuf(struct pbuf *p)
{
u32_t acc;
struct pbuf *q;
int swapped = 0;
acc = 0;
for (q = p; q != NULL; q = q->next) {
acc += LWIP_CHKSUM(q->payload, q->len);
acc = FOLD_U32T(acc);
if (q->len % 2 != 0) {
swapped = !swapped;
acc = SWAP_BYTES_IN_WORD(acc);
}
}
if (swapped) {
acc = SWAP_BYTES_IN_WORD(acc);
}
return (u16_t)~(acc & 0xffffUL);
}
/* These are some implementations for LWIP_CHKSUM_COPY, which copies data
* like MEMCPY but generates a checksum at the same time. Since this is a
* performance-sensitive function, you might want to create your own version
* in assembly targeted at your hardware by defining it in lwipopts.h:
* #define LWIP_CHKSUM_COPY(dst, src, len) your_chksum_copy(dst, src, len)
*/
#if (LWIP_CHKSUM_COPY_ALGORITHM == 1) /* Version #1 */
/** Safe but slow: first call MEMCPY, then call LWIP_CHKSUM.
* For architectures with big caches, data might still be in cache when
* generating the checksum after copying.
*/
u16_t
lwip_chksum_copy(void *dst, const void *src, u16_t len)
{
MEMCPY(dst, src, len);
return LWIP_CHKSUM(dst, len);
}
#endif /* (LWIP_CHKSUM_COPY_ALGORITHM == 1) */

View File

@ -0,0 +1,380 @@
/**
* @file
* Modules initialization
*
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*/
#include "lwip/opt.h"
#include "lwip/init.h"
#include "lwip/stats.h"
#include "lwip/sys.h"
#include "lwip/mem.h"
#include "lwip/memp.h"
#include "lwip/pbuf.h"
#include "lwip/netif.h"
#include "lwip/sockets.h"
#include "lwip/ip.h"
#include "lwip/raw.h"
#include "lwip/udp.h"
#include "lwip/priv/tcp_priv.h"
#include "lwip/igmp.h"
#include "lwip/dns.h"
#include "lwip/timeouts.h"
#include "lwip/etharp.h"
#include "lwip/ip6.h"
#include "lwip/nd6.h"
#include "lwip/mld6.h"
#include "lwip/api.h"
#include "netif/ppp/ppp_opts.h"
#include "netif/ppp/ppp_impl.h"
#ifndef LWIP_SKIP_PACKING_CHECK
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/bpstruct.h"
#endif
PACK_STRUCT_BEGIN
struct packed_struct_test {
PACK_STRUCT_FLD_8(u8_t dummy1);
PACK_STRUCT_FIELD(u32_t dummy2);
} PACK_STRUCT_STRUCT;
PACK_STRUCT_END
#ifdef PACK_STRUCT_USE_INCLUDES
# include "arch/epstruct.h"
#endif
#define PACKED_STRUCT_TEST_EXPECTED_SIZE 5
#endif
/* Compile-time sanity checks for configuration errors.
* These can be done independently of LWIP_DEBUG, without penalty.
*/
#ifndef BYTE_ORDER
#error "BYTE_ORDER is not defined, you have to define it in your cc.h"
#endif
#if (!IP_SOF_BROADCAST && IP_SOF_BROADCAST_RECV)
#error "If you want to use broadcast filter per pcb on recv operations, you have to define IP_SOF_BROADCAST=1 in your lwipopts.h"
#endif
#if (!LWIP_UDP && LWIP_UDPLITE)
#error "If you want to use UDP Lite, you have to define LWIP_UDP=1 in your lwipopts.h"
#endif
#if (!LWIP_UDP && LWIP_DHCP)
#error "If you want to use DHCP, you have to define LWIP_UDP=1 in your lwipopts.h"
#endif
#if (!LWIP_UDP && !LWIP_RAW && LWIP_MULTICAST_TX_OPTIONS)
#error "If you want to use LWIP_MULTICAST_TX_OPTIONS, you have to define LWIP_UDP=1 and/or LWIP_RAW=1 in your lwipopts.h"
#endif
#if (!LWIP_UDP && LWIP_DNS)
#error "If you want to use DNS, you have to define LWIP_UDP=1 in your lwipopts.h"
#endif
#if !MEMP_MEM_MALLOC /* MEMP_NUM_* checks are disabled when not using the pool allocator */
#if (LWIP_ARP && ARP_QUEUEING && (MEMP_NUM_ARP_QUEUE<=0))
#error "If you want to use ARP Queueing, you have to define MEMP_NUM_ARP_QUEUE>=1 in your lwipopts.h"
#endif
#if (LWIP_RAW && (MEMP_NUM_RAW_PCB<=0))
#error "If you want to use RAW, you have to define MEMP_NUM_RAW_PCB>=1 in your lwipopts.h"
#endif
#if (LWIP_UDP && (MEMP_NUM_UDP_PCB<=0))
#error "If you want to use UDP, you have to define MEMP_NUM_UDP_PCB>=1 in your lwipopts.h"
#endif
#if (LWIP_TCP && (MEMP_NUM_TCP_PCB<=0))
#error "If you want to use TCP, you have to define MEMP_NUM_TCP_PCB>=1 in your lwipopts.h"
#endif
#if (LWIP_IGMP && (MEMP_NUM_IGMP_GROUP<=1))
#error "If you want to use IGMP, you have to define MEMP_NUM_IGMP_GROUP>1 in your lwipopts.h"
#endif
#if (LWIP_IGMP && !LWIP_MULTICAST_TX_OPTIONS)
#error "If you want to use IGMP, you have to define LWIP_MULTICAST_TX_OPTIONS==1 in your lwipopts.h"
#endif
#if (LWIP_IGMP && !LWIP_IPV4)
#error "IGMP needs LWIP_IPV4 enabled in your lwipopts.h"
#endif
#if ((LWIP_NETCONN || LWIP_SOCKET) && (MEMP_NUM_TCPIP_MSG_API<=0))
#error "If you want to use Sequential API, you have to define MEMP_NUM_TCPIP_MSG_API>=1 in your lwipopts.h"
#endif
/* There must be sufficient timeouts, taking into account requirements of the subsystems. */
#if LWIP_TIMERS && (MEMP_NUM_SYS_TIMEOUT < LWIP_NUM_SYS_TIMEOUT_INTERNAL)
#error "MEMP_NUM_SYS_TIMEOUT is too low to accomodate all required timeouts"
#endif
#if (IP_REASSEMBLY && (MEMP_NUM_REASSDATA > IP_REASS_MAX_PBUFS))
#error "MEMP_NUM_REASSDATA > IP_REASS_MAX_PBUFS doesn't make sense since each struct ip_reassdata must hold 2 pbufs at least!"
#endif
#endif /* !MEMP_MEM_MALLOC */
#if LWIP_WND_SCALE
#if (LWIP_TCP && (TCP_WND > 0xffffffff))
#error "If you want to use TCP, TCP_WND must fit in an u32_t, so, you have to reduce it in your lwipopts.h"
#endif
#if (LWIP_TCP && (TCP_RCV_SCALE > 14))
#error "The maximum valid window scale value is 14!"
#endif
#if (LWIP_TCP && (TCP_WND > (0xFFFFU << TCP_RCV_SCALE)))
#error "TCP_WND is bigger than the configured LWIP_WND_SCALE allows!"
#endif
#if (LWIP_TCP && ((TCP_WND >> TCP_RCV_SCALE) == 0))
#error "TCP_WND is too small for the configured LWIP_WND_SCALE (results in zero window)!"
#endif
#else /* LWIP_WND_SCALE */
#if (LWIP_TCP && (TCP_WND > 0xffff))
#error "If you want to use TCP, TCP_WND must fit in an u16_t, so, you have to reduce it in your lwipopts.h (or enable window scaling)"
#endif
#endif /* LWIP_WND_SCALE */
#if (LWIP_TCP && (TCP_SND_QUEUELEN > 0xffff))
#error "If you want to use TCP, TCP_SND_QUEUELEN must fit in an u16_t, so, you have to reduce it in your lwipopts.h"
#endif
#if (LWIP_TCP && (TCP_SND_QUEUELEN < 2))
#error "TCP_SND_QUEUELEN must be at least 2 for no-copy TCP writes to work"
#endif
#if (LWIP_TCP && ((TCP_MAXRTX > 12) || (TCP_SYNMAXRTX > 12)))
#error "If you want to use TCP, TCP_MAXRTX and TCP_SYNMAXRTX must less or equal to 12 (due to tcp_backoff table), so, you have to reduce them in your lwipopts.h"
#endif
#if (LWIP_TCP && TCP_LISTEN_BACKLOG && ((TCP_DEFAULT_LISTEN_BACKLOG < 0) || (TCP_DEFAULT_LISTEN_BACKLOG > 0xff)))
#error "If you want to use TCP backlog, TCP_DEFAULT_LISTEN_BACKLOG must fit into an u8_t"
#endif
#if (LWIP_TCP && LWIP_TCP_SACK_OUT && !TCP_QUEUE_OOSEQ)
#error "To use LWIP_TCP_SACK_OUT, TCP_QUEUE_OOSEQ needs to be enabled"
#endif
#if (LWIP_TCP && LWIP_TCP_SACK_OUT && (LWIP_TCP_MAX_SACK_NUM < 1))
#error "LWIP_TCP_MAX_SACK_NUM must be greater than 0"
#endif
#if (LWIP_NETIF_API && (NO_SYS==1))
#error "If you want to use NETIF API, you have to define NO_SYS=0 in your lwipopts.h"
#endif
#if ((LWIP_SOCKET || LWIP_NETCONN) && (NO_SYS==1))
#error "If you want to use Sequential API, you have to define NO_SYS=0 in your lwipopts.h"
#endif
#if (LWIP_PPP_API && (NO_SYS==1))
#error "If you want to use PPP API, you have to define NO_SYS=0 in your lwipopts.h"
#endif
#if (LWIP_PPP_API && (PPP_SUPPORT==0))
#error "If you want to use PPP API, you have to enable PPP_SUPPORT in your lwipopts.h"
#endif
#if (((!LWIP_DHCP) || (!LWIP_AUTOIP)) && LWIP_DHCP_AUTOIP_COOP)
#error "If you want to use DHCP/AUTOIP cooperation mode, you have to define LWIP_DHCP=1 and LWIP_AUTOIP=1 in your lwipopts.h"
#endif
#if (((!LWIP_DHCP) || (!LWIP_ARP)) && DHCP_DOES_ARP_CHECK)
#error "If you want to use DHCP ARP checking, you have to define LWIP_DHCP=1 and LWIP_ARP=1 in your lwipopts.h"
#endif
#if (!LWIP_ARP && LWIP_AUTOIP)
#error "If you want to use AUTOIP, you have to define LWIP_ARP=1 in your lwipopts.h"
#endif
#if (LWIP_TCP && ((LWIP_EVENT_API && LWIP_CALLBACK_API) || (!LWIP_EVENT_API && !LWIP_CALLBACK_API)))
#error "One and exactly one of LWIP_EVENT_API and LWIP_CALLBACK_API has to be enabled in your lwipopts.h"
#endif
#if (LWIP_ALTCP && LWIP_EVENT_API)
#error "The application layered tcp API does not work with LWIP_EVENT_API"
#endif
#if (MEM_LIBC_MALLOC && MEM_USE_POOLS)
#error "MEM_LIBC_MALLOC and MEM_USE_POOLS may not both be simultaneously enabled in your lwipopts.h"
#endif
#if (MEM_USE_POOLS && !MEMP_USE_CUSTOM_POOLS)
#error "MEM_USE_POOLS requires custom pools (MEMP_USE_CUSTOM_POOLS) to be enabled in your lwipopts.h"
#endif
#if (PBUF_POOL_BUFSIZE <= MEM_ALIGNMENT)
#error "PBUF_POOL_BUFSIZE must be greater than MEM_ALIGNMENT or the offset may take the full first pbuf"
#endif
#if (DNS_LOCAL_HOSTLIST && !DNS_LOCAL_HOSTLIST_IS_DYNAMIC && !(defined(DNS_LOCAL_HOSTLIST_INIT)))
#error "you have to define define DNS_LOCAL_HOSTLIST_INIT {{'host1', 0x123}, {'host2', 0x234}} to initialize DNS_LOCAL_HOSTLIST"
#endif
#if PPP_SUPPORT && !PPPOS_SUPPORT && !PPPOE_SUPPORT && !PPPOL2TP_SUPPORT
#error "PPP_SUPPORT needs at least one of PPPOS_SUPPORT, PPPOE_SUPPORT or PPPOL2TP_SUPPORT turned on"
#endif
#if PPP_SUPPORT && !PPP_IPV4_SUPPORT && !PPP_IPV6_SUPPORT
#error "PPP_SUPPORT needs PPP_IPV4_SUPPORT and/or PPP_IPV6_SUPPORT turned on"
#endif
#if PPP_SUPPORT && PPP_IPV4_SUPPORT && !LWIP_IPV4
#error "PPP_IPV4_SUPPORT needs LWIP_IPV4 turned on"
#endif
#if PPP_SUPPORT && PPP_IPV6_SUPPORT && !LWIP_IPV6
#error "PPP_IPV6_SUPPORT needs LWIP_IPV6 turned on"
#endif
#if !LWIP_ETHERNET && (LWIP_ARP || PPPOE_SUPPORT)
#error "LWIP_ETHERNET needs to be turned on for LWIP_ARP or PPPOE_SUPPORT"
#endif
#if LWIP_TCPIP_CORE_LOCKING_INPUT && !LWIP_TCPIP_CORE_LOCKING
#error "When using LWIP_TCPIP_CORE_LOCKING_INPUT, LWIP_TCPIP_CORE_LOCKING must be enabled, too"
#endif
#if LWIP_TCP && LWIP_NETIF_TX_SINGLE_PBUF && !TCP_OVERSIZE
#error "LWIP_NETIF_TX_SINGLE_PBUF needs TCP_OVERSIZE enabled to create single-pbuf TCP packets"
#endif
#if LWIP_NETCONN && LWIP_TCP
#if NETCONN_COPY != TCP_WRITE_FLAG_COPY
#error "NETCONN_COPY != TCP_WRITE_FLAG_COPY"
#endif
#if NETCONN_MORE != TCP_WRITE_FLAG_MORE
#error "NETCONN_MORE != TCP_WRITE_FLAG_MORE"
#endif
#endif /* LWIP_NETCONN && LWIP_TCP */
#if LWIP_SOCKET
#endif /* LWIP_SOCKET */
/* Compile-time checks for deprecated options.
*/
#ifdef MEMP_NUM_TCPIP_MSG
#error "MEMP_NUM_TCPIP_MSG option is deprecated. Remove it from your lwipopts.h."
#endif
#ifdef TCP_REXMIT_DEBUG
#error "TCP_REXMIT_DEBUG option is deprecated. Remove it from your lwipopts.h."
#endif
#ifdef RAW_STATS
#error "RAW_STATS option is deprecated. Remove it from your lwipopts.h."
#endif
#ifdef ETHARP_QUEUE_FIRST
#error "ETHARP_QUEUE_FIRST option is deprecated. Remove it from your lwipopts.h."
#endif
#ifdef ETHARP_ALWAYS_INSERT
#error "ETHARP_ALWAYS_INSERT option is deprecated. Remove it from your lwipopts.h."
#endif
#if !NO_SYS && LWIP_TCPIP_CORE_LOCKING && LWIP_COMPAT_MUTEX && !defined(LWIP_COMPAT_MUTEX_ALLOWED)
#error "LWIP_COMPAT_MUTEX cannot prevent priority inversion. It is recommended to implement priority-aware mutexes. (Define LWIP_COMPAT_MUTEX_ALLOWED to disable this error.)"
#endif
#ifndef LWIP_DISABLE_TCP_SANITY_CHECKS
#define LWIP_DISABLE_TCP_SANITY_CHECKS 0
#endif
#ifndef LWIP_DISABLE_MEMP_SANITY_CHECKS
#define LWIP_DISABLE_MEMP_SANITY_CHECKS 0
#endif
/* MEMP sanity checks */
#if MEMP_MEM_MALLOC
#if !LWIP_DISABLE_MEMP_SANITY_CHECKS
#if LWIP_NETCONN || LWIP_SOCKET
#if !MEMP_NUM_NETCONN && LWIP_SOCKET
#error "lwip_sanity_check: WARNING: MEMP_NUM_NETCONN cannot be 0 when using sockets!"
#endif
#else /* MEMP_MEM_MALLOC */
#if MEMP_NUM_NETCONN > (MEMP_NUM_TCP_PCB+MEMP_NUM_TCP_PCB_LISTEN+MEMP_NUM_UDP_PCB+MEMP_NUM_RAW_PCB)
#error "lwip_sanity_check: WARNING: MEMP_NUM_NETCONN should be less than the sum of MEMP_NUM_{TCP,RAW,UDP}_PCB+MEMP_NUM_TCP_PCB_LISTEN. If you know what you are doing, define LWIP_DISABLE_MEMP_SANITY_CHECKS to 1 to disable this error."
#endif
#endif /* LWIP_NETCONN || LWIP_SOCKET */
#endif /* !LWIP_DISABLE_MEMP_SANITY_CHECKS */
#if MEM_USE_POOLS
#error "MEMP_MEM_MALLOC and MEM_USE_POOLS cannot be enabled at the same time"
#endif
#ifdef LWIP_HOOK_MEMP_AVAILABLE
#error "LWIP_HOOK_MEMP_AVAILABLE doesn't make sense with MEMP_MEM_MALLOC"
#endif
#endif /* MEMP_MEM_MALLOC */
/* TCP sanity checks */
#if !LWIP_DISABLE_TCP_SANITY_CHECKS
#if LWIP_TCP
#if !MEMP_MEM_MALLOC && (MEMP_NUM_TCP_SEG < TCP_SND_QUEUELEN)
#error "lwip_sanity_check: WARNING: MEMP_NUM_TCP_SEG should be at least as big as TCP_SND_QUEUELEN. If you know what you are doing, define LWIP_DISABLE_TCP_SANITY_CHECKS to 1 to disable this error."
#endif
#if TCP_SND_BUF < (2 * TCP_MSS)
#error "lwip_sanity_check: WARNING: TCP_SND_BUF must be at least as much as (2 * TCP_MSS) for things to work smoothly. If you know what you are doing, define LWIP_DISABLE_TCP_SANITY_CHECKS to 1 to disable this error."
#endif
#if TCP_SND_QUEUELEN < (2 * (TCP_SND_BUF / TCP_MSS))
#error "lwip_sanity_check: WARNING: TCP_SND_QUEUELEN must be at least as much as (2 * TCP_SND_BUF/TCP_MSS) for things to work. If you know what you are doing, define LWIP_DISABLE_TCP_SANITY_CHECKS to 1 to disable this error."
#endif
#if TCP_SNDLOWAT >= TCP_SND_BUF
#error "lwip_sanity_check: WARNING: TCP_SNDLOWAT must be less than TCP_SND_BUF. If you know what you are doing, define LWIP_DISABLE_TCP_SANITY_CHECKS to 1 to disable this error."
#endif
#if TCP_SNDLOWAT >= (0xFFFF - (4 * TCP_MSS))
#error "lwip_sanity_check: WARNING: TCP_SNDLOWAT must at least be 4*MSS below u16_t overflow!"
#endif
#if TCP_SNDQUEUELOWAT >= TCP_SND_QUEUELEN
#error "lwip_sanity_check: WARNING: TCP_SNDQUEUELOWAT must be less than TCP_SND_QUEUELEN. If you know what you are doing, define LWIP_DISABLE_TCP_SANITY_CHECKS to 1 to disable this error."
#endif
#if !MEMP_MEM_MALLOC && PBUF_POOL_SIZE && (PBUF_POOL_BUFSIZE <= (PBUF_LINK_ENCAPSULATION_HLEN + PBUF_LINK_HLEN + PBUF_IP_HLEN + PBUF_TRANSPORT_HLEN))
#error "lwip_sanity_check: WARNING: PBUF_POOL_BUFSIZE does not provide enough space for protocol headers. If you know what you are doing, define LWIP_DISABLE_TCP_SANITY_CHECKS to 1 to disable this error."
#endif
#if !MEMP_MEM_MALLOC && PBUF_POOL_SIZE && (TCP_WND > (PBUF_POOL_SIZE * (PBUF_POOL_BUFSIZE - (PBUF_LINK_ENCAPSULATION_HLEN + PBUF_LINK_HLEN + PBUF_IP_HLEN + PBUF_TRANSPORT_HLEN))))
#error "lwip_sanity_check: WARNING: TCP_WND is larger than space provided by PBUF_POOL_SIZE * (PBUF_POOL_BUFSIZE - protocol headers). If you know what you are doing, define LWIP_DISABLE_TCP_SANITY_CHECKS to 1 to disable this error."
#endif
#if TCP_WND < TCP_MSS
#error "lwip_sanity_check: WARNING: TCP_WND is smaller than MSS. If you know what you are doing, define LWIP_DISABLE_TCP_SANITY_CHECKS to 1 to disable this error."
#endif
#endif /* LWIP_TCP */
#endif /* !LWIP_DISABLE_TCP_SANITY_CHECKS */
/**
* @ingroup lwip_nosys
* Initialize all modules.
* Use this in NO_SYS mode. Use tcpip_init() otherwise.
*/
void
lwip_init(void)
{
#ifndef LWIP_SKIP_CONST_CHECK
int a = 0;
LWIP_UNUSED_ARG(a);
LWIP_ASSERT("LWIP_CONST_CAST not implemented correctly. Check your lwIP port.", LWIP_CONST_CAST(void *, &a) == &a);
#endif
#ifndef LWIP_SKIP_PACKING_CHECK
LWIP_ASSERT("Struct packing not implemented correctly. Check your lwIP port.", sizeof(struct packed_struct_test) == PACKED_STRUCT_TEST_EXPECTED_SIZE);
#endif
/* Modules initialization */
stats_init();
#if !NO_SYS
sys_init();
#endif /* !NO_SYS */
mem_init();
memp_init();
pbuf_init();
netif_init();
#if LWIP_IPV4
ip_init();
#if LWIP_ARP
etharp_init();
#endif /* LWIP_ARP */
#endif /* LWIP_IPV4 */
#if LWIP_RAW
raw_init();
#endif /* LWIP_RAW */
#if LWIP_UDP
udp_init();
#endif /* LWIP_UDP */
#if LWIP_TCP
tcp_init();
#endif /* LWIP_TCP */
#if LWIP_IGMP
igmp_init();
#endif /* LWIP_IGMP */
#if LWIP_DNS
dns_init();
#endif /* LWIP_DNS */
#if PPP_SUPPORT
ppp_init();
#endif
#if LWIP_TIMERS
sys_timeouts_init();
#endif /* LWIP_TIMERS */
}

View File

@ -0,0 +1,167 @@
/**
* @file
* Common IPv4 and IPv6 code
*
* @defgroup ip IP
* @ingroup callbackstyle_api
*
* @defgroup ip4 IPv4
* @ingroup ip
*
* @defgroup ip6 IPv6
* @ingroup ip
*
* @defgroup ipaddr IP address handling
* @ingroup infrastructure
*
* @defgroup ip4addr IPv4 only
* @ingroup ipaddr
*
* @defgroup ip6addr IPv6 only
* @ingroup ipaddr
*/
/*
* Copyright (c) 2001-2004 Swedish Institute of Computer Science.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. The name of the author may not be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
* SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE.
*
* This file is part of the lwIP TCP/IP stack.
*
* Author: Adam Dunkels <adam@sics.se>
*
*/
#include "lwip/opt.h"
#if LWIP_IPV4 || LWIP_IPV6
#include "lwip/ip_addr.h"
#include "lwip/ip.h"
/** Global data for both IPv4 and IPv6 */
struct ip_globals ip_data;
#if LWIP_IPV4 && LWIP_IPV6
const ip_addr_t ip_addr_any_type = IPADDR_ANY_TYPE_INIT;
/**
* @ingroup ipaddr
* Convert numeric IP address (both versions) into ASCII representation.
* returns ptr to static buffer; not reentrant!
*
* @param addr ip address in network order to convert
* @return pointer to a global static (!) buffer that holds the ASCII
* representation of addr
*/
char *ipaddr_ntoa(const ip_addr_t *addr)
{
if (addr == NULL) {
return NULL;
}
if (IP_IS_V6(addr)) {
return ip6addr_ntoa(ip_2_ip6(addr));
} else {
return ip4addr_ntoa(ip_2_ip4(addr));
}
}
/**
* @ingroup ipaddr
* Same as ipaddr_ntoa, but reentrant since a user-supplied buffer is used.
*
* @param addr ip address in network order to convert
* @param buf target buffer where the string is stored
* @param buflen length of buf
* @return either pointer to buf which now holds the ASCII
* representation of addr or NULL if buf was too small
*/
char *ipaddr_ntoa_r(const ip_addr_t *addr, char *buf, int buflen)
{
if (addr == NULL) {
return NULL;
}
if (IP_IS_V6(addr)) {
return ip6addr_ntoa_r(ip_2_ip6(addr), buf, buflen);
} else {
return ip4addr_ntoa_r(ip_2_ip4(addr), buf, buflen);
}
}
/**
* @ingroup ipaddr
* Convert IP address string (both versions) to numeric.
* The version is auto-detected from the string.
*
* @param cp IP address string to convert
* @param addr conversion result is stored here
* @return 1 on success, 0 on error
*/
int
ipaddr_aton(const char *cp, ip_addr_t *addr)
{
if (cp != NULL) {
const char *c;
for (c = cp; *c != 0; c++) {
if (*c == ':') {
/* contains a colon: IPv6 address */
if (addr) {
IP_SET_TYPE_VAL(*addr, IPADDR_TYPE_V6);
}
return ip6addr_aton(cp, ip_2_ip6(addr));
} else if (*c == '.') {
/* contains a dot: IPv4 address */
break;
}
}
/* call ip4addr_aton as fallback or if IPv4 was found */
if (addr) {
IP_SET_TYPE_VAL(*addr, IPADDR_TYPE_V4);
}
return ip4addr_aton(cp, ip_2_ip4(addr));
}
return 0;
}
/**
* @ingroup lwip_nosys
* If both IP versions are enabled, this function can dispatch packets to the correct one.
* Don't call directly, pass to netif_add() and call netif->input().
*/
err_t
ip_input(struct pbuf *p, struct netif *inp)
{
if (p != NULL) {
if (IP_HDR_GET_VERSION(p->payload) == 6) {
return ip6_input(p, inp);
}
return ip4_input(p, inp);
}
return ERR_VAL;
}
#endif /* LWIP_IPV4 && LWIP_IPV6 */
#endif /* LWIP_IPV4 || LWIP_IPV6 */

View File

@ -0,0 +1,10 @@
SRC_FILES += autoip.c \
dhcp.c \
etharp.c \
icmp.c \
igmp.c \
ip4_addr.c \
ip4_frag.c \
ip4.c
include $(KERNEL_ROOT)/compiler.mk

Some files were not shown because too many files have changed in this diff Show More