/*
* sendto() a socket
*/
int
sosendto(struct socket *so, struct mbuf *m)
{
int ret;
SockAddress addr;
uint32_t addr_ip;
uint16_t addr_port;
DEBUG_CALL("sosendto");
DEBUG_ARG("so = %lx", (long)so);
DEBUG_ARG("m = %lx", (long)m);
if ((so->so_faddr_ip & 0xffffff00) == special_addr_ip) {
/* It's an alias */
int low = so->so_faddr_ip & 0xff;
if ( CTL_IS_DNS(low) )
addr_ip = dns_addr[low - CTL_DNS];
else
addr_ip = loopback_addr_ip;
} else
addr_ip = so->so_faddr_ip;
addr_port = so->so_faddr_port;
/*
* test for generic forwarding; this function replaces the arguments
* only on success
*/
unsigned long faddr = addr_ip;
int fport = addr_port;
if (slirp_should_net_forward(faddr, fport, &faddr, &fport)) {
time_t timestamp = time(NULL);
slirp_drop_log(
"Redirected UDP: src: 0x%08lx:0x%04x org dst: 0x%08lx:0x%04x "
"new dst: 0x%08lx:0x%04x %ld\n",
so->so_laddr_ip, so->so_laddr_port,
addr_ip, addr_port,
faddr, fport, timestamp
);
}
addr_ip = faddr;
addr_port = fport;
sock_address_init_inet(&addr, addr_ip, addr_port);
DEBUG_MISC((dfd, " sendto()ing, addr.sin_port=%d, addr.sin_addr.s_addr=%08x\n", addr_port, addr_ip));
/* Don't care what port we get */
ret = socket_sendto(so->s, m->m_data, m->m_len,&addr);
if (ret < 0)
return -1;
/*
* Kill the socket if there's no reply in 4 minutes,
* but only if it's an expirable socket
*/
if (so->so_expire)
so->so_expire = curtime + SO_EXPIRE;
so->so_state = SS_ISFCONNECTED; /* So that it gets select()ed */
return 0;
}
/*
* Send packet contained in buf of length len to peer at specified ip, and port.
* Use this function to transmit binary data that might contain 0x00 bytes.
* This function returns sent data size for success else -1.
*/
uint16_t udp_send_packet (udp_t *u, const uint8_t *buf, unsigned len,
uint8_t *ip, unsigned port)
{
return socket_sendto (u->sock, buf, len, ip, port);
}
void
icmp_input(struct mbuf *m, int hlen)
{
register struct icmp *icp;
register struct ip *ip=mtod(m, struct ip *);
int icmplen=ip->ip_len;
/* int code; */
DEBUG_CALL("icmp_input");
DEBUG_ARG("m = %lx", (long )m);
DEBUG_ARG("m_len = %d", m->m_len);
STAT(icmpstat.icps_received++);
/*
* Locate icmp structure in mbuf, and check
* that its not corrupted and of at least minimum length.
*/
if (icmplen < ICMP_MINLEN) { /* min 8 bytes payload */
STAT(icmpstat.icps_tooshort++);
freeit:
m_freem(m);
goto end_error;
}
m->m_len -= hlen;
m->m_data += hlen;
icp = mtod(m, struct icmp *);
if (cksum(m, icmplen)) {
STAT(icmpstat.icps_checksum++);
goto freeit;
}
m->m_len += hlen;
m->m_data -= hlen;
/* icmpstat.icps_inhist[icp->icmp_type]++; */
/* code = icp->icmp_code; */
DEBUG_ARG("icmp_type = %d", icp->icmp_type);
switch (icp->icmp_type) {
case ICMP_ECHO:
icp->icmp_type = ICMP_ECHOREPLY;
ip->ip_len += hlen; /* since ip_input subtracts this */
if (ip_geth(ip->ip_dst) == alias_addr_ip) {
icmp_reflect(m);
} else {
struct socket *so;
SockAddress addr;
uint32_t addr_ip;
uint16_t addr_port;
if ((so = socreate()) == NULL) goto freeit;
if(udp_attach(so) == -1) {
DEBUG_MISC((dfd,"icmp_input udp_attach errno = %d-%s\n",
errno,errno_str));
sofree(so);
m_free(m);
goto end_error;
}
so->so_m = m;
so->so_faddr_ip = ip_geth(ip->ip_dst);
so->so_faddr_port = 7;
so->so_laddr_ip = ip_geth(ip->ip_src);
so->so_laddr_port = 9;
so->so_iptos = ip->ip_tos;
so->so_type = IPPROTO_ICMP;
so->so_state = SS_ISFCONNECTED;
/* Send the packet */
if ((so->so_faddr_ip & 0xffffff00) == special_addr_ip) {
/* It's an alias */
int low = so->so_faddr_ip & 0xff;
if (low >= CTL_DNS && low < CTL_DNS + dns_addr_count)
addr_ip = dns_addr[low - CTL_DNS];
else
addr_ip = loopback_addr_ip;
} else {
addr_ip = so->so_faddr_ip;
}
addr_port = so->so_faddr_port;
sock_address_init_inet( &addr, addr_ip, addr_port );
if(socket_sendto(so->s, icmp_ping_msg, strlen(icmp_ping_msg), &addr) < 0) {
DEBUG_MISC((dfd,"icmp_input udp sendto tx errno = %d-%s\n",
errno,errno_str));
icmp_error(m, ICMP_UNREACH,ICMP_UNREACH_NET, 0,errno_str);
udp_detach(so);
}
} /* if ip->ip_dst.s_addr == alias_addr.s_addr */
break;
case ICMP_UNREACH:
/* XXX? report error? close socket? */
case ICMP_TIMXCEED:
case ICMP_PARAMPROB:
case ICMP_SOURCEQUENCH:
case ICMP_TSTAMP:
case ICMP_MASKREQ:
case ICMP_REDIRECT:
STAT(icmpstat.icps_notsupp++);
m_freem(m);
break;
default:
STAT(icmpstat.icps_badtype++);
m_freem(m);
} /* swith */
end_error:
/* m is m_free()'d xor put in a socket xor or given to ip_send */
return;
}
/**
* This function is used to send a message over Ethernet
*
* @param socket_index - Index of the socket on which to send message
* @param to - Pointer to socket address structure to send message
* @param buffers - Array of transport buffers to send
* @param num_buffers - Number of transport buffers in 'buffers' array
*
* @return None
*
* @note This function requires that the first transport buffer in the 'buffers'
* array contain the Transport header.
*
*****************************************************************************/
void transport_send(int socket_index, struct sockaddr * to, warp_ip_udp_buffer ** buffers, u32 num_buffers) {
u32 i;
int status;
transport_header * transport_header_tx;
u16 buffer_length = 0;
// interrupt_state_t prev_interrupt_state;
// Check that we have a valid socket to send a message on
if (socket_index == SOCKET_INVALID_SOCKET) {
wlan_exp_printf(WLAN_EXP_PRINT_ERROR, print_type_transport, "Invalid socket.\n");
return;
}
// Initialize the header
// NOTE: We require that the first warp_ip_udp_buffer always contain the wl_transport_header
//
transport_header_tx = (transport_header *)(buffers[0]->data);
// Compute the length
for (i = 0; i < num_buffers; i++) {
buffer_length += buffers[i]->size;
}
//
// NOTE: Through performance testing, we found that it was faster to just manipulate the header
// in place vs creating a copy, updating the header and then restoring the copy.
//
// Make the outgoing transport header endian safe for sending on the network
// NOTE: Set the 'length' to the computed value above
//
transport_header_tx->dest_id = Xil_Htons(transport_header_tx->dest_id);
transport_header_tx->src_id = Xil_Htons(transport_header_tx->src_id);
transport_header_tx->length = Xil_Htons(buffer_length + WARP_IP_UDP_DELIM_LEN);
transport_header_tx->seq_num = Xil_Htons(transport_header_tx->seq_num);
transport_header_tx->flags = Xil_Htons(transport_header_tx->flags);
// Check the interrupt status; Disable interrupts if enabled
// NOTE: This is done inside the Eth send function
// prev_interrupt_state = wlan_mac_high_interrupt_stop();
// Send the Ethernet packet
status = socket_sendto(socket_index, to, buffers, num_buffers);
// Restore interrupts
// NOTE: This is done inside the Eth send function
// wlan_mac_high_interrupt_restore_state(prev_interrupt_state);
// Restore wl_header_tx
transport_header_tx->dest_id = Xil_Ntohs(transport_header_tx->dest_id);
transport_header_tx->src_id = Xil_Ntohs(transport_header_tx->src_id);
transport_header_tx->length = 0;
transport_header_tx->seq_num = Xil_Ntohs(transport_header_tx->seq_num);
transport_header_tx->flags = Xil_Ntohs(transport_header_tx->flags);
// Check that the packet was sent correctly
if (status == WARP_IP_UDP_FAILURE) {
wlan_exp_printf(WLAN_EXP_PRINT_WARNING, print_type_transport, "Issue sending packet %d to host.\n", transport_header_tx->seq_num);
}
}
int simple_sendtowithaddrinfo(int fd, const char* data, int length, int flag, const struct addrinfo* addr){
return socket_sendto(fd, data, length, flag, addr->ai_addr, addr->ai_addrlen);
}
static void vbeacon_rfd( void *pvParameters )
{
buffer_t *buffer=0;
uint8_t send=0, *ptr, i, sqn=0;
int16_t byte;
uint16_t count = 0;
pause(200);
debug_init(115200);
pause(300);
stack_event = open_stack_event_bus(); /* Open socket for stack status message */
stack_service_init( stack_event,NULL, 0 , NULL );
if(stack_start(NULL)==START_SUCCESS)
{
debug("Start Ok\r\n");
}
/*****************************************************************************************************/
/****************************************************************************************************/
for (;;)
{
byte = debug_read_blocking(10);
if(byte != -1)
{
switch(byte)
{
case 'm':
ptr=mac_get_mac_pib_parameter(MAC_IEEE_ADDRESS);
if(ptr)
{
ptr +=7;
debug("Devices mac-address: ");
for(i=0; i< 8 ;i++)
{
if(i)
debug(":");
debug_hex(*ptr--);
}
debug("\r\n");
}
break;
case 'b':
debug_int(uxQueueMessagesWaiting(buffers));
debug(" buffers available.\r\n");
break;
case 'c':
ptr=mac_get_mac_pib_parameter(MAC_CURRENT_CHANNEL);
if(ptr)
{
debug("Current channel: ");
debug_int(*ptr);
debug("\r\n");
}
break;
case '\r':
debug("\r\n");
break;
default:
debug_put(byte);
break;
}
}
if(send && data)
{
buffer_t *buf = socket_buffer_get(data);
if (buf)
{
/* Create data packet */
/*buf->buf[buf->buf_end++] = 'S';
buf->buf[buf->buf_end++] = 'e';
buf->buf[buf->buf_end++] = 'n';
buf->buf[buf->buf_end++] = 's';
buf->buf[buf->buf_end++] = 'o';
buf->buf[buf->buf_end++] = 'r';
buf->buf[buf->buf_end++] = '_';
buf->buf[buf->buf_end++] = mac_long.address[1];
buf->buf[buf->buf_end++] = mac_long.address[0];*/
buf->buf[buf->buf_end++] = sqn;
buf->buf[buf->buf_end++] = 0xc2;
buf->buf[buf->buf_end++] = 0x00;
buf->buf[buf->buf_end++] = 0x08;
buf->buf[buf->buf_end++] = 0x00;
buf->buf[buf->buf_end++] = 0x08;
buf->buf[buf->buf_end++] = 0x00;
buf->buf[buf->buf_end++] = 0x10;
buf->buf[buf->buf_end++] = 0x23;
buf->buf[buf->buf_end++] = 0x16;
if (socket_sendto(data, &cord_address, buf) != pdTRUE)
{
debug("Data send failed.\r\n");
socket_buffer_free(buf);
buf=0;
}
if(sqn==0x0f)
sqn=0;
else
sqn++;
send=0;
}
}
if (count++ >= 400)
{
send=1;
count = 0;
}
if(stack_event)
{
buffer=0;
buffer = waiting_stack_event(10);
if(buffer)
{
switch (parse_event_message(buffer))
{
case BROKEN_LINK:
debug("Route broken to ");
debug("\r\n");
debug_address(&(buffer->dst_sa));
debug("\r\n");
break;
case ASSOCIATION_WITH_COORDINATOR:
debug("Assoc ok ");
if(get_coord_address(&cord_address) == pdTRUE)
{
data = socket(MODULE_CUDP, 0); /* Socket for response data from port number 61619 */
if (data)
{
if (socket_bind(data, &cord_address) != pdTRUE)
{
debug("Bind failed.\r\n");
}
}
}
break;
case NOT_ASSOCIATED:
debug("Application design error:\r\n");
debug("RFD try send data before association\r\n");
break;
case ASSOCIATION_LOST:
if(socket_close(data) == pdTRUE)
{
data=0;
scan_network();
}
break;
case TOO_LONG_PACKET:
debug("Payload Too Length, to ");
debug("\r\n");
break;
default:
break;
}
if(buffer)
{
socket_buffer_free(buffer);
buffer = 0;
}
}
}
LED1_ON();
LED1_OFF();
}
}
