void icmps_tx_error(TCPIPS* tcpips, IO* original, ICMP_ERROR err, unsigned int offset)
{
//unhide ip header
IP_STACK* ip_stack;
ip_stack = io_stack(original);
original->data_offset -= ip_stack->hdr_size;
original->data_size += ip_stack->hdr_size;
switch (err)
{
case ICMP_ERROR_NETWORK:
case ICMP_ERROR_HOST:
case ICMP_ERROR_PROTOCOL:
case ICMP_ERROR_PORT:
case ICMP_ERROR_FRAGMENTATION:
case ICMP_ERROR_ROUTE:
icmps_tx_destination_unreachable(tcpips, err, original);
break;
case ICMP_ERROR_TTL_EXCEED:
case ICMP_ERROR_FRAGMENT_REASSEMBLY_EXCEED:
icmps_tx_time_exceed(tcpips, err - ICMP_ERROR_TTL_EXCEED, original);
break;
case ICMP_ERROR_PARAMETER:
icmps_tx_parameter_problem(tcpips, offset, original);
break;
case ICMP_ERROR_OK:
break;
}
original->data_offset -= ip_stack->hdr_size;
original->data_size += ip_stack->hdr_size;
}
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 void icmps_control_prepare(TCPIPS* tcpips, uint8_t cmd, uint8_t code, IO* original)
{
IP_STACK* ip_stack;
ip_stack = io_stack(original);
//cmd
((uint8_t*)io_data(original))[0] = cmd;
//code
((uint8_t*)io_data(original))[1] = code;
//generally unused
*(uint32_t*)(((uint8_t*)io_data(original)) + 4) = 0;
//64 bits + original IP header
memmove(((uint8_t*)io_data(original)) + sizeof(ICMP_HEADER), ((uint8_t*)io_data(original)) - ip_stack->hdr_size, ip_stack->hdr_size + 8);
}
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);
}
static inline void lpc_sdmmc_io(CORE* core, HANDLE process, HANDLE user, IO* io, unsigned int size, bool read)
{
SHA1_CTX sha1;
unsigned int count;
STORAGE_STACK* stack = io_stack(io);
io_pop(io, sizeof(STORAGE_STACK));
if (!core->sdmmc.active)
{
error(ERROR_NOT_CONFIGURED);
return;
}
if (core->sdmmc.state != SDMMC_STATE_IDLE)
{
error(ERROR_IN_PROGRESS);
return;
}
if ((user != core->sdmmc.user) || (size == 0))
{
error(ERROR_INVALID_PARAMS);
return;
}
//erase
if (!read && ((stack->flags & STORAGE_MASK_MODE) == 0))
{
sdmmcs_erase(&core->sdmmc.sdmmcs, stack->sector, size);
return;
}
if (size % core->sdmmc.sdmmcs.sector_size)
{
error(ERROR_INVALID_PARAMS);
return;
}
count = size / core->sdmmc.sdmmcs.sector_size;
core->sdmmc.total = size;
if (core->sdmmc.total > LPC_SDMMC_TOTAL_SIZE)
{
error(ERROR_INVALID_PARAMS);
return;
}
if ((core->sdmmc.activity != INVALID_HANDLE) && !(stack->flags & STORAGE_FLAG_IGNORE_ACTIVITY_ON_REQUEST))
{
ipc_post_inline(core->sdmmc.activity, HAL_CMD(HAL_SDMMC, STORAGE_NOTIFY_ACTIVITY), core->sdmmc.user, read ? 0 : STORAGE_FLAG_WRITE, 0);
core->sdmmc.activity = INVALID_HANDLE;
}
core->sdmmc.io = io;
core->sdmmc.process = process;
lpc_sdmmc_prepare_descriptors(core);
if (read)
{
io->data_size = 0;
core->sdmmc.state = SDMMC_STATE_READ;
if (sdmmcs_read(&core->sdmmc.sdmmcs, stack->sector, count))
error(ERROR_SYNC);
}
else
{
switch (stack->flags & STORAGE_MASK_MODE)
{
case STORAGE_FLAG_WRITE:
core->sdmmc.state = SDMMC_STATE_WRITE;
if (sdmmcs_write(&core->sdmmc.sdmmcs, stack->sector, count))
error(ERROR_SYNC);
break;
default:
//verify/verify write
sha1_init(&sha1);
sha1_update(&sha1, io_data(io), core->sdmmc.total);
sha1_final(&sha1, core->sdmmc.hash);
core->sdmmc.sector = stack->sector;
switch (stack->flags & STORAGE_MASK_MODE)
{
case STORAGE_FLAG_VERIFY:
core->sdmmc.state = SDMMC_STATE_VERIFY;
if (sdmmcs_read(&core->sdmmc.sdmmcs, stack->sector, count))
error(ERROR_SYNC);
break;
case (STORAGE_FLAG_VERIFY | STORAGE_FLAG_WRITE):
core->sdmmc.state = SDMMC_STATE_WRITE_VERIFY;
if (sdmmcs_write(&core->sdmmc.sdmmcs, stack->sector, count))
error(ERROR_SYNC);
break;
default:
error(ERROR_NOT_SUPPORTED);
return;
}
}
}
}
