/**************************************************************************** * netutils/discover/discover.c * * Copyright (C) 2012 Max Holtzberg. All rights reserved. * Copyright (C) 2008, 2011-2012 Gregory Nutt. All rights reserved. * * Authors: Max Holtzberg * Gregory Nutt * * This code is derived from the netutils/dhcpd code. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name NuttX nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /**************************************************************************** * Included Files ****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include "netutils/discover.h" /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ /* Configuration ************************************************************/ #ifndef CONFIG_DISCOVER_STACK_SIZE # define CONFIG_DISCOVER_STACK_SIZE 1024 #endif #ifndef CONFIG_DISCOVER_PRIORITY # define CONFIG_DISCOVER_PRIORITY SCHED_PRIORITY_DEFAULT #endif #ifndef CONFIG_DISCOVER_PORT # define CONFIG_DISCOVER_PORT 96 #endif #ifndef CONFIG_DISCOVER_INTERFACE # define CONFIG_DISCOVER_INTERFACE "eth0" #endif #ifndef CONFIG_DISCOVER_DEVICE_CLASS # define CONFIG_DISCOVER_DEVICE_CLASS DISCOVER_ALL #endif #ifndef CONFIG_DISCOVER_DESCR # define CONFIG_DISCOVER_DESCR CONFIG_ARCH_BOARD #endif /* Internal Definitions *****************************************************/ /* Discover request packet format: * Byte Description * 0 Protocol identifier (0x99) * 1 Request command 0x01 * 2 Destination device class (For querying subsets of available devices) * 0xff for all devices * 3 Checksum (Byte 0 - Byte 1 - Byte n) & 0xff */ /* Discover response packet format: * Byte Description * 0 Protocol identifier (0x99) * 1 Response command (0x02) * 2-33 Device description string with 0 bytes filled * 34 Checksum (Byte 0 - Byte 1 - Byte n) & 0xff */ #define DISCOVER_PROTO_ID 0x99 #define DISCOVER_REQUEST 0x01 #define DISCOVER_RESPONSE 0x02 #define DISCOVER_ALL 0xff #define DISCOVER_REQUEST_SIZE 4 #define DISCOVER_RESPONSE_SIZE 35 /**************************************************************************** * Private Types ****************************************************************************/ typedef uint8_t request_t[DISCOVER_REQUEST_SIZE]; typedef uint8_t response_t[DISCOVER_RESPONSE_SIZE]; struct discover_state_s { struct discover_info_s info; in_addr_t serverip; request_t request; response_t response; }; /**************************************************************************** * Public Data ****************************************************************************/ struct discover_state_s g_state = { {CONFIG_DISCOVER_DEVICE_CLASS, CONFIG_DISCOVER_DESCR} }; /**************************************************************************** * Private Function Prototypes ****************************************************************************/ static int discover_daemon(int argc, char *argv[]); static inline int discover_socket(void); static inline int discover_openlistener(void); static inline int discover_openresponder(void); static inline int discover_parse(request_t packet); static inline int discover_respond(in_addr_t *ipaddr); static inline void discover_initresponse(void); /**************************************************************************** * Private Functions ****************************************************************************/ static inline void discover_initresponse() { int chk = 0; int i; g_state.response[0] = DISCOVER_PROTO_ID; g_state.response[1] = DISCOVER_RESPONSE; strncpy((char*) &g_state.response[2], g_state.info.description, DISCOVER_RESPONSE_SIZE-3); for (i = 0; i < DISCOVER_RESPONSE_SIZE-1; i++) { chk -= g_state.response[i]; } /* Append check sum */ g_state.response[DISCOVER_RESPONSE_SIZE-1] = chk & 0xff; } static int discover_daemon(int argc, char *argv[]) { int sockfd = -1; int nbytes; int addrlen = sizeof(struct sockaddr_in); struct sockaddr_in srcaddr; /* memset(&g_state, 0, sizeof(struct discover_state_s)); */ discover_initresponse(); ninfo("Started\n"); for (;;) { /* Create a socket to listen for requests from DHCP clients */ if (sockfd < 0) { sockfd = discover_openlistener(); if (sockfd < 0) { nerr("ERROR: Failed to create socket\n"); break; } } /* Read the next packet */ nbytes = recvfrom(sockfd, &g_state.request, sizeof(g_state.request), 0, (struct sockaddr*) &srcaddr, (socklen_t *) &addrlen); if (nbytes < 0) { /* On errors (other EINTR), close the socket and try again */ nerr("ERROR: recv failed: %d\n", errno); if (errno != EINTR) { close(sockfd); sockfd = -1; } continue; } if (discover_parse(g_state.request) != OK) { continue; } ninfo("Received discover from %08lx'\n", srcaddr.sin_addr.s_addr); discover_respond(&srcaddr.sin_addr.s_addr); } return OK; } static inline int discover_parse(request_t packet) { int i; uint8_t chk = 0; if (packet[0] != DISCOVER_PROTO_ID) { nerr("ERROR: Wrong protocol id: %d\n", packet[0]); return ERROR; } if (packet[1] != DISCOVER_REQUEST) { nerr("ERROR: Wrong command: %d\n", packet[1]); return ERROR; } if (packet[2] == 0xff || packet[2] == g_state.info.devclass) { for (i = 0; i < DISCOVER_REQUEST_SIZE-1; i++) chk -= packet[i]; if ((chk & 0xff) != packet[3]) { nerr("ERROR: Checksum does not match: %d\n", packet[3]); return ERROR; } else { return OK; } } return ERROR; } static inline int discover_respond(in_addr_t *ipaddr) { struct sockaddr_in addr; int sockfd; int ret; sockfd = discover_openresponder(); if (sockfd < 0) { nerr("ERROR: discover_openresponder failed\n"); return ERROR; } /* Then send the response to the DHCP client port at that address */ memset(&addr, 0, sizeof(struct sockaddr_in)); addr.sin_family = AF_INET; addr.sin_port = HTONS(CONFIG_DISCOVER_PORT); addr.sin_addr.s_addr = *ipaddr; ret = sendto(sockfd, &g_state.response, sizeof(g_state.response), 0, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret < 0) { nerr("ERROR: Could not send discovery response: %d\n", errno); } close(sockfd); return ret; } static inline int discover_socket() { int sockfd; #if defined(HAVE_SO_REUSEADDR) || defined(HAVE_SO_BROADCAST) int optval; int ret; #endif /* Create a socket to listen for requests from DHCP clients */ sockfd = socket(PF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { nerr("ERROR: socket failed: %d\n", errno); return ERROR; } /* Configure the socket */ #ifdef HAVE_SO_REUSEADDR optval = 1; ret = setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (void*)&optval, sizeof(int)); if (ret < 0) { nerr("ERROR: setsockopt SO_REUSEADDR failed: %d\n", errno); close(sockfd); return ERROR; } #endif #ifdef HAVE_SO_BROADCAST optval = 1; ret = setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, (void*)&optval, sizeof(int)); if (ret < 0) { nerr("ERROR: setsockopt SO_BROADCAST failed: %d\n", errno); close(sockfd); return ERROR; } #endif return sockfd; } static inline int discover_openlistener() { struct sockaddr_in addr; struct ifreq req; int sockfd; int ret; /* Create a socket to listen for requests from DHCP clients */ sockfd = discover_socket(); if (sockfd < 0) { nerr("ERROR: socket failed: %d\n", errno); return ERROR; } /* Get the IP address of the selected device */ strncpy(req.ifr_name, CONFIG_DISCOVER_INTERFACE, IFNAMSIZ); ret = ioctl(sockfd, SIOCGIFADDR, (unsigned long)&req); if (ret < 0) { nerr("ERROR: setsockopt SIOCGIFADDR failed: %d\n", errno); close(sockfd); return ERROR; } g_state.serverip = ((struct sockaddr_in*)&req.ifr_addr)->sin_addr.s_addr; ninfo("serverip: %08lx\n", ntohl(g_state.serverip)); /* Bind the socket to a local port. We have to bind to INADDRY_ANY to * receive broadcast messages. */ addr.sin_family = AF_INET; addr.sin_port = htons(CONFIG_DISCOVER_PORT); addr.sin_addr.s_addr = INADDR_ANY; ret = bind(sockfd, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret < 0) { nerr("ERROR: bind failed, port=%d addr=%08lx: %d\n", addr.sin_port, (long)addr.sin_addr.s_addr, errno); close(sockfd); return ERROR; } return sockfd; } static inline int discover_openresponder(void) { struct sockaddr_in addr; int sockfd; int ret; /* Create a socket for responding to discovery message */ sockfd = discover_socket(); if (sockfd < 0) { nerr("ERROR: socket failed: %d\n", errno); return ERROR; } /* Bind the socket to a local port.*/ addr.sin_family = AF_INET; addr.sin_port = 0; addr.sin_addr.s_addr = g_state.serverip; ret = bind(sockfd, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); if (ret < 0) { nerr("ERROR: bind failed, port=%d addr=%08lx: %d\n", addr.sin_port, (long)addr.sin_addr.s_addr, errno); close(sockfd); return ERROR; } return sockfd; } /**************************************************************************** * Public Functions ****************************************************************************/ /**************************************************************************** * Name: discover_start * * Description: * Start the discover daemon. * * Return: * The process ID (pid) of the new discover daemon is returned on * success; A negated errno is returned if the daemon was not successfully * started. * ****************************************************************************/ int discover_start(struct discover_info_s *info) { pid_t pid; if (info) { g_state.info = *info; } /* Then start the new daemon */ pid = task_create("Discover daemon", CONFIG_DISCOVER_PRIORITY, CONFIG_DISCOVER_STACK_SIZE, discover_daemon, NULL); if (pid < 0) { int errval = errno; nerr("ERROR: Failed to start the discover daemon: %d\n", errval); return -errval; } /* Return success */ return pid; }