You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
151 lines
4.8 KiB
151 lines
4.8 KiB
/* BSD Socket API Example
|
|
|
|
This example code is in the Public Domain (or CC0 licensed, at your option.)
|
|
|
|
Unless required by applicable law or agreed to in writing, this
|
|
software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
|
|
CONDITIONS OF ANY KIND, either express or implied.
|
|
*/
|
|
|
|
#include "platform.h"
|
|
#if PL_CONFIG_USE_UDP_SERVER
|
|
#include "udp_server.h"
|
|
#include <string.h>
|
|
#include <sys/param.h>
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "esp_system.h"
|
|
#include "esp_wifi.h"
|
|
#include "esp_event.h"
|
|
#include "esp_log.h"
|
|
#include "nvs_flash.h"
|
|
#include "esp_netif.h"
|
|
|
|
#include "lwip/err.h"
|
|
#include "lwip/sockets.h"
|
|
#include "lwip/sys.h"
|
|
#include <lwip/netdb.h>
|
|
|
|
#include "Shell.h"
|
|
#include "McuUtility.h"
|
|
#include "McuLog.h"
|
|
|
|
#define CONFIG_EXAMPLE_IPV4
|
|
|
|
static TaskHandle_t taskHandle = NULL; /* udp server task handle */
|
|
|
|
static int SendToSocket(int sock, const char *msg, const struct sockaddr *to, socklen_t tolen) {
|
|
return sendto(sock, msg, McuUtility_strlen((char*)msg), 0, to, tolen);
|
|
}
|
|
|
|
static void udp_server_task(void *pvParameters) {
|
|
char rx_buffer[128];
|
|
char addr_str[128];
|
|
int addr_family;
|
|
int ip_protocol;
|
|
|
|
vTaskSuspend(NULL); /* UDP_Server_Start() will wake me up */
|
|
for(;;) {
|
|
#ifdef CONFIG_EXAMPLE_IPV4
|
|
struct sockaddr_in dest_addr;
|
|
|
|
dest_addr.sin_addr.s_addr = htonl(INADDR_ANY); /** 0.0.0.0 */
|
|
dest_addr.sin_family = AF_INET;
|
|
dest_addr.sin_port = htons(UDP_SERVER_PORT);
|
|
addr_family = AF_INET;
|
|
ip_protocol = IPPROTO_IP;
|
|
inet_ntoa_r(dest_addr.sin_addr, addr_str, sizeof(addr_str) - 1);
|
|
#else // IPV6
|
|
struct sockaddr_in6 dest_addr;
|
|
bzero(&dest_addr.sin6_addr.un, sizeof(dest_addr.sin6_addr.un));
|
|
dest_addr.sin6_family = AF_INET6;
|
|
dest_addr.sin6_port = htons(UDP_SERVER_PORT);PO
|
|
addr_family = AF_INET6;
|
|
ip_protocol = IPPROTO_IPV6;
|
|
inet6_ntoa_r(dest_addr.sin6_addr, addr_str, sizeof(addr_str) - 1);
|
|
#endif
|
|
|
|
int sock = socket(addr_family, SOCK_DGRAM, ip_protocol);
|
|
if (sock < 0) {
|
|
McuLog_error("Unable to create socket: errno %d", errno);
|
|
break;
|
|
}
|
|
McuLog_info("Socket created");
|
|
|
|
int err = bind(sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
|
|
if (err < 0) {
|
|
McuLog_error("Socket unable to bind: errno %d", errno);
|
|
}
|
|
McuLog_info("Socket bound, port %d", UDP_SERVER_PORT);
|
|
while (1) {
|
|
McuLog_info("Waiting for data on port %d", UDP_SERVER_PORT);
|
|
struct sockaddr_in6 source_addr; /* Large enough for both IPv4 or IPv6 */
|
|
socklen_t socklen = sizeof(source_addr);
|
|
|
|
/* receive data (blocking): */
|
|
int len = recvfrom(sock, rx_buffer, sizeof(rx_buffer)-1, 0, (struct sockaddr *)&source_addr, &socklen);
|
|
|
|
/* Error occurred during receiving */
|
|
if (len < 0) {
|
|
McuLog_error("recvfrom failed: errno %d", errno);
|
|
break;
|
|
} else { /* Data received */
|
|
/* Get the sender's ip address as string */
|
|
if (source_addr.sin6_family == PF_INET) {
|
|
inet_ntoa_r(((struct sockaddr_in *)&source_addr)->sin_addr.s_addr, addr_str, sizeof(addr_str) - 1);
|
|
} else if (source_addr.sin6_family == PF_INET6) {
|
|
inet6_ntoa_r(source_addr.sin6_addr, addr_str, sizeof(addr_str) - 1);
|
|
}
|
|
rx_buffer[len] = '\0'; /* Null-terminate whatever we received and treat like a string... */
|
|
McuLog_info("Received %d bytes from %s:\n%s", len, addr_str, rx_buffer);
|
|
|
|
/* \TODO Need to handle messages and send them to the robot */
|
|
|
|
/* send back response */
|
|
unsigned char test_response[128];
|
|
int err;
|
|
|
|
McuLog_info("Sending back response");
|
|
McuUtility_strcpy(test_response, sizeof(test_response), (unsigned char*)"OK"); /* default response */
|
|
if (McuUtility_strncmp(rx_buffer, "test", sizeof("test")-1)==0) { /* hard-coded command */
|
|
McuUtility_strcpy(test_response, sizeof(test_response), (unsigned char*)"test_response");
|
|
}
|
|
err = SendToSocket(sock, (const char*)test_response, (struct sockaddr *)&source_addr, sizeof(source_addr));
|
|
if (err < 0) {
|
|
McuLog_error("Error occurred during sending: errno %d", errno);
|
|
}
|
|
} /* if */
|
|
} /* while */
|
|
if (sock != -1) {
|
|
McuLog_error("Shutting down socket and restarting...");
|
|
shutdown(sock, 0);
|
|
close(sock);
|
|
}
|
|
} /* for */
|
|
vTaskDelete(NULL);
|
|
}
|
|
|
|
void UDP_Server_Start(void) {
|
|
if (taskHandle!=NULL) {
|
|
vTaskResume(taskHandle);
|
|
}
|
|
}
|
|
|
|
void UDP_Server_Stop(void) {
|
|
if (taskHandle!=NULL) {
|
|
vTaskSuspend(taskHandle);
|
|
}
|
|
}
|
|
|
|
void UDP_Server_Init(void) {
|
|
BaseType_t res;
|
|
|
|
res = xTaskCreate(udp_server_task, "udp_server", (16*1024)/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+5, &taskHandle);
|
|
if (res==pdPASS) {
|
|
McuLog_info("created UDP server task");
|
|
} else {
|
|
McuLog_error("failed creating UDP server task!");
|
|
}
|
|
}
|
|
|
|
#endif /* PL_CONFIG_USE_UDP_SERVER */
|
|
|