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