/**
* Allocates space for a new socket structure
* @return Pointer to the allocated structure or NULL on failure
*/
comms_socket_t *comms_sock_new(void)
{
comms_socket_t *sock;
sock = (comms_socket_t *) calloc(1, sizeof(struct comms_socket));
if (!sock) return NULL;
log_enter();
sock->can_read = false;
sock->can_write = false;
sock->have_exception = false;
sock->poll_handle = NULL;
sock->state = NETSOCK_STATE_NONE;
sock->fd = INVALID_SOCK;
sock->last_errno = -1;
sock->is_socket = true;
sock->in_buffer = netbuf_new(1, 4 * 1024 * 1024);
if (!sock->in_buffer) {
BLAST_SAFE_FREE(sock);
blast_err("Could not allocate in_buffer");
return NULL;
}
sock->out_buffer = netbuf_new(1, 1024 * 1024);
if (!sock->out_buffer) {
netbuf_free(sock->in_buffer);
BLAST_SAFE_FREE(sock);
log_leave("Could not allocate out_buffer");
return NULL;
}
log_leave();
return sock;
}
err_t __netconn_recv(struct netconn *netconn, struct netbuf **nb, int bsize)
{
size_t numRead = 0;
char *buf = malloc(bsize + 1);
if(!buf) {
error("malloc(NULL)");
return -1;
}
struct netbuf *b = netbuf_new();
if(!b) {
error("netbuf_new()");
return -1;
}
numRead = sk_readline(netconn->sk, buf, bsize);
if(numRead == -1) {
error("read(%d)", netconn->sk);
return -1;
}
dprintf("line [%d]:", numRead);
mem_printf(buf, MIN(numRead, bsize-1));
numRead -= (buf[MIN(numRead, bsize) - 1] == '\0');
if(netbuf_ref(b, buf, numRead) != ERR_OK) {
error("netbuf_ref()");
return -1;
}
*nb = b;
return 0;
}
/*! \brief event handler for mpu9150 udp data
* send one packet of mpu9150 data on event.
*/
static void data_udp_send_mpu9150_data(eventid_t id) {
(void) id;
uint8_t* data;
MPU_packet packet;
const char myid[(sizeof("MPU9")-1)] = "MPU9";
memset (&packet.timestamp, 0, sizeof(packet.timestamp));
strncpy(packet.ID, myid, sizeof(myid));
memcpy(&packet.data, (void*) &mpu9150_current_read, sizeof(MPU9150_read_data) );
packet.data_length = (uint16_t) sizeof(MPU9150_read_data);
mpu9150_mac_info.buf = netbuf_new();
data = netbuf_alloc(mpu9150_mac_info.buf, sizeof(packet));
if(data != NULL) {
memcpy (data, (void*) &packet, sizeof(packet));
//palSetPad(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN);
netconn_send(mpu9150_mac_info.conn, mpu9150_mac_info.buf);
//palClearPad(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN);
netbuf_delete(mpu9150_mac_info.buf);
}
}
void message_transmit(uint8_t *input_buffer, size_t input_buffer_size, ip_addr_t *addr, uint16_t port)
{
struct netconn *conn;
struct netbuf *buf;
conn = netconn_new(NETCONN_UDP);
if (conn == NULL) {
// TODO: Do something useful
return;
}
buf = netbuf_new();
if (buf == NULL) {
// TODO: Do something useful
return;
}
netbuf_ref(buf, input_buffer, input_buffer_size);
netconn_sendto(conn, buf, addr, port);
netbuf_delete(buf);
netconn_delete(conn);
}
database_user_t* database_create_user(database_t* db, const std::string& username, const std::string& userpass)
{
database_user_t* nuser = (database_user_t*) malloc(sizeof(database_user_t));
nuser->m_name = netbuf_new(username.c_str(), username.length());
std::string key; std::string iv;
Encryption::user_create_keypass(key, iv, userpass.c_str(), userpass.size());
nuser->m_key = netbuf_new(key.c_str(), key.length());
nuser->m_iv = netbuf_new(iv.c_str(), iv.length());
nuser->status = 0.0f;
db->data.push_back(nuser);
return nuser;
}
static void handle_dhcp_discover(struct dhcp_msg *dhcpmsg)
{
if(dhcpmsg->htype != DHCP_HTYPE_ETH)
return;
if(dhcpmsg->hlen > NETIF_MAX_HWADDR_LEN)
return;
dhcp_lease_t *freelease = find_lease_slot(dhcpmsg->chaddr);
if(!freelease) {
printf("DHCP Server: All leases taken.\r\n");
return; /* Nothing available, so do nothing */
}
/* Reuse the DISCOVER buffer for the OFFER response */
dhcpmsg->op = DHCP_BOOTREPLY;
bzero(dhcpmsg->options, DHCP_OPTIONS_LEN);
ip_addr_copy(dhcpmsg->yiaddr, state->first_client_addr);
ip4_addr4(&(dhcpmsg->yiaddr)) += (freelease - state->leases);
uint8_t *opt = (uint8_t *)&dhcpmsg->options;
opt = add_dhcp_option_byte(opt, DHCP_OPTION_MESSAGE_TYPE, DHCP_OFFER);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_SERVER_ID, &state->server_if->ip_addr, 4);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_SUBNET_MASK, &state->server_if->netmask, 4);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_END, NULL, 0);
struct netbuf *netbuf = netbuf_new();
netbuf_alloc(netbuf, sizeof(struct dhcp_msg));
netbuf_take(netbuf, dhcpmsg, sizeof(struct dhcp_msg));
netconn_sendto(state->nc, netbuf, IP_ADDR_BROADCAST, 68);
netbuf_delete(netbuf);
}
void TerminalTask(void* params)
{
terminalQueue = xQueueCreate(TERMINAL_QUEUE_LEN, MAX_CHARS);
char msg[MAX_CHARS];
struct netconn* conn = netconn_new( NETCONN_UDP );
netconn_bind(conn, IP_ADDR_ANY, LOG_LOCAL_PORT);
struct ip_addr ip;
ip.addr = inet_addr("10.3.84.100");
netconn_connect(conn, &ip, LOG_REMOTE_PORT);
for(;;)
{
// Try to receive message, block the task for at most TERMINAL_QUEUE_TIMEOUT ticks if queue is empty.
if (xQueueReceive(terminalQueue, &msg, TERMINAL_QUEUE_TIMEOUT))
//if (xQueueReceive(LogQueueHandle, &msg, 100))
{
// Methods for UDP send.
struct netbuf *buf = netbuf_new();
char * data = netbuf_alloc(buf, sizeof(msg)); // Also deallocated with netbuf_delete(buf)
memcpy (data, msg, sizeof (msg));
netconn_send(conn, buf);
netbuf_delete(buf); // Deallocate packet buffer
}
}
}
nsapi_size_or_error_t LWIP::socket_sendto(nsapi_socket_t handle, const SocketAddress &address, const void *data, nsapi_size_t size)
{
struct mbed_lwip_socket *s = (struct mbed_lwip_socket *)handle;
ip_addr_t ip_addr;
nsapi_addr_t addr = address.get_addr();
if (!convert_mbed_addr_to_lwip(&ip_addr, &addr)) {
return NSAPI_ERROR_PARAMETER;
}
struct netbuf *buf = netbuf_new();
err_t err = netbuf_ref(buf, data, (u16_t)size);
if (err != ERR_OK) {
netbuf_free(buf);
return err_remap(err);
}
err = netconn_sendto(s->conn, buf, &ip_addr, address.get_port());
netbuf_delete(buf);
if (err != ERR_OK) {
return err_remap(err);
}
return size;
}
void serial_init(int baudrate)
{
lwip_init();
ser_conn = netconn_new(NETCONN_UDP);
netconn_bind(ser_conn, IP_ADDR_ANY, 1441);
buf = netbuf_new();
putc_pos = oob_pos = 0;
}
gerror_t database_create(database_t*& to, const std::string& dbname, const std::string& dbpass)
{
to = (database_t*) malloc(sizeof(database_t));
to->m_name = netbuf_new(dbname.c_str(), dbname.length());
Encryption::user_create_keypass(to->key, to->iv, dbpass.c_str(), dbpass.length());
return GERROR_NONE;
}
gerror_t database_create2(database_t*& to, const std::string& key, const std::string& iv)
{
to = new database_t;
to->m_name = netbuf_new(0);
to->key = key;
to->iv = iv;
return GERROR_NONE;
}
int
lwip_send(int s, void *data, int size, unsigned int flags)
{
struct lwip_socket *sock;
struct netbuf *buf;
err_t err;
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_send(%d, data=%p, size=%d, flags=0x%x)\n", s, data, size, flags));
sock = get_socket(s);
if (!sock) {
set_errno(EBADF);
return -1;
}
switch (netconn_type(sock->conn)) {
case NETCONN_RAW:
case NETCONN_UDP:
case NETCONN_UDPLITE:
case NETCONN_UDPNOCHKSUM:
/* create a buffer */
buf = netbuf_new();
if (!buf) {
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_send(%d) ENOBUFS\n", s));
sock_set_errno(sock, ENOBUFS);
return -1;
}
/* make the buffer point to the data that should
be sent */
netbuf_ref(buf, data, size);
/* send the data */
err = netconn_send(sock->conn, buf);
/* deallocated the buffer */
netbuf_delete(buf);
break;
case NETCONN_TCP:
err = netconn_write(sock->conn, data, size, NETCONN_COPY);
break;
default:
err = ERR_ARG;
break;
}
if (err != ERR_OK) {
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_send(%d) err=%d\n", s, err));
sock_set_errno(sock, err_to_errno(err));
return -1;
}
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_send(%d) ok size=%d\n", s, size));
sock_set_errno(sock, 0);
return size;
}
msg_t data_udp_send_thread(void *p) {
void * arg __attribute__ ((unused)) = p;
err_t err;
uint8_t count = 0;
struct netconn *conn;
struct netbuf *buf;
char* data;
char msg[DATA_UDP_MSG_SIZE] ;
ip_addr_t ip_addr_sensor;
ip_addr_t ip_addr_fc;
IMU_A_IP_ADDR(&ip_addr_sensor);
IP_PSAS_FC(&ip_addr_fc);
chRegSetThreadName("data_udp_send_thread");
conn = netconn_new( NETCONN_UDP );
/* Bind to the local address, or to ANY address */
// netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY)
err = netconn_bind(conn, &ip_addr_sensor, IMU_A_TX_PORT ); //local port
if (err == ERR_OK) {
/* Connect to specific address or a broadcast address */
/*
* \todo Understand why a UDP needs a connect...
* This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally.
*
*/
// netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT );
err = netconn_connect(conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A );
if(err == ERR_OK) {
for( ;; ){
buf = netbuf_new();
data = netbuf_alloc(buf, sizeof(msg));
sprintf(msg, "sensor tx: %d", count++);
memcpy (data, msg, sizeof (msg));
netconn_send(conn, buf);
netbuf_delete(buf); // De-allocate packet buffer
chThdSleepMilliseconds(500);
}
return RDY_OK;
} else {
return RDY_RESET;
}
} else {
return RDY_RESET;
}
}
void ICACHE_FLASH_ATTR
_setBulbState(int idx, uint16_t state) {
struct netconn * sendconn;
struct netbuf * sendbuf;
char * data;
err_t ret;
if (!network_ready) return;
sendconn = netconn_new( NETCONN_UDP );
if (sendconn == NULL) {
os_printf("[setbulb] Couldn't get netconn\r\n");
return;
}
sendbuf = netbuf_new();
if (sendbuf == NULL) {
os_printf("[setbulb] couldn't get sendbuff\r\n");
return;
}
data = netbuf_alloc(sendbuf, 38);
if (data == NULL) {
os_printf("[setbulb] couldn't alloc data\r\n");
return;
}
getPktSetBulbPower(data, 38, idx, state);
ret = netconn_bind(sendconn, &myaddr, 34435);
if (ret != ERR_OK) {
os_printf("[setbulb] connect error: %d\r\n", ret);
return;
}
netconn_set_recvtimeout( sendconn, 1000);
ret = netconn_connect(sendconn, getBulbAddr(idx), 56700);
if (ret != ERR_OK) {
os_printf("[setbulb] connect error: %d\r\n", ret);
return;
}
ret = netconn_send(sendconn, sendbuf);
if (ret != ERR_OK) {
os_printf("[setbulb] send error: %d\r\n", ret);
} else {
}
netbuf_free(sendbuf);
netbuf_delete(sendbuf);
netconn_disconnect(sendconn);
netconn_delete(sendconn);
}
static void send_dhcp_nak(struct dhcp_msg *dhcpmsg)
{
/* Reuse 'dhcpmsg' for the NAK */
dhcpmsg->op = DHCP_BOOTREPLY;
bzero(dhcpmsg->options, DHCP_OPTIONS_LEN);
uint8_t *opt = (uint8_t *)&dhcpmsg->options;
opt = add_dhcp_option_byte(opt, DHCP_OPTION_MESSAGE_TYPE, DHCP_NAK);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_SERVER_ID, &state->server_if->ip_addr, 4);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_END, NULL, 0);
struct netbuf *netbuf = netbuf_new();
netbuf_alloc(netbuf, sizeof(struct dhcp_msg));
netbuf_take(netbuf, dhcpmsg, sizeof(struct dhcp_msg));
netconn_sendto(state->nc, netbuf, IP_ADDR_BROADCAST, 68);
netbuf_delete(netbuf);
}
void uart_wifi_debug_init()
{
uart_wifi_debug_buf=netbuf_new();
// int ret;
if(NULL == (uart_wifi_debug_base_conn = netconn_new(NETCONN_UDP)))
{
LOG_WARN( "conn new err...\r\n");
// ret = STATE_ERROR;
}
if(netconn_bind(uart_wifi_debug_base_conn, IP_ADDR_ANY, 1112) != ERR_OK)
{
LOG_WARN( "bind err...\r\n");
}
}
/*! \brief event handler for mpu9150 udp data
* send one packet of mpu9150 data on event.
*/
static void data_udp_send_mpu9150_data(eventid_t id) {
(void) id;
uint8_t* data;
BaseSequentialStream *chp = getActiveUsbSerialStream();
mpu9150_mac_info.buf = netbuf_new();
chprintf(chp, "ACCL: x: %d\ty: %d\tz: %d\r\n", mpu9150_current_read.accel_xyz.x, mpu9150_current_read.accel_xyz.y, mpu9150_current_read.accel_xyz.z);
data = netbuf_alloc(mpu9150_mac_info.buf, sizeof(MPU9150_read_data));
if(data != NULL) {
memcpy (data, (void*) &mpu9150_current_read, sizeof(MPU9150_read_data));
chprintf(chp, "size: %d\r\n", sizeof(MPU9150_read_data));
palSetPad(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN);
netconn_send(mpu9150_mac_info.conn, mpu9150_mac_info.buf);
palClearPad(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN);
netbuf_delete(mpu9150_mac_info.buf);
}
}
/*! \brief event handler for mpu9150 udp data
* send one packet of mpu9150 data on event.
*/
static void data_udp_send_mpl3115a2_data(eventid_t id) {
(void) id;
uint8_t* data;
MPL_packet packet;
const char myid[(sizeof("MPL3")-1)] = "MPL3";
memset (&packet.timestamp, 0, sizeof(packet.timestamp));
strncpy(packet.ID, myid, sizeof(myid));
memcpy(&packet.data, (void*) &mpl3115a2_current_read, sizeof(MPL3115A2_read_data) );
packet.data_length = (uint16_t) sizeof(MPL3115A2_read_data);
mpl3115a2_mac_info.buf = netbuf_new();
data = netbuf_alloc(mpl3115a2_mac_info.buf, sizeof(packet));
if(data != NULL) {
memcpy (data, (void*) &packet, sizeof(packet));
netconn_send(mpl3115a2_mac_info.conn, mpl3115a2_mac_info.buf);
netbuf_delete(mpl3115a2_mac_info.buf);
}
}
static int lwip_socket_sendto(nsapi_stack_t *stack, nsapi_socket_t handle, nsapi_addr_t addr, uint16_t port, const void *data, unsigned size)
{
struct lwip_socket *s = (struct lwip_socket *)handle;
if (addr.version != NSAPI_IPv4) {
return NSAPI_ERROR_PARAMETER;
}
struct netbuf *buf = netbuf_new();
err_t err = netbuf_ref(buf, data, (u16_t)size);
if (err != ERR_OK) {
netbuf_free(buf);
return lwip_err_remap(err);;
}
err = netconn_sendto(s->conn, buf, (ip_addr_t *)addr.bytes, port);
netbuf_delete(buf);
if (err != ERR_OK) {
return lwip_err_remap(err);
}
return size;
}
/*! \brief event handler for adis16405 udp data
* send one packet of adis16405 data on event.
*/
static void data_udp_send_adis16405_data(eventid_t id) {
(void) id;
ADIS_packet packet;
const char myid[(sizeof("ADIS")-1)] = "ADIS";
uint8_t* data;
memset (&packet.timestamp, 0, sizeof(packet.timestamp));
strncpy(packet.ID, myid, sizeof(myid));
memcpy(&packet.data, (void*) &adis16405_burst_data, sizeof(ADIS16405_burst_data) );
packet.data_length = (uint16_t) sizeof(ADIS16405_burst_data);
adis16405_mac_info.buf = netbuf_new();
data = netbuf_alloc(adis16405_mac_info.buf, sizeof(ADIS_packet));
if(data != NULL) {
memcpy (data, (void*) &packet, sizeof(ADIS_packet));
netconn_send(adis16405_mac_info.conn, adis16405_mac_info.buf);
netbuf_delete(adis16405_mac_info.buf);
}
}
static nsapi_size_or_error_t mbed_lwip_socket_sendto(nsapi_stack_t *stack, nsapi_socket_t handle, nsapi_addr_t addr, uint16_t port, const void *data, nsapi_size_t size)
{
struct lwip_socket *s = (struct lwip_socket *)handle;
ip_addr_t ip_addr;
if (!convert_mbed_addr_to_lwip(&ip_addr, &addr)) {
return NSAPI_ERROR_PARAMETER;
}
struct netbuf *buf = netbuf_new();
err_t err = netbuf_ref(buf, data, (u16_t)size);
if (err != ERR_OK) {
netbuf_free(buf);
return mbed_lwip_err_remap(err);
}
err = netconn_sendto(s->conn, buf, &ip_addr, port);
netbuf_delete(buf);
if (err != ERR_OK) {
return mbed_lwip_err_remap(err);
}
return size;
}
msg_t data_udp_send_thread(void *p) {
void * arg __attribute__ ((unused)) = p;
BaseSequentialStream *chp = (BaseSequentialStream *)&SD1;
err_t err;
uint8_t count = 0;
struct netconn *conn;
struct netbuf *buf;
char* data;
char msg[DATA_UDP_MSG_SIZE] ;
ip_addr_t ip_addr_rnet;
ip_addr_t ip_addr_fc;
ip_addr_t my_ip;
uint16_t my_port;
RNET_A_IP_ADDR(&ip_addr_rnet);
IP_PSAS_FC(&ip_addr_fc);
chRegSetThreadName("data_udp_send_thread");
conn = netconn_new( NETCONN_UDP );
chThdSleepMilliseconds(1000);
chprintf(chp, "Start udp send thread\n\r");
/* Bind to the local address, or to ANY address */
// netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY)
err = netconn_bind(conn, &ip_addr_rnet, RNET_A_TX_PORT ); //local port
netconn_getaddr(conn, &my_ip, &my_port, local_ip);
print_ip(my_ip, my_port);
if (err == ERR_OK) {
/* Connect to specific address or a broadcast address */
/*
* \todo Understand why a UDP needs a connect...
* This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally.
*
*/
// netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT );
// err = netconn_connect(conn, &ip_addr_fc, FC_LISTEN_PORT_RNET_A );
err = netconn_connect(conn, IP_ADDR_BROADCAST, FC_LISTEN_PORT_RNET_A );
if(err == ERR_OK) {
for( ;; ){
buf = netbuf_new();
data = netbuf_alloc(buf, sizeof(msg));
sprintf(msg, "rnet tx: %d", count++);
memcpy (data, msg, sizeof (msg));
err = netconn_send(conn, buf);
// chprintf(chp, "rnet sent: index: %d. Error: %d\r\n", count, err);
netbuf_delete(buf); // De-allocate packet buffer
chThdSleepMilliseconds(500);
}
return RDY_OK;
} else {
return RDY_RESET;
}
} else {
return RDY_RESET;
}
}
static void LwipInitTask(void* pvArguments) {
err_t err;
struct netif fsl_netif0;
ip_addr_t fsl_netif0_ipaddr, fsl_netif0_netmask, fsl_netif0_gw;
char msg[] = "This is my message";
(void)pvArguments;
// Init lwip stack
tcpip_init(NULL,NULL);
printf("%s: lwip init called ..\n", __FUNCTION__);
// Setup IP Config for DHCP ...
IP4_ADDR(&fsl_netif0_ipaddr, 0,0,0,0);
IP4_ADDR(&fsl_netif0_netmask, 0,0,0,0);
IP4_ADDR(&fsl_netif0_gw, 0,0,0,0);
/* Add a network interface to the list of lwIP netifs. */
netif_add(&fsl_netif0, &fsl_netif0_ipaddr, &fsl_netif0_netmask, &fsl_netif0_gw, NULL, ethernetif_init, ethernet_input);
/* Set the network interface as the default network interface. */
netif_set_default(&fsl_netif0);
/* obtain the IP address, default gateway and subnet mask by using DHCP*/
err = dhcp_start(&fsl_netif0);
printf("%s : Started DCHP request (%s)\n", __FUNCTION__, lwip_strerr(err));
for(int i=0; i < DHCP_TIMEOUT && fsl_netif0.dhcp->state != DHCP_BOUND; i++) {
printf("%s : Current DHCP State : %d\n", __FUNCTION__, fsl_netif0.dhcp->state);
// Wait a second
vTaskDelay(1000/portTICK_PERIOD_MS);
}
// Make it active ...
netif_set_up(&fsl_netif0);
printf("%s : Interface is up : %d\n", __FUNCTION__, fsl_netif0.dhcp->state);
printf("%s : IP %s\n", __FUNCTION__, ipaddr_ntoa(&fsl_netif0.ip_addr));
printf("%s : NM %s\n", __FUNCTION__, ipaddr_ntoa(&fsl_netif0.netmask));
printf("%s : GW %s\n", __FUNCTION__, ipaddr_ntoa(&fsl_netif0.gw));
if (fsl_netif0.dhcp->state == DHCP_BOUND) {
// Send out some UDP data
struct netconn* pConnection;
// Create UDP connection
pConnection = netconn_new(NETCONN_UDP);
// Connect to local port
err = netconn_bind(pConnection, IP_ADDR_ANY, 12345);
printf("%s : Bound to IP_ADDR_ANY port 12345 (%s)\n", __FUNCTION__, lwip_strerr(err));
err = netconn_connect(pConnection, IP_ADDR_BROADCAST, 12346 );
printf("%s : Connected to IP_ADDR_BROADCAST port 12346 (%s)\n", __FUNCTION__, lwip_strerr(err));
for(int i = 0; i < 10; i++ ){
struct netbuf* buf = netbuf_new();
void* data = netbuf_alloc(buf, sizeof(msg));
memcpy (data, msg, sizeof (msg));
err = netconn_send(pConnection, buf);
printf("%s : Sending to IP_ADDR_BROADCAST port 12346 (%s)\n", __FUNCTION__, lwip_strerr(err));
netbuf_delete(buf); // De-allocate packet buffer
// Wait a second
vTaskDelay(1000/portTICK_PERIOD_MS);
}
err = netconn_disconnect(pConnection);
printf("%s : Disconnected from IP_ADDR_BROADCAST port 12346 (%s)\n", __FUNCTION__, lwip_strerr(err));
err = netconn_delete(pConnection);
printf("%s : Deleted connection (%s)\n", __FUNCTION__, lwip_strerr(err));
}
// Wait a second
vTaskDelay(1000/portTICK_PERIOD_MS);
/* finish the lease of the IP address */
err = dhcp_release(&fsl_netif0);
printf("%s : DHCP Release (%s)\n", __FUNCTION__, lwip_strerr(err));
for(;;) {};
}
void broadcast_temperature(void *pvParameters)
{
uint8_t amount = 0;
uint8_t sensors = 1;
ds18b20_addr_t addrs[sensors];
float results[sensors];
// Use GPIO 13 as one wire pin.
uint8_t GPIO_FOR_ONE_WIRE = 13;
char msg[100];
// Broadcaster part
err_t err;
while(1) {
// Send out some UDP data
struct netconn* conn;
// Create UDP connection
conn = netconn_new(NETCONN_UDP);
// Connect to local port
err = netconn_bind(conn, IP_ADDR_ANY, 8004);
if (err != ERR_OK) {
netconn_delete(conn);
printf("%s : Could not bind! (%s)\n", __FUNCTION__, lwip_strerr(err));
continue;
}
err = netconn_connect(conn, IP_ADDR_BROADCAST, 8005);
if (err != ERR_OK) {
netconn_delete(conn);
printf("%s : Could not connect! (%s)\n", __FUNCTION__, lwip_strerr(err));
continue;
}
for(;;) {
// Search all DS18B20, return its amount and feed 't' structure with result data.
amount = ds18b20_scan_devices(GPIO_FOR_ONE_WIRE, addrs, sensors);
if (amount < sensors){
printf("Something is wrong, I expect to see %d sensors \nbut just %d was detected!\n", sensors, amount);
}
ds18b20_measure_and_read_multi(GPIO_FOR_ONE_WIRE, addrs, sensors, results);
for (int i = 0; i < sensors; ++i)
{
// ("\xC2\xB0" is the degree character (U+00B0) in UTF-8)
sprintf(msg, "Sensor %08x%08x reports: %f \xC2\xB0""C\n", (uint32_t)(addrs[i] >> 32), (uint32_t)addrs[i], results[i]);
printf("%s", msg);
struct netbuf* buf = netbuf_new();
void* data = netbuf_alloc(buf, strlen(msg));
memcpy (data, msg, strlen(msg));
err = netconn_send(conn, buf);
if (err != ERR_OK) {
printf("%s : Could not send data!!! (%s)\n", __FUNCTION__, lwip_strerr(err));
continue;
}
netbuf_delete(buf); // De-allocate packet buffer
}
vTaskDelay(1000/portTICK_PERIOD_MS);
}
err = netconn_disconnect(conn);
printf("%s : Disconnected from IP_ADDR_BROADCAST port 12346 (%s)\n", __FUNCTION__, lwip_strerr(err));
err = netconn_delete(conn);
printf("%s : Deleted connection (%s)\n", __FUNCTION__, lwip_strerr(err));
vTaskDelay(1000/portTICK_PERIOD_MS);
}
}
/**
* SNTP request
*/
time_t sntp_request(void)
{
unsigned char * sntp_request;
unsigned char * sntp_response;
struct ip_addr sntp_server_address;
time_t timestamp = 0;
struct netconn * sendUDPNetConn;
struct netbuf * sendUDPNetBuf;
struct netbuf * receiveUDPNetBuf;
u16_t dataLen;
err_t errLWIP;
/* initialize SNTP server address */
sntp_server_address.addr = SNTP_SERVER_ADDRESS;
/* if we got a valid SNTP server address... */
if (sntp_server_address.addr != 0)
{
/* create new socket */
sendUDPNetConn = netconn_new( NETCONN_UDP );
sendUDPNetBuf = netbuf_new();
// Create data space for netbuf, if we can.
sntp_request = (unsigned char *) netbuf_alloc(sendUDPNetBuf, SNTP_MAX_DATA_LEN);
if ((NULL != sendUDPNetConn) && (NULL != sendUDPNetBuf) && (NULL != sntp_request))
{
errLWIP = netconn_connect(sendUDPNetConn, &sntp_server_address, SNTP_PORT);
if(ERR_OK == errLWIP)
{
/* prepare SNTP request */
memset(sntp_request, 0, SNTP_MAX_DATA_LEN);
sntp_request[0] = SNTP_LI_NO_WARNING | SNTP_VERSION | SNTP_MODE_CLIENT;
errLWIP = netconn_send(sendUDPNetConn, sendUDPNetBuf);
// Send SNTP request to server.
if (ERR_OK == errLWIP)
{
// Set recv timeout.
sendUDPNetConn->recv_timeout = SNTP_RECV_TIMEOUT;
// Receive SNTP server response.
receiveUDPNetBuf = netconn_recv(sendUDPNetConn);
if (NULL != receiveUDPNetBuf)
{
// Get pointer to response data.
netbuf_data(receiveUDPNetBuf, (void **) &sntp_response,(u16_t *) &dataLen);
// If the response size is good.
if (dataLen == SNTP_MAX_DATA_LEN)
{
// If this is a SNTP response...
if (((sntp_response[0] & SNTP_MODE_MASK) == SNTP_MODE_SERVER) || ((sntp_response[0]
& SNTP_MODE_MASK) == SNTP_MODE_BROADCAST))
{
/* extract GMT time from response */
memcpy(×tamp, (sntp_response + SNTP_RCV_TIME_OFS), sizeof(timestamp));
timestamp = (ntohl(timestamp) - DIFF_SEC_1900_1970);
syslog(LOG_DEBUG | LOG_USER, "Received timestamp %u", timestamp);
}
else
{
syslog(LOG_INFO | LOG_USER, "Received data did not match frame code");
}
}
else
{
syslog(LOG_NOTICE | LOG_USER, "Length of data did not match SNTP_MAX_DATA_LEN, received len %u", dataLen);
}
}
else
{
syslog(LOG_WARNING | LOG_USER, "Netconn receive failed with %d", netconn_err(sendUDPNetConn));
}
} // netconn_send(sendUDPNetConn, sendUDPNetBuf);
else
{
syslog(LOG_WARNING | LOG_USER, "Netconn sendto failed with %d", errLWIP);
}
} //netconn_connect(sendUDPNetConn, &sntp_server_address, SNTP_PORT);
else
{
syslog(LOG_WARNING | LOG_USER, "Netconn connect to server %X, port %u failed with %d", sntp_server_address.addr, SNTP_PORT, errLWIP);
}
} // if ((NULL != sendUDPNetConn) && (NULL != sendUDPNetBuf) && (NULL != sntp_request))
else
{
syslog(LOG_ERR | LOG_USER, "Netconn or netbuf or data allocation failed.");
}
} //if (sntp_server_address != 0)
else
{
syslog(LOG_WARNING | LOG_USER, "Invalid NTP server address %X", SNTP_SERVER_ADDRESS);
}
netbuf_delete(sendUDPNetBuf);
netbuf_delete(receiveUDPNetBuf);
netconn_delete(sendUDPNetConn);
return timestamp;
}
void broadcast_temperature(void *pvParameters)
{
uint8_t amount = 0;
uint8_t sensors = 2;
ds_sensor_t t[sensors];
// Use GPIO 13 as one wire pin.
uint8_t GPIO_FOR_ONE_WIRE = 13;
char msg[100];
// Broadcaster part
err_t err;
// Initialize one wire bus.
onewire_init(GPIO_FOR_ONE_WIRE);
while(1) {
// Send out some UDP data
struct netconn* conn;
// Create UDP connection
conn = netconn_new(NETCONN_UDP);
// Connect to local port
err = netconn_bind(conn, IP_ADDR_ANY, 8004);
if (err != ERR_OK) {
netconn_delete(conn);
printf("%s : Could not bind! (%s)\n", __FUNCTION__, lwip_strerr(err));
continue;
}
err = netconn_connect(conn, IP_ADDR_BROADCAST, 8005);
if (err != ERR_OK) {
netconn_delete(conn);
printf("%s : Could not connect! (%s)\n", __FUNCTION__, lwip_strerr(err));
continue;
}
for(;;) {
// Search all DS18B20, return its amount and feed 't' structure with result data.
amount = ds18b20_read_all(GPIO_FOR_ONE_WIRE, t);
if (amount < sensors){
printf("Something is wrong, I expect to see %d sensors \nbut just %d was detected!\n", sensors, amount);
}
for (int i = 0; i < amount; ++i)
{
int intpart = (int)t[i].value;
int fraction = (int)((t[i].value - intpart) * 100);
// Multiple "" here is just to satisfy compiler and don`t raise 'hex escape sequence out of range' warning.
sprintf(msg, "Sensor %d report: %d.%02d ""\xC2""\xB0""C\n",t[i].id, intpart, fraction);
printf("%s", msg);
struct netbuf* buf = netbuf_new();
void* data = netbuf_alloc(buf, strlen(msg));
memcpy (data, msg, strlen(msg));
err = netconn_send(conn, buf);
if (err != ERR_OK) {
printf("%s : Could not send data!!! (%s)\n", __FUNCTION__, lwip_strerr(err));
continue;
}
netbuf_delete(buf); // De-allocate packet buffer
}
vTaskDelay(1000/portTICK_RATE_MS);
}
err = netconn_disconnect(conn);
printf("%s : Disconnected from IP_ADDR_BROADCAST port 12346 (%s)\n", __FUNCTION__, lwip_strerr(err));
err = netconn_delete(conn);
printf("%s : Deleted connection (%s)\n", __FUNCTION__, lwip_strerr(err));
vTaskDelay(1000/portTICK_RATE_MS);
}
}
void XMLRPCServer::UDPSend(void* params)
{
UDPHandler* uh = UDPHandler::instance();
struct netconn* conn = netconn_new( NETCONN_UDP );
netconn_bind(conn, IP_ADDR_ANY, UDP_LOCAL_PORT);
static uint8_t counter = 1;
pinMode(GPIO_PD11, OUTPUT);
UDPMessage msg;
struct ip_addr ip;
ip.addr = inet_addr(ROS_MASTER_IP);
for (;;) {
//digitalWrite(GPIO_PD11, HIGH);
digitalWrite(GPIO_PD11, pin3);
uh->dequeueMessage(&msg);
//digitalWrite(GPIO_PD11, LOW);
// inter-node communication
TopicReader* tr = getTopicReader(msg.topic);
if (tr != NULL) {
tr->enqueueMessage(msg.data);
}
TopicWriter* tw = getTopicWriter(msg.topic);
if (tw != NULL) {
UDPConnection* const* connections = tw->getConnections();
if (connections != NULL) {
for (int i = 0; i < MAX_UDP_CONNECTIONS; i++) {
const UDPConnection* connection = connections[i];
if (connection && connection->isValid()) {
err_t err = netconn_connect(conn, &ip, connection->getPort());
//os_printf("1Port: %d LWIP Error:%d\n", endpoint.port, err);
os_printf("Connecting %s:%d, err:%d\n", ip, connection->getPort(), err);
struct netbuf *buf = netbuf_new();
char msgHeader[8];
uint32_t connectionID = connection->getID();
memcpy(&msgHeader[0], &connectionID, sizeof(uint32_t));
msgHeader[4] = 0;
msgHeader[5] = counter++;
msgHeader[6] = 0x01;
msgHeader[7] = 0;
uint32_t msgLen = *((uint32_t*) msg.data) + 4;
void* data = netbuf_alloc(buf, msgLen + sizeof(msgHeader)); // Also deallocated with netbuf_delete(buf)
if (data != NULL) {
memcpy (data, msgHeader, sizeof(msgHeader));
memcpy (data + sizeof(msgHeader), msg.data, msgLen);
} else {
os_printf("XMLRPCServer::UDPSend data is NULL!\n");
}
err = netconn_send(conn, buf);
//os_printf("2Port: %d LWIP Error:%d\n", endpoint.port, err);
netbuf_delete(buf);
}
}
}
}
pin3 = !pin3;
}
}
static void handle_dhcp_request(struct dhcp_msg *dhcpmsg)
{
static char ipbuf[16];
if(dhcpmsg->htype != DHCP_HTYPE_ETH)
return;
if(dhcpmsg->hlen > NETIF_MAX_HWADDR_LEN)
return;
ip_addr_t requested_ip;
uint8_t *requested_ip_opt = find_dhcp_option(dhcpmsg, DHCP_OPTION_REQUESTED_IP, 4, NULL);
if(requested_ip_opt) {
memcpy(&requested_ip.addr, requested_ip_opt, 4);
} else if(ip_addr_cmp(&requested_ip, IP_ADDR_ANY)) {
ip_addr_copy(requested_ip, dhcpmsg->ciaddr);
} else {
printf("DHCP Server Error: No requested IP\r\n");
send_dhcp_nak(dhcpmsg);
return;
}
/* Test the first 4 octets match */
if(ip4_addr1(&requested_ip) != ip4_addr1(&state->first_client_addr)
|| ip4_addr2(&requested_ip) != ip4_addr2(&state->first_client_addr)
|| ip4_addr3(&requested_ip) != ip4_addr3(&state->first_client_addr)) {
sprintf_ipaddr(&requested_ip, ipbuf);
printf("DHCP Server Error: %s not an allowed IP\r\n", ipbuf);
send_dhcp_nak(dhcpmsg);
return;
}
/* Test the last octet is in the MAXCLIENTS range */
int16_t octet_offs = ip4_addr4(&requested_ip) - ip4_addr4(&state->first_client_addr);
if(octet_offs < 0 || octet_offs >= state->max_leases) {
printf("DHCP Server Error: Address out of range\r\n");
send_dhcp_nak(dhcpmsg);
return;
}
dhcp_lease_t *requested_lease = state->leases + octet_offs;
if(requested_lease->expires != 0 && memcmp(requested_lease->hwaddr, dhcpmsg->chaddr,dhcpmsg->hlen))
{
printf("DHCP Server Error: Lease for address already taken\r\n");
send_dhcp_nak(dhcpmsg);
return;
}
memcpy(requested_lease->hwaddr, dhcpmsg->chaddr, dhcpmsg->hlen);
sprintf_ipaddr(&requested_ip, ipbuf);
printf("DHCP lease addr %s assigned to MAC %02x:%02x:%02x:%02x:%02x:%02x\r\n", ipbuf, requested_lease->hwaddr[0],
requested_lease->hwaddr[1], requested_lease->hwaddr[2], requested_lease->hwaddr[3], requested_lease->hwaddr[4],
requested_lease->hwaddr[5]);
requested_lease->expires = DHCPSERVER_LEASE_TIME * configTICK_RATE_HZ;
/* Reuse the REQUEST message as the ACK message */
dhcpmsg->op = DHCP_BOOTREPLY;
bzero(dhcpmsg->options, DHCP_OPTIONS_LEN);
ip_addr_copy(dhcpmsg->yiaddr, requested_ip);
uint8_t *opt = (uint8_t *)&dhcpmsg->options;
opt = add_dhcp_option_byte(opt, DHCP_OPTION_MESSAGE_TYPE, DHCP_ACK);
uint32_t expiry = htonl(DHCPSERVER_LEASE_TIME);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_LEASE_TIME, &expiry, 4);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_SERVER_ID, &state->server_if->ip_addr, 4);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_SUBNET_MASK, &state->server_if->netmask, 4);
opt = add_dhcp_option_bytes(opt, DHCP_OPTION_END, NULL, 0);
struct netbuf *netbuf = netbuf_new();
netbuf_alloc(netbuf, sizeof(struct dhcp_msg));
netbuf_take(netbuf, dhcpmsg, sizeof(struct dhcp_msg));
netconn_sendto(state->nc, netbuf, IP_ADDR_BROADCAST, 68);
netbuf_delete(netbuf);
}
