static void icmps_tx(TCPIPS* tcpips, IO* io, const IP* dst)
{
ICMP_HEADER* icmp = io_data(io);
short2be(icmp->checksum_be, 0);
short2be(icmp->checksum_be, ip_checksum(io_data(io), io->data_size));
ips_tx(tcpips, io, dst);
}
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 void udps_replay(TCPIPS* tcpips, IO* io, const IP* src)
{
UDP_HEADER* udp;
IP dst;
uint16_t src_port;
dst.u32.ip = src->u32.ip;
io_unhide(io, sizeof(UDP_HEADER));
udp = io_data(io);
//format header
src_port = be2short(udp->dst_port_be);
udp->dst_port_be[0] = udp->src_port_be[0];
udp->dst_port_be[1] = udp->src_port_be[1];
short2be(udp->src_port_be, src_port);
short2be(udp->len_be, io->data_size);
short2be(udp->checksum_be, 0);
short2be(udp->checksum_be, udp_checksum(io_data(io), io->data_size, &tcpips->ips.ip, &dst));
ips_tx(tcpips, io, &dst);
}
