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
}
}
}
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);
}
/*! \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);
}
}
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;
}
}
/*
* ip_arp_recv_request()
*/
static void ip_arp_recv_request(struct ip_datalink_instance *idi, struct netbuf *nb)
{
u8_t *send_packet;
struct eth_phys_header *reph;
struct arp_header *rah, *sah;
struct ip_arp_header *rp, *sp;
struct netbuf *nbrep;
struct ethernet_ip_instance *eii;
struct ethernet_server *es;
eii = (struct ethernet_ip_instance *)idi;
reph = (struct eth_phys_header *)nb->nb_datalink;
rah = (struct arp_header *)nb->nb_network;
rp = (struct ip_arp_header *)(rah + 1);
/*
* Issue an ARP reply.
*/
send_packet = membuf_alloc(sizeof(struct arp_header) + sizeof(struct ip_arp_header), NULL);
sah = (struct arp_header *)send_packet;
sp = (struct ip_arp_header *)(sah + 1);
memcpy(sp->iah_dest_mac, rp->iah_src_mac, 6);
spinlock_lock(&idi->idi_lock);
es = eii->eii_arp_server;
ethernet_server_ref(es);
spinlock_unlock(&idi->idi_lock);
memcpy(sp->iah_src_mac, es->es_get_mac(es), 6);
sah->ah_hardware = rah->ah_hardware;
sah->ah_protocol = rah->ah_protocol;
sah->ah_haddr_len = rah->ah_haddr_len;
sah->ah_paddr_len = rah->ah_paddr_len;
sah->ah_operation = hton16(2);
sp->iah_src_ip = hton32(idi->idi_client->idc_addr);
sp->iah_dest_ip = hton32(rp->iah_src_ip);
nbrep = netbuf_alloc();
nbrep->nb_network_membuf = sah;
nbrep->nb_network = sah;
nbrep->nb_network_size = sizeof(struct arp_header) + sizeof(struct ip_arp_header);
es->es_send(es, sp->iah_dest_mac, nbrep);
netbuf_deref(nbrep);
ethernet_server_deref(es);
}
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);
}
/*
* ip_arp_request()
* Issue an ARP request for a given IP address.
*/
u8_t ip_arp_request(struct ip_datalink_instance *idi, u32_t ip)
{
u8_t *pkt;
struct netbuf *nb;
struct arp_header *ah;
struct ip_arp_header *iah;
static u8_t broadcast_mac[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
struct ethernet_ip_instance *eii;
struct ethernet_server *es;
eii = (struct ethernet_ip_instance *)idi;
pkt = membuf_alloc(sizeof(struct arp_header) + sizeof(struct ip_arp_header), NULL);
ah = (struct arp_header *)pkt;
ah->ah_hardware = hton16(1);
ah->ah_protocol = hton16(0x0800);
ah->ah_haddr_len = 6;
ah->ah_paddr_len = 4;
ah->ah_operation = hton16(1);
iah = (struct ip_arp_header *)(ah + 1);
iah->iah_src_ip = hton32(idi->idi_client->idc_addr);
iah->iah_dest_ip = hton32(ip);
spinlock_lock(&idi->idi_lock);
es = eii->eii_arp_server;
ethernet_server_ref(es);
spinlock_unlock(&idi->idi_lock);
memcpy(iah->iah_src_mac, es->es_get_mac(es), 6);
memcpy(iah->iah_dest_mac, broadcast_mac, 6);
nb = netbuf_alloc();
nb->nb_network_membuf = ah;
nb->nb_network = ah;
nb->nb_network_size = sizeof(struct arp_header) + sizeof(struct ip_arp_header);
es->es_send(es, broadcast_mac, nb);
netbuf_deref(nb);
ethernet_server_deref(es);
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;
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);
}
}
/*! \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);
}
}
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;
}
}
/*
* i82595_recv_get_packet()
* Fetch the next packet out of the receive ring buffer.
*/
static struct netbuf *i82595_recv_get_packet(void)
{
u8_t d;
u8_t event;
u16_t status;
u8_t nextpl, nextph;
struct netbuf *nb = NULL;
/*
* Point at the next received packet slot.
*/
i82595_write8(I82595_PG0_HOST_ADDRL, next_rxl);
i82595_write8(I82595_PG0_HOST_ADDRH, next_rxh);
/*
* Get the receive event.
*/
event = i82595_read8(I82595_PG0_IOPORT);
if (event != 0x08) {
return NULL;
}
/*
* Read an empty slot.
*/
d = i82595_read8(I82595_PG0_IOPORT + 1);
/*
* Get the status word.
*/
status = i82595_read16(I82595_PG0_IOPORT);
/*
* Get the pointer to the next frame.
*/
nextpl = i82595_read8(I82595_PG0_IOPORT);
nextph = i82595_read8(I82595_PG0_IOPORT + 1);
if ((status & 0x2d81) == 0x2000) {
u8_t *pkt;
u16_t *buf;
u16_t words;
u16_t pktsz;
/*
* Get the frame size.
*/
pktsz = i82595_read16(I82595_PG0_IOPORT);
pkt = (u8_t *)membuf_alloc(pktsz, NULL);
buf = (u16_t *)pkt;
/*
* Read the packet.
*/
words = (pktsz + 1) >> 1;
while (words) {
*buf++ = i82595_read16(I82595_PG0_IOPORT);
words--;
}
nb = netbuf_alloc();
nb->nb_datalink_membuf = pkt;
nb->nb_datalink = pkt;
nb->nb_datalink_size = pktsz;
}
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);
}
}
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);
}
}
/**
* 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;
}
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 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);
}
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(;;) {};
}
/*
* udp_recv_netbuf()
*/
static void udp_recv_netbuf(void *clnt, struct netbuf *nb)
{
struct ip_client *ic;
struct udp_instance *ui;
struct udp_client *uc;
u16_t csum;
struct ip_header *iph = (struct ip_header *)nb->nb_network;
struct udp_header *udh = (struct udp_header *)nb->nb_transport;
ic = (struct ip_client *)clnt;
ui = (struct udp_instance *)ic->ic_instance;
/*
* When we get here our application data is still hooked under the
* transport layer, so the first thing to do is untangle it.
*/
nb->nb_application = udh + 1;
nb->nb_application_size = nb->nb_transport_size - sizeof(struct udp_header);
nb->nb_transport_size = sizeof(struct udp_header);
/*
* If we've been given a checksum then check it!
*/
if (udh->uh_csum != 0) {
csum = ipcsum_pseudo_partial(hton32(((struct ip_instance *)ic->ic_server->is_instance)->ii_addr),
iph->ih_src_addr, 0x11, udh->uh_len);
csum = ipcsum_partial(csum, nb->nb_transport, nb->nb_transport_size);
csum = ipcsum(csum, nb->nb_application, nb->nb_application_size);
if (csum != 0) {
debug_print_pstr("\fudp recv: csum fail: ");
debug_print16(csum);
return;
}
}
/*
* Look up the socket and see if we know anyone who will try and deal
* with it!
*/
spinlock_lock(&ui->ui_lock);
uc = ui->ui_client_list;
while (uc && (uc->uc_port != hton16(udh->uh_dest_port))) {
uc = uc->uc_next;
}
if (uc) {
udp_client_ref(uc);
spinlock_unlock(&ui->ui_lock);
uc->uc_recv(uc, nb);
udp_client_deref(uc);
} else {
struct ip_header *iphc;
struct netbuf *nbrep;
struct ip_server *is;
u32_t empty = 0;
is = ui->ui_server;
ip_server_ref(is);
spinlock_unlock(&ui->ui_lock);
/*
* Issue an ICMP destination unreachable message (port unreachable).
*/
nbrep = netbuf_alloc();
nbrep->nb_application_membuf = membuf_alloc(nb->nb_network_size + 8, NULL);
nbrep->nb_application = nbrep->nb_application_membuf;
nbrep->nb_application_size = nb->nb_network_size + 8;
iphc = (struct ip_header *)nbrep->nb_application;
memcpy(iphc, iph, nb->nb_network_size);
memcpy((iphc + 1), nb->nb_transport, 8);
is->is_send_icmp(is, iph->ih_src_addr, 0x03, 0x03, (u8_t *)(&empty), nbrep);
ip_server_deref(is);
netbuf_deref(nbrep);
}
}