static inline void udps_write(TCPIPS* tcpips, HANDLE handle, IO* io)
{
IO* cur;
unsigned int offset, size;
unsigned short remote_port;
IP dst;
UDP_STACK* udp_stack;
UDP_HEADER* udp;
UDP_HANDLE* uh = so_get(&tcpips->udps.handles, handle);
if (uh == NULL)
return;
#if (ICMP)
if (uh->err != ERROR_OK)
{
error(uh->err);
return;
}
#endif //ICMP
//listening socket
if (uh->remote_port == 0)
{
if (io->stack_size < sizeof(UDP_STACK))
{
error(ERROR_INVALID_PARAMS);
return;
}
udp_stack = io_stack(io);
remote_port = udp_stack->remote_port;
dst.u32.ip = udp_stack->remote_addr.u32.ip;
io_pop(io, sizeof(UDP_STACK));
}
else
{
remote_port = uh->remote_port;
dst.u32.ip = uh->remote_addr.u32.ip;
}
for (offset = 0; offset < io->data_size; offset += size)
{
size = UDP_FRAME_MAX_DATA_SIZE;
if (size > io->data_size - offset)
size = io->data_size - offset;
cur = ips_allocate_io(tcpips, size + sizeof(UDP_HEADER), PROTO_UDP);
if (cur == NULL)
return;
//copy data
memcpy((uint8_t*)io_data(cur) + sizeof(UDP_HEADER), (uint8_t*)io_data(io) + offset, size);
udp = io_data(cur);
// correct size
cur->data_size = size + sizeof(UDP_HEADER);
//format header
short2be(udp->src_port_be, uh->local_port);
short2be(udp->dst_port_be, remote_port);
short2be(udp->len_be, size + sizeof(UDP_HEADER));
short2be(udp->checksum_be, 0);
short2be(udp->checksum_be, udp_checksum(io_data(cur), cur->data_size, &tcpips->ips.ip, &dst));
ips_tx(tcpips, cur, &dst);
}
}
static inline void icmps_tx_echo(TCPIPS* tcpips)
{
ICMP_HEADER_ID_SEQ* icmp;
IO* io = ips_allocate_io(tcpips, ICMP_DATA_MAGIC_SIZE + sizeof(ICMP_HEADER), PROTO_ICMP);
if (io == NULL)
icmps_echo_complete(tcpips, get_last_error());
icmp = io_data(io);
icmp->type = ICMP_CMD_ECHO;
icmp->code = 0;
short2be(icmp->id_be, tcpips->icmps.id);
short2be(icmp->seq_be, tcpips->icmps.seq);
memcpy(((uint8_t*)io_data(io)) + sizeof(ICMP_HEADER), __ICMP_DATA_MAGIC, ICMP_DATA_MAGIC_SIZE);
tcpips->icmps.ttl = tcpips->seconds + ICMP_ECHO_TIMEOUT;
io->data_size = ICMP_DATA_MAGIC_SIZE + sizeof(ICMP_HEADER);
icmps_tx(tcpips, io, &tcpips->icmps.echo_ip);
}
static inline void icmps_tx_time_exceed(TCPIPS* tcpips, uint8_t code, IO* original)
{
ICMP_HEADER* icmp;
IP_STACK* ip_stack = io_stack(original);
IP_HEADER* hdr = io_data(original);
#if (ICMP_DEBUG)
printf("ICMP: Time exceeded(%d) to ", code);
ip_print(&hdr->src);
printf("\n");
#endif
IO* io = ips_allocate_io(tcpips, sizeof(ICMP_HEADER) + ip_stack->hdr_size + 8, PROTO_ICMP);
if (io == NULL)
return;
icmp = io_data(io);
icmp->type = ICMP_CMD_TIME_EXCEEDED;
icmp->code = code;
memcpy(((uint8_t*)io_data(io)) + sizeof(ICMP_HEADER), io_data(original), ip_stack->hdr_size + 8);
io->data_size = ip_stack->hdr_size + 8 + sizeof(ICMP_HEADER);
icmps_tx(tcpips, io, &hdr->src);
}
static inline void icmps_tx_destination_unreachable(TCPIPS* tcpips, uint8_t code, IO* original)
{
ICMP_HEADER* icmp;
IP_STACK* ip_stack = io_stack(original);
IP_HEADER* hdr = io_data(original);
#if (ICMP_DEBUG)
printf("ICMP: Destination unreachable(%d) to ", code);
ip_print(&hdr->src);
printf("\n");
#endif
IO* io = ips_allocate_io(tcpips, sizeof(ICMP_HEADER) + ip_stack->hdr_size + 8, PROTO_ICMP);
if (io == NULL)
return;
icmp = io_data(io);
icmp->type = ICMP_CMD_DESTINATION_UNREACHABLE;
icmp->code = code;
memcpy(((uint8_t*)io_data(io)) + sizeof(ICMP_HEADER), io_data(original), ip_stack->hdr_size + 8);
io->data_size = ip_stack->hdr_size + 8 + sizeof(ICMP_HEADER);
icmps_tx(tcpips, io, &hdr->src);
}
static inline void icmps_tx_parameter_problem(TCPIPS* tcpips, uint8_t offset, IO* original)
{
ICMP_HEADER_PARAM* icmp;
IP_STACK* ip_stack = io_stack(original);
IP_HEADER* hdr = io_data(original);
#if (ICMP_DEBUG)
printf("ICMP: Parameter problem(%d) to ", offset);
ip_print(&hdr->src);
printf("\n");
#endif
IO* io = ips_allocate_io(tcpips, sizeof(ICMP_HEADER_PARAM) + ip_stack->hdr_size + 8, PROTO_ICMP);
if (io == NULL)
return;
icmp = io_data(io);
icmp->type = ICMP_CMD_PARAMETER_PROBLEM;
icmp->code = 0;
icmp->param = offset;
memcpy(((uint8_t*)io_data(io)) + sizeof(ICMP_HEADER_PARAM), io_data(original), ip_stack->hdr_size + 8);
io->data_size = ip_stack->hdr_size + 8 + sizeof(ICMP_HEADER_PARAM);
icmps_tx(tcpips, io, &hdr->src);
}
