static inline void cdc_acmd_read_complete(USBD* usbd, CDC_ACMD* cdc_acmd) { if (cdc_acmd->suspended) return; unsigned int to_read; if (cdc_acmd->rx_free < cdc_acmd->rx->data_size) cdc_acmd->rx_free = stream_get_free(cdc_acmd->rx_stream); to_read = cdc_acmd->rx->data_size; if (to_read > cdc_acmd->rx_free) to_read = cdc_acmd->rx_free; if (to_read < cdc_acmd->rx->data_size) cdc_acmd_notify_serial_state(usbd, cdc_acmd, CDC_SERIAL_STATE_DCD | CDC_SERIAL_STATE_DSR | CDC_SERIAL_STATE_OVERRUN); #if (USBD_CDC_ACM_DEBUG_FLOW) int i; printf("USB CDC ACM: rx "); for (i = 0; i < cdc_acmd->rx->data_size; ++i) if (((uint8_t*)io_data(cdc_acmd->rx))[i] >= ' ' && ((uint8_t*)io_data(cdc_acmd->rx))[i] <= '~') printf("%c", ((char*)io_data(cdc_acmd->rx))[i]); else printf("\\x%d", ((uint8_t*)io_data(cdc_acmd->rx))[i]); printf("\n"); #endif //USBD_CDC_ACM_DEBUG_FLOW if (to_read && stream_write(cdc_acmd->rx_stream_handle, io_data(cdc_acmd->rx), to_read)) cdc_acmd->rx_free -= to_read; usbd_usb_ep_read(usbd, cdc_acmd->data_ep, cdc_acmd->rx, cdc_acmd->data_ep_size); }
static inline void udps_read(TCPIPS* tcpips, HANDLE handle, IO* io) { IO* cur; UDP_HANDLE* uh; uh = so_get(&tcpips->udps.handles, handle); if (uh == NULL) return; #if (ICMP) if (uh->err != ERROR_OK) { error(uh->err); return; } #endif //ICMP io->data_size = 0; *((IO**)io_data(io)) = NULL; //add to head if (uh->head == NULL) uh->head = io; //add to end else { for (cur = uh->head; *((IO**)io_data(cur)) != NULL; cur = *((IO**)io_data(cur))) {} *((IO**)io_data(cur)) = io; } error(ERROR_SYNC); }
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 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 }
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 void loras_open(LORA* lora, HANDLE process, IO* io_config) { LORA_CONFIG* config = &lora->config; if (!(io_config && process != INVALID_HANDLE)) { error(ERROR_INVALID_PARAMS); return; } lora->process = process; memcpy(config, io_data(io_config), sizeof(LORA_CONFIG)); if (!loras_spi_open(lora)) { error(ERROR_INVALID_PARAMS); return; } loras_clear_vars(lora); if (!loras_hw_open(lora)) { error(ERROR_INVALID_PARAMS); return; } lora->opened = true; #if (LORA_DEBUG) printf("[loras] [info] open completed\n"); #endif }
static inline int set_line_coding(USBD* usbd, CDC_ACMD* cdc_acmd, IO* io) { LINE_CODING_STRUCT* lc = io_data(io); cdc_acmd->baud.baud = lc->dwDTERate; if (lc->bCharFormat >= LC_BAUD_STOP_BITS_SIZE) return -1; cdc_acmd->baud.stop_bits = LC_BAUD_STOP_BITS[lc->bCharFormat]; if (lc->bParityType >= LC_BAUD_PARITY_SIZE) return -1; cdc_acmd->baud.parity = LC_BAUD_PARITY[lc->bParityType]; cdc_acmd->baud.data_bits = lc->bDataBits; #if (USBD_CDC_ACM_FLOW_CONTROL) if (cdc_acmd->flow_sending || cdc_acmd->break_count) cdc_acmd->flow_changed = true; else { usbd_post_user(usbd, cdc_acmd->data_iface, 0, HAL_REQ(HAL_USBD_IFACE, USB_CDC_ACM_BAUDRATE_REQUEST), cdc_acmd->baud.baud, (cdc_acmd->baud.data_bits << 16) | (cdc_acmd->baud.parity << 8) | cdc_acmd->baud.stop_bits); cdc_acmd->flow_sending = true; } #endif //USBD_CDC_ACM_FLOW_CONTROL #if (USBD_CDC_ACM_DEBUG) printf("USB CDC ACM: set line coding %d %d%c%d\n", cdc_acmd->baud.baud, cdc_acmd->baud.data_bits, cdc_acmd->baud.parity, cdc_acmd->baud.stop_bits); #endif return 0; }
void cdc_acmd_write(USBD* usbd, CDC_ACMD* cdc_acmd) { if (!cdc_acmd->DTR || !cdc_acmd->tx_idle || cdc_acmd->suspended) return; unsigned int to_write; if (cdc_acmd->tx_size == 0) cdc_acmd->tx_size = stream_get_size(cdc_acmd->tx_stream); to_write = cdc_acmd->tx_size; if (to_write > cdc_acmd->data_ep_size) to_write = cdc_acmd->data_ep_size; if (to_write) { cdc_acmd->tx_size -= to_write; if (stream_read(cdc_acmd->tx_stream_handle, io_data(cdc_acmd->tx), to_write)) { cdc_acmd->tx_idle = false; cdc_acmd->tx->data_size = to_write; usbd_usb_ep_write(usbd, cdc_acmd->data_ep, cdc_acmd->tx); } //just in case of driver failure else stream_listen(cdc_acmd->tx_stream, USBD_IFACE(cdc_acmd->data_iface, 0), HAL_USBD_IFACE); } else stream_listen(cdc_acmd->tx_stream, USBD_IFACE(cdc_acmd->data_iface, 0), HAL_USBD_IFACE); }
static void icmps_rx_error_process(TCPIPS* tcpips, IO* io, ICMP_ERROR code) { //hide ICMP header for protocol-depending processing IP_HEADER* hdr; IP dst; unsigned int hdr_size; hdr = io_data(io); hdr_size = (hdr->ver_ihl & 0xf) << 2; io->data_offset += hdr_size; io->data_size -= hdr_size; if (hdr->src.u32.ip == tcpips->ips.ip.u32.ip) { dst.u32.ip = hdr->dst.u32.ip; switch (hdr->proto) { case PROTO_ICMP: icmps_error_process(tcpips, code); break; #if (UDP) case PROTO_UDP: udps_icmps_error_process(tcpips, io, code, &dst); break; #endif //UDP case PROTO_TCP: tcps_icmps_error_process(tcpips, io, code, &dst); default: break; } } //restore IP header io->data_offset -= hdr_size; io->data_size += hdr_size; }
static inline void lpc_sdmmc_verify(CORE* core) { SHA1_CTX sha1; uint8_t hash_in[SHA1_BLOCK_SIZE]; if ((core->sdmmc.state != SDMMC_STATE_VERIFY) && (core->sdmmc.state != SDMMC_STATE_WRITE_VERIFY)) return; sha1_init(&sha1); sha1_update(&sha1, io_data(core->sdmmc.io), core->sdmmc.total); sha1_final(&sha1, core->sdmmc.state == SDMMC_STATE_VERIFY ? hash_in : core->sdmmc.hash); if (core->sdmmc.state == SDMMC_STATE_WRITE_VERIFY) { core->sdmmc.state = SDMMC_STATE_VERIFY; lpc_sdmmc_prepare_descriptors(core); if (sdmmcs_read(&core->sdmmc.sdmmcs, core->sdmmc.sector, core->sdmmc.total / core->sdmmc.sdmmcs.sector_size)) return; } else { if (memcmp(core->sdmmc.hash, hash_in, SHA1_BLOCK_SIZE) == 0) { io_complete(core->sdmmc.process, HAL_IO_CMD(HAL_SDMMC, IPC_WRITE), core->sdmmc.user, core->sdmmc.io); core->sdmmc.io = NULL; core->sdmmc.process = INVALID_HANDLE; core->sdmmc.state = SDMMC_STATE_IDLE; return; } } io_complete_ex(core->sdmmc.process, HAL_IO_CMD(HAL_SDMMC, IPC_WRITE), core->sdmmc.user, core->sdmmc.io, get_last_error()); core->sdmmc.io = NULL; core->sdmmc.process = INVALID_HANDLE; core->sdmmc.state = SDMMC_STATE_IDLE; }
static inline int hidd_kbd_get_report(HIDD_KBD* hidd, unsigned int type, IO* io) { int res = -1; uint8_t* report = io_data(io); switch (type) { case HID_REPORT_TYPE_INPUT: #if (USBD_HID_DEBUG_REQUESTS) printf("HIDD KBD: get INPUT report\n"); #endif report[0] = hidd->kbd.modifier; report[1] = 0; memcpy(report + 2, &hidd->kbd.keys, 6); res = 8; break; case HID_REPORT_TYPE_OUTPUT: #if (USBD_HID_DEBUG_REQUESTS) printf("HIDD KBD: get OUTPUT report\n"); #endif report[0] = hidd->kbd.leds; res = 1; break; } return res; }
static IO* udps_peek_head(TCPIPS* tcpips, UDP_HANDLE* uh) { IO* io = uh->head; if (io) uh->head = *((IO**)io_data(uh->head)); return io; }
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 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); }
void* vfss_read_sectors(VFSS_TYPE* vfss, unsigned long sector, unsigned size) { bool res; //cache read if ((sector == vfss->current_sector) && (vfss->io->data_size == size)) return io_data(vfss->io); #if (VFS_BER) if (vfss->volume.sector_mode == SECTOR_MODE_BER) res = ber_read_sectors(vfss, sector, size); else #endif //VFS_BER res = storage_read_sync(vfss->volume.hal, vfss->volume.process, vfss->volume.user, vfss->io, sector + vfss->volume.first_sector, size); if (!res) return NULL; vfss->current_sector = sector; return io_data(vfss->io); }
void icmps_rx(TCPIPS* tcpips, IO *io, IP* src) { ICMP_HEADER* icmp = io_data(io); //drop broken ICMP without control, because ICMP is control protocol itself if (io->data_size < sizeof(ICMP_HEADER)) { ips_release_io(tcpips, io); return; } if (ip_checksum(io_data(io), io->data_size)) { ips_release_io(tcpips, io); return; } switch (icmp->type) { case ICMP_CMD_ECHO_REPLY: icmps_rx_echo_reply(tcpips, io, src); break; #if (ICMP_ECHO) case ICMP_CMD_ECHO: icmps_rx_echo(tcpips, io, src); break; #endif case ICMP_CMD_DESTINATION_UNREACHABLE: icmps_rx_destination_unreachable(tcpips, io); break; case ICMP_CMD_TIME_EXCEEDED: icmps_rx_time_exceeded(tcpips, io); break; case ICMP_CMD_PARAMETER_PROBLEM: icmps_rx_parameter_problem(tcpips, io); break; default: #if (ICMP_DEBUG) printf("ICMP: unhandled type %d from ", icmp->type); ip_print(src); printf("\n"); #endif ips_release_io(tcpips, io); break; } }
static inline int hidd_kbd_get_protocol(HIDD_KBD* hidd, IO* io) { uint8_t* report = io_data(io); #if (USBD_HID_DEBUG_REQUESTS) printf("HIDD KBD: get protocol\n"); #endif report[0] = hidd->boot_protocol; return 1; }
static inline int mscd_get_max_lun(USBD* usbd, MSCD* mscd, IO* io) { *((uint8_t*)io_data(io)) = mscd->lun_count - 1; io->data_size = sizeof(uint8_t); #if (USBD_MSC_DEBUG_REQUESTS) printf("MSCD: Get Max Lun - %d\n", mscd->lun_count - 1); #endif //USBD_MSC_DEBUG_REQUESTS return sizeof(uint8_t); }
static inline int hidd_kbd_get_idle(HIDD_KBD* hidd, IO* io) { uint8_t* report = io_data(io); #if (USBD_HID_DEBUG_REQUESTS) printf("HIDD KBD: get idle\n"); #endif report[0] = hidd->idle; return 1; }
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 rndis_set_vendor_description(HANDLE usbd, unsigned int iface, const char* vendor) { IO* io; unsigned int size = strlen(vendor) + 1; io = io_create(size); strcpy(io_data(io), vendor); io->data_size = size; io_write_sync(usbd, HAL_IO_REQ(HAL_USBD_IFACE, RNDIS_SET_VENDOR_DESCRIPTION), iface, io); io_destroy(io); }
void icmps_parameter_problem(TCPIPS* tcpips, uint8_t offset, IO* original, const IP* dst) { #if (ICMP_DEBUG) printf("ICMP: Parameter problem(%d) to ", offset); ip_print(dst); printf("\n"); #endif icmps_control_prepare(tcpips, ICMP_CMD_PARAMETER_PROBLEM, 0, original); ((uint8_t*)io_data(original))[4] = offset; icmps_tx(tcpips, original, dst); }
static inline int hidd_kbd_set_report(USBD* usbd, HIDD_KBD* hidd, IO* io, unsigned int length) { uint8_t* report = io_data(io); #if (USBD_HID_DEBUG_REQUESTS) printf("HIDD KBD: set LEDs %#X\n", report[0]); #endif if (hidd->kbd.leds != report[0]) usbd_post_user(usbd, hidd->iface, 0, HAL_CMD(HAL_USBD_IFACE, USB_HID_KBD_LEDS_STATE_CHANGED), report[0], 0); hidd->kbd.leds = report[0]; return 0; }
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_rx_echo(TCPIPS* tcpips, IO* io, IP* src) { ICMP_HEADER_ID_SEQ* icmp = io_data(io); #if (ICMP_DEBUG) printf("ICMP: ECHO from "); ip_print(src); printf("\n"); #endif icmp->type = ICMP_CMD_ECHO_REPLY; icmps_tx(tcpips, io, 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); }
void cdc_acmd_notify_serial_state(USBD* usbd, CDC_ACMD* cdc_acmd, unsigned int state) { if (cdc_acmd->notify_busy) { cdc_acmd->notify_state = state; cdc_acmd->notify_pending = true; return; } SETUP* setup = io_data(cdc_acmd->notify); setup->bmRequestType = BM_REQUEST_DIRECTION_DEVICE_TO_HOST | BM_REQUEST_TYPE_CLASS | BM_REQUEST_RECIPIENT_INTERFACE; setup->bRequest = CDC_SERIAL_STATE; setup->wValue = 0; setup->wIndex = 1; setup->wLength = 2; uint16_t* serial_state = (uint16_t*)(io_data(cdc_acmd->notify) + sizeof(SETUP)); *serial_state = state; cdc_acmd->notify->data_size = sizeof(SETUP) + 2; usbd_usb_ep_write(usbd, cdc_acmd->control_ep, cdc_acmd->notify); cdc_acmd->notify_busy = true; }
static inline void stm32_otg_tx(EXO* exo, int num) { EP* ep = exo->usb.in[USB_EP_NUM(num)]; int size = ep->io->data_size - ep->size; if (size > ep->mps) size = ep->mps; OTG_FS_DEVICE->INEP[USB_EP_NUM(num)].TSIZ = (1 << OTG_FS_DEVICE_ENDPOINT_TSIZ_PKTCNT_POS) | (size << OTG_FS_DEVICE_ENDPOINT_TSIZ_XFRSIZ_POS); OTG_FS_DEVICE->INEP[USB_EP_NUM(num)].CTL |= OTG_FS_DEVICE_ENDPOINT_CTL_EPENA | OTG_FS_DEVICE_ENDPOINT_CTL_CNAK; memcpy4((void*)(OTG_FS_FIFO_BASE + USB_EP_NUM(num) * 0x1000), io_data(ep->io) + ep->size, size); ep->size += size; }