static void udps_send_user(TCPIPS* tcpips, IP* src, IO* io, HANDLE handle) { IO* user_io; unsigned int offset, size; UDP_STACK* udp_stack; UDP_HANDLE* uh; UDP_HEADER* hdr = io_data(io); uh = so_get(&tcpips->udps.handles, handle); for (offset = sizeof(UDP_HEADER); uh->head && offset < io->data_size; offset += size) { user_io = udps_peek_head(tcpips, uh); udp_stack = io_push(user_io, sizeof(UDP_STACK)); udp_stack->remote_addr.u32.ip = src->u32.ip; udp_stack->remote_port = be2short(hdr->src_port_be); size = io_get_free(user_io); if (size > io->data_size - offset) size = io->data_size - offset; memcpy(io_data(user_io), (uint8_t*)io_data(io) + offset, size); user_io->data_size = size; io_complete(uh->process, HAL_IO_CMD(HAL_UDP, IPC_READ), handle, user_io); } #if (UDP_DEBUG) if (offset < io->data_size) printf("UDP: %d byte(s) dropped\n", io->data_size - offset); #endif //UDP_DEBUG }
void udps_icmps_error_process(TCPIPS* tcpips, IO* io, ICMP_ERROR code, const IP* src) { HANDLE handle; UDP_HANDLE* uh; uint16_t src_port, dst_port; UDP_HEADER* hdr = io_data(io); src_port = be2short(hdr->src_port_be); dst_port = be2short(hdr->dst_port_be); if ((handle = udps_find(tcpips, src_port)) == INVALID_HANDLE) return; uh = so_get(&tcpips->udps.handles, handle); //ignore errors on listening sockets if (uh->remote_port == dst_port && uh->remote_addr.u32.ip == src->u32.ip) { switch (code) { case ICMP_ERROR_NETWORK: case ICMP_ERROR_HOST: case ICMP_ERROR_PROTOCOL: case ICMP_ERROR_PORT: case ICMP_ERROR_FRAGMENTATION: case ICMP_ERROR_ROUTE: uh->err = ERROR_NOT_RESPONDING; break; case ICMP_ERROR_TTL_EXCEED: case ICMP_ERROR_FRAGMENT_REASSEMBLY_EXCEED: uh->err = ERROR_TIMEOUT; break; case ICMP_ERROR_PARAMETER: uh->err = ERROR_INVALID_PARAMS; break; default: break; } udps_flush(tcpips, handle); } }
static inline void icmps_rx_echo_reply(TCPIPS* tcpips, IO* io, IP* src) { ICMP_HEADER_ID_SEQ* icmp = io_data(io); #if (ICMP_DEBUG) printf("ICMP: ECHO REPLY from "); ip_print(src); printf("\n"); #endif //compare src, sequence, id and data. Maybe asynchronous response from failed request if ((tcpips->icmps.echo_ip.u32.ip != src->u32.ip) || (tcpips->icmps.id != be2short(icmp->id_be)) || (tcpips->icmps.seq != be2short(icmp->seq_be))) { ips_release_io(tcpips, io); return; } icmps_echo_complete(tcpips, (memcmp(((uint8_t*)io_data(io)) + sizeof(ICMP_HEADER), __ICMP_DATA_MAGIC, ICMP_DATA_MAGIC_SIZE)) ? ERROR_CRC : ERROR_OK); }
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); }
void scsis_request_cmd(SCSIS* scsis, IO* io, uint8_t* req) { scsis->io = io; switch (req[0]) { case SCSI_SPC_CMD_MODE_SENSE6: scsis_pc_mode_sense6(scsis, req); break; case SCSI_SPC_CMD_MODE_SELECT6: scsis_pc_mode_select6(scsis, req); break; case SCSI_SPC_CMD_TEST_UNIT_READY: scsis_pc_test_unit_ready(scsis, req); break; case SCSI_SPC_CMD_INQUIRY: scsis_pc_inquiry(scsis, req); break; case SCSI_SPC_CMD_REQUEST_SENSE: scsis_pc_request_sense(scsis, req); break; case SCSI_SPC_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL: scsis_pc_prevent_allow_medium_removal(scsis, req); break; case SCSI_SBC_CMD_READ_CAPACITY10: scsis_bc_read_capacity10(scsis, req); break; case SCSI_SBC_CMD_START_STOP_UNIT: scsis_bc_start_stop_unit(scsis, req); break; case SCSI_SBC_CMD_READ6: scsis_bc_read6(scsis, req); break; case SCSI_SBC_CMD_READ10: scsis_bc_read10(scsis, req); break; case SCSI_SBC_CMD_READ12: scsis_bc_read12(scsis, req); break; case SCSI_SBC_CMD_WRITE6: scsis_bc_write6(scsis, req); break; case SCSI_SBC_CMD_WRITE10: scsis_bc_write10(scsis, req); break; case SCSI_SBC_CMD_WRITE12: scsis_bc_write12(scsis, req); break; #if (SCSI_VERIFY_SUPPORTED) case SCSI_SBC_CMD_VERIFY10: scsis_bc_verify10(scsis, req); break; case SCSI_SBC_CMD_VERIFY12: scsis_bc_verify12(scsis, req); break; case SCSI_SBC_CMD_WRITE_AND_VERIFY10: scsis_bc_write_verify10(scsis, req); break; case SCSI_SBC_CMD_WRITE_AND_VERIFY12: scsis_bc_write_verify12(scsis, req); break; #endif //SCSI_VERIFY_SUPPORTED #if (SCSI_LONG_LBA) case SCSI_SPC_CMD_MODE_SENSE10: scsis_pc_mode_sense10(scsis, req); break; case SCSI_SPC_CMD_MODE_SELECT10: scsis_pc_mode_select10(scsis, req); break; case SCSI_SBC_CMD_READ16: scsis_bc_read16(scsis, req); break; case SCSI_SBC_CMD_WRITE16: scsis_bc_write16(scsis, req); break; #if (SCSI_VERIFY_SUPPORTED) case SCSI_SBC_CMD_VERIFY16: scsis_bc_verify16(scsis, req); break; case SCSI_SBC_CMD_WRITE_AND_VERIFY16: scsis_bc_write_verify16(scsis, req); break; #endif //SCSI_VERIFY_SUPPORTED case SCSI_SBC_CMD_EXT_7F: switch (be2short(req + 2)) { case SCSI_SBC_CMD_EXT_7F_READ32: scsis_bc_read32(scsis, req); break; case SCSI_SBC_CMD_EXT_7F_WRITE32: scsis_bc_write32(scsis, req); break; #if (SCSI_VERIFY_SUPPORTED) case SCSI_SBC_CMD_EXT_7F_VERIFY32: scsis_bc_verify32(scsis, req); break; case SCSI_SBC_CMD_EXT_7F_WRITE_AND_VERIFY32: scsis_bc_write_verify32(scsis, req); break; #endif //SCSI_VERIFY_SUPPORTED default: #if (SCSI_DEBUG_ERRORS) printf("SCSI: unknown cmd 0x7f action: %02xh\n", be2short(req + 2)); #endif //SCSI_DEBUG_ERRORS scsis_fail(scsis, SENSE_KEY_ILLEGAL_REQUEST, ASCQ_INVALID_COMMAND_OPERATION_CODE); } break; #endif //SCSI_LONG_LBA #if (SCSI_SAT) //SAT case SCSI_SAT_CMD_ATA_PASS_THROUGH12: scsis_sat_ata_pass_through12(scsis, req); break; case SCSI_SAT_CMD_ATA_PASS_THROUGH16: scsis_sat_ata_pass_through16(scsis, req); break; #endif //SCSI_SAT #if (SCSI_MMC) case SCSI_MMC_CMD_READ_TOC: scsis_mmc_read_toc(scsis, req); break; case SCSI_MMC_CMD_GET_CONFIGURATION: scsis_mmc_get_configuration(scsis, req); break; case SCSI_MMC_CMD_GET_EVENT_STATUS_NOTIFICATION: scsis_mmc_get_event_status_notification(scsis, req); break; #endif //SCSI_MMC default: #if (SCSI_DEBUG_ERRORS) printf("SCSI: unknown cmd opcode: %02xh\n", req[0]); #endif //SCSI_DEBUG_ERRORS scsis_fail(scsis, SENSE_KEY_ILLEGAL_REQUEST, ASCQ_INVALID_COMMAND_OPERATION_CODE); break; } }
void udps_rx(TCPIPS* tcpips, IO* io, IP* src) { HANDLE handle; UDP_HEADER* hdr; UDP_HANDLE* uh; uint16_t src_port, dst_port; #if(UDP_BROADCAST) const IP* dst; dst = (const IP*)io_data(io) - 1; if (io->data_size < sizeof(UDP_HEADER) || udp_checksum(io_data(io), io->data_size, src, dst)) #else if (io->data_size < sizeof(UDP_HEADER) || udp_checksum(io_data(io), io->data_size, src, &tcpips->ips.ip)) #endif { ips_release_io(tcpips, io); return; } hdr = io_data(io); src_port = be2short(hdr->src_port_be); dst_port = be2short(hdr->dst_port_be); #if (UDP_DEBUG_FLOW) printf("UDP: "); ip_print(src); printf(":%d -> ", src_port); ip_print(&tcpips->ips.ip); printf(":%d, %d byte(s)\n", dst_port, io->data_size - sizeof(UDP_HEADER)); #endif //UDP_DEBUG_FLOW #if(DHCPS) if((dst_port == DHCP_SERVER_PORT)||(src_port == DHCP_CLIENT_PORT)) { io_hide(io, sizeof(UDP_HEADER)); if(dhcps_rx(tcpips,io,src)) udps_replay(tcpips,io,&__BROADCAST); else ips_release_io(tcpips, io); return; } #endif #if(DNSS) if((dst_port == DNS_PORT)&&(dst->u32.ip == tcpips->ips.ip.u32.ip)) { io_hide(io, sizeof(UDP_HEADER)); if(dnss_rx(tcpips,io,src)) udps_replay(tcpips,io,src); else ips_release_io(tcpips, io); return; } #endif //search in listeners handle = udps_find(tcpips, dst_port); if (handle != INVALID_HANDLE) { uh = so_get(&tcpips->udps.handles, handle); //listener or connected if (uh->remote_port == 0 || (uh->remote_port == src_port && uh->remote_addr.u32.ip == src->u32.ip)) udps_send_user(tcpips, src, io, handle); else handle = INVALID_HANDLE; } #if(UDP_BROADCAST) if ((handle == INVALID_HANDLE) && (dst->u32.ip != BROADCAST)) #else if (handle == INVALID_HANDLE) #endif { #if (UDP_DEBUG) printf("UDP: no connection, datagramm dropped\n"); #endif //UDP_DEBUG #if (ICMP) icmps_tx_error(tcpips, io, ICMP_ERROR_PORT, 0); #endif //ICMP } ips_release_io(tcpips, io); }