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); }