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