X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/4cd41f34ead6351f3b0a897453b25c036021c53f..refs/pull/256/head:/client/flash.c diff --git a/client/flash.c b/client/flash.c index 68d3b097..0b940e99 100644 --- a/client/flash.c +++ b/client/flash.c @@ -1,271 +1,477 @@ +//----------------------------------------------------------------------------- +// Copyright (C) 2010 Hector Martin "marcan" <marcan@marcansoft.com> +// +// This code is licensed to you under the terms of the GNU GPL, version 2 or, +// at your option, any later version. See the LICENSE.txt file for the text of +// the license. +//----------------------------------------------------------------------------- +// ELF file flasher +//----------------------------------------------------------------------------- + #include <stdio.h> #include <string.h> #include <stdlib.h> -#include "sleep.h" -#include "proxusb.h" +#include <inttypes.h> +#include <unistd.h> +#include "proxmark3.h" +#include "util.h" #include "flash.h" #include "elf.h" +#include "proxendian.h" +#include "usb_cmd.h" + +void SendCommand(UsbCommand* txcmd); +void ReceiveCommand(UsbCommand* rxcmd); +void CloseProxmark(); +int OpenProxmark(size_t i); + +// FIXME: what the fuckity fuck +unsigned int current_command = CMD_UNKNOWN; + +#define FLASH_START 0x100000 +#define FLASH_SIZE (256*1024) +#define FLASH_END (FLASH_START + FLASH_SIZE) +#define BOOTLOADER_SIZE 0x2000 +#define BOOTLOADER_END (FLASH_START + BOOTLOADER_SIZE) + +#define BLOCK_SIZE 0x200 + +static const uint8_t elf_ident[] = { + 0x7f, 'E', 'L', 'F', + ELFCLASS32, + ELFDATA2LSB, + EV_CURRENT +}; -static uint32_t ExpectedAddr; -static uint8_t QueuedToSend[256]; -#define PHYSICAL_FLASH_START 0x100000 -#define PHYSICAL_FLASH_END 0x200000 - -void WaitForAck(void) +// Turn PHDRs into flasher segments, checking for PHDR sanity and merging adjacent +// unaligned segments if needed +static int build_segs_from_phdrs(flash_file_t *ctx, FILE *fd, Elf32_Phdr *phdrs, int num_phdrs) { - UsbCommand ack; - ReceiveCommand(&ack); - if (ack.cmd != CMD_ACK) { - printf("bad ACK\n"); - exit(-1); - } + Elf32_Phdr *phdr = phdrs; + flash_seg_t *seg; + uint32_t last_end = 0; + + ctx->segments = malloc(sizeof(flash_seg_t) * num_phdrs); + if (!ctx->segments) { + fprintf(stderr, "Out of memory\n"); + return -1; + } + ctx->num_segs = 0; + seg = ctx->segments; + + fprintf(stderr, "Loading usable ELF segments:\n"); + for (int i = 0; i < num_phdrs; i++) { + if (le32(phdr->p_type) != PT_LOAD) { + phdr++; + continue; + } + uint32_t vaddr = le32(phdr->p_vaddr); + uint32_t paddr = le32(phdr->p_paddr); + uint32_t filesz = le32(phdr->p_filesz); + uint32_t memsz = le32(phdr->p_memsz); + uint32_t offset = le32(phdr->p_offset); + uint32_t flags = le32(phdr->p_flags); + if (!filesz) { + phdr++; + continue; + } + fprintf(stderr, "%d: V 0x%08x P 0x%08x (0x%08x->0x%08x) [%c%c%c] @0x%x\n", + i, vaddr, paddr, filesz, memsz, + flags & PF_R ? 'R' : ' ', + flags & PF_W ? 'W' : ' ', + flags & PF_X ? 'X' : ' ', + offset); + if (filesz != memsz) { + fprintf(stderr, "Error: PHDR file size does not equal memory size\n" + "(DATA+BSS PHDRs do not make sense on ROM platforms!)\n"); + return -1; + } + if (paddr < last_end) { + fprintf(stderr, "Error: PHDRs not sorted or overlap\n"); + return -1; + } + if (paddr < FLASH_START || (paddr+filesz) > FLASH_END) { + fprintf(stderr, "Error: PHDR is not contained in Flash\n"); + return -1; + } + if (vaddr >= FLASH_START && vaddr < FLASH_END && (flags & PF_W)) { + fprintf(stderr, "Error: Flash VMA segment is writable\n"); + return -1; + } + + uint8_t *data; + // make extra space if we need to move the data forward + data = malloc(filesz + BLOCK_SIZE); + if (!data) { + fprintf(stderr, "Out of memory\n"); + return -1; + } + if (fseek(fd, offset, SEEK_SET) < 0 || fread(data, 1, filesz, fd) != filesz) { + fprintf(stderr, "Error while reading PHDR payload\n"); + free(data); + return -1; + } + + uint32_t block_offset = paddr & (BLOCK_SIZE-1); + if (block_offset) { + if (ctx->num_segs) { + flash_seg_t *prev_seg = seg - 1; + uint32_t this_end = paddr + filesz; + uint32_t this_firstblock = paddr & ~(BLOCK_SIZE-1); + uint32_t prev_lastblock = (last_end - 1) & ~(BLOCK_SIZE-1); + + if (this_firstblock == prev_lastblock) { + uint32_t new_length = this_end - prev_seg->start; + uint32_t this_offset = paddr - prev_seg->start; + uint32_t hole = this_offset - prev_seg->length; + uint8_t *new_data = malloc(new_length); + if (!new_data) { + fprintf(stderr, "Out of memory\n"); + free(data); + return -1; + } + memset(new_data, 0xff, new_length); + memcpy(new_data, prev_seg->data, prev_seg->length); + memcpy(new_data + this_offset, data, filesz); + fprintf(stderr, "Note: Extending previous segment from 0x%x to 0x%x bytes\n", + prev_seg->length, new_length); + if (hole) + fprintf(stderr, "Note: 0x%x-byte hole created\n", hole); + free(data); + free(prev_seg->data); + prev_seg->data = new_data; + prev_seg->length = new_length; + last_end = this_end; + phdr++; + continue; + } + } + fprintf(stderr, "Warning: segment does not begin on a block boundary, will pad\n"); + memmove(data + block_offset, data, filesz); + memset(data, 0xFF, block_offset); + filesz += block_offset; + paddr -= block_offset; + } + + seg->data = data; + seg->start = paddr; + seg->length = filesz; + seg++; + ctx->num_segs++; + + last_end = paddr + filesz; + phdr++; + } + return 0; } -struct partition partitions[] = { - {0x100000, 0x102000, 1, "bootrom"}, - {0x102000, 0x110000, 0, "fpga"}, - {0x110000, 0x140000, 0, "os"}, - {0, 0, 0, NULL}, -}; +// Sanity check segments and check for bootloader writes +static int check_segs(flash_file_t *ctx, int can_write_bl) { + for (int i = 0; i < ctx->num_segs; i++) { + flash_seg_t *seg = &ctx->segments[i]; + + if (seg->start & (BLOCK_SIZE-1)) { + fprintf(stderr, "Error: Segment is not aligned\n"); + return -1; + } + if (seg->start < FLASH_START) { + fprintf(stderr, "Error: Segment is outside of flash bounds\n"); + return -1; + } + if (seg->start + seg->length > FLASH_END) { + fprintf(stderr, "Error: Segment is outside of flash bounds\n"); + return -1; + } + if (!can_write_bl && seg->start < BOOTLOADER_END) { + fprintf(stderr, "Attempted to write bootloader but bootloader writes are not enabled\n"); + return -1; + } + } + return 0; +} -void WriteBlock(unsigned int block_start, unsigned int len, unsigned char *buf) +// Load an ELF file and prepare it for flashing +int flash_load(flash_file_t *ctx, const char *name, int can_write_bl) { - unsigned char temp_buf[256]; - if (block_start & 0xFF) { - printf("moving stuff forward by %d bytes\n", block_start & 0xFF); - memset(temp_buf, 0xFF, block_start & 0xFF); - memcpy(temp_buf + (block_start & 0xFF), buf, len - (block_start & 0xFF)); - block_start &= ~0xFF; - } else { - memcpy(temp_buf, buf, len); - } - - UsbCommand c = {CMD_SETUP_WRITE}; - - for (int i = 0; i < 240; i += 48) { - memcpy(c.d.asBytes, temp_buf+i, 48); - c.arg[0] = (i/4); - SendCommand(&c); - WaitForAck(); - } - - c.cmd = CMD_FINISH_WRITE; - c.arg[0] = block_start; - -// printf("writing block %08x\r", c.arg[0]); - memcpy(c.d.asBytes, temp_buf+240, 16); + FILE *fd = NULL; + Elf32_Ehdr ehdr; + Elf32_Phdr *phdrs = NULL; + int num_phdrs; + int res; + + fd = fopen(name, "rb"); + if (!fd) { + fprintf(stderr, "Could not open file '%s': ", name); + perror(NULL); + goto fail; + } + + fprintf(stderr, "Loading ELF file '%s'...\n", name); + + if (fread(&ehdr, sizeof(ehdr), 1, fd) != 1) { + fprintf(stderr, "Error while reading ELF file header\n"); + goto fail; + } + if (memcmp(ehdr.e_ident, elf_ident, sizeof(elf_ident)) + || le32(ehdr.e_version) != 1) + { + fprintf(stderr, "Not an ELF file or wrong ELF type\n"); + goto fail; + } + if (le16(ehdr.e_type) != ET_EXEC) { + fprintf(stderr, "ELF is not executable\n"); + goto fail; + } + if (le16(ehdr.e_machine) != EM_ARM) { + fprintf(stderr, "Wrong ELF architecture\n"); + goto fail; + } + if (!ehdr.e_phnum || !ehdr.e_phoff) { + fprintf(stderr, "ELF has no PHDRs\n"); + goto fail; + } + if (le16(ehdr.e_phentsize) != sizeof(Elf32_Phdr)) { + // could be a structure padding issue... + fprintf(stderr, "Either the ELF file or this code is made of fail\n"); + goto fail; + } + num_phdrs = le16(ehdr.e_phnum); + + phdrs = malloc(le16(ehdr.e_phnum) * sizeof(Elf32_Phdr)); + if (!phdrs) { + fprintf(stderr, "Out of memory\n"); + goto fail; + } + if (fseek(fd, le32(ehdr.e_phoff), SEEK_SET) < 0) { + fprintf(stderr, "Error while reading ELF PHDRs\n"); + goto fail; + } + if (fread(phdrs, sizeof(Elf32_Phdr), num_phdrs, fd) != num_phdrs) { + fprintf(stderr, "Error while reading ELF PHDRs\n"); + goto fail; + } + + res = build_segs_from_phdrs(ctx, fd, phdrs, num_phdrs); + if (res < 0) + goto fail; + res = check_segs(ctx, can_write_bl); + if (res < 0) + goto fail; + + free(phdrs); + fclose(fd); + ctx->filename = name; + return 0; + +fail: + if (phdrs) + free(phdrs); + if (fd) + fclose(fd); + flash_free(ctx); + return -1; +} + +// Get the state of the proxmark, backwards compatible +static int get_proxmark_state(uint32_t *state) +{ + UsbCommand c; + c.cmd = CMD_DEVICE_INFO; SendCommand(&c); - WaitForAck(); + UsbCommand resp; + ReceiveCommand(&resp); + + // Three outcomes: + // 1. The old bootrom code will ignore CMD_DEVICE_INFO, but respond with an ACK + // 2. The old os code will respond with CMD_DEBUG_PRINT_STRING and "unknown command" + // 3. The new bootrom and os codes will respond with CMD_DEVICE_INFO and flags + + switch (resp.cmd) { + case CMD_ACK: + *state = DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM; + break; + case CMD_DEBUG_PRINT_STRING: + *state = DEVICE_INFO_FLAG_CURRENT_MODE_OS; + break; + case CMD_DEVICE_INFO: + *state = resp.arg[0]; + break; + default: + fprintf(stderr, "Error: Couldn't get proxmark state, bad response type: 0x%04" PRIx64 "\n", resp.cmd); + return -1; + break; + } + + return 0; } -void LoadFlashFromFile(const char *file, int start_addr, int end_addr) +// Enter the bootloader to be able to start flashing +static int enter_bootloader(char *serial_port_name) { - FILE *f = fopen(file, "rb"); - if (!f) { - printf("couldn't open file\n"); - exit(-1); - } - - char buf[4]; - fread(buf, 1, 4, f); - if (memcmp(buf, "\x7F" "ELF", 4) == 0) { - fseek(f, 0, SEEK_SET); - Elf32_Ehdr header; - fread(&header, 1, sizeof(header), f); - int count = header.e_phnum; - printf("count=%d phoff=%x\n", count, header.e_phoff); - Elf32_Phdr phdr; - - for (int i = 0; i < header.e_phnum; ++i) { - fseek(f, header.e_phoff + i * sizeof(Elf32_Phdr), SEEK_SET); - fread(&phdr, 1, sizeof(phdr), f); - printf("type=%d offset=%x paddr=%x vaddr=%x filesize=%x memsize=%x flags=%x align=%x\n", - phdr.p_type, phdr.p_offset, phdr.p_paddr, phdr.p_vaddr, phdr.p_filesz, phdr.p_memsz, phdr.p_flags, phdr.p_align); - if (phdr.p_type == PT_LOAD && phdr.p_filesz > 0 && phdr.p_paddr >= PHYSICAL_FLASH_START - && (phdr.p_paddr + phdr.p_filesz) < PHYSICAL_FLASH_END) { - printf("flashing offset=%x paddr=%x size=%x\n", phdr.p_offset, phdr.p_paddr, phdr.p_filesz); - if (phdr.p_offset == 0) { - printf("skipping forward 0x2000 because of strange linker thing\n"); - phdr.p_offset += 0x2000; - phdr.p_paddr += 0x2000; - phdr.p_filesz -= 0x2000; - phdr.p_memsz -= 0x2000; - } - - fseek(f, phdr.p_offset, SEEK_SET); - ExpectedAddr = phdr.p_paddr; - while (ExpectedAddr < (phdr.p_paddr + phdr.p_filesz)) { - unsigned int bytes_to_read = phdr.p_paddr + phdr.p_filesz - ExpectedAddr; - if (bytes_to_read > 256) - bytes_to_read=256; - else - memset(QueuedToSend, 0xFF, 256); - fread(QueuedToSend, 1, bytes_to_read, f); - printf("WriteBlock(%x, %d, %02x %02x %02x %02x %02x %02x %02x %02x ... %02x %02x %02x %02x %02x %02x %02x %02x)\n", ExpectedAddr, bytes_to_read, - QueuedToSend[0], QueuedToSend[1], QueuedToSend[2], QueuedToSend[3], - QueuedToSend[4], QueuedToSend[5], QueuedToSend[6], QueuedToSend[7], - QueuedToSend[248], QueuedToSend[249], QueuedToSend[250], QueuedToSend[251], - QueuedToSend[252], QueuedToSend[253], QueuedToSend[254], QueuedToSend[255]); - WriteBlock(ExpectedAddr, 256, QueuedToSend); - ExpectedAddr += bytes_to_read; - } - } - } - - fclose(f); - printf("\ndone.\n"); - return; - } else printf("Bad file format\n"); + uint32_t state; + + if (get_proxmark_state(&state) < 0) + return -1; + + if (state & DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM) { + /* Already in flash state, we're done. */ + return 0; + } + + if (state & DEVICE_INFO_FLAG_CURRENT_MODE_OS) { + fprintf(stderr,"Entering bootloader...\n"); + UsbCommand c; + memset(&c, 0, sizeof (c)); + + if ((state & DEVICE_INFO_FLAG_BOOTROM_PRESENT) + && (state & DEVICE_INFO_FLAG_OSIMAGE_PRESENT)) + { + // New style handover: Send CMD_START_FLASH, which will reset the board + // and enter the bootrom on the next boot. + c.cmd = CMD_START_FLASH; + SendCommand(&c); + fprintf(stderr,"(Press and release the button only to abort)\n"); + } else { + // Old style handover: Ask the user to press the button, then reset the board + c.cmd = CMD_HARDWARE_RESET; + SendCommand(&c); + fprintf(stderr,"Press and hold down button NOW if your bootloader requires it.\n"); + } + msleep(100); + CloseProxmark(); + + fprintf(stderr,"Waiting for Proxmark to reappear on %s",serial_port_name); + do { + sleep(1); + fprintf(stderr, "."); + } while (!OpenProxmark(0)); + fprintf(stderr," Found.\n"); + + return 0; + } + + fprintf(stderr, "Error: Unknown Proxmark mode\n"); + return -1; } -int PrepareFlash(struct partition *p, const char *filename, unsigned int state) +static int wait_for_ack(void) { - if (state & DEVICE_INFO_FLAG_UNDERSTANDS_START_FLASH) { - UsbCommand c = {CMD_START_FLASH, {p->start, p->end, 0}}; - - /* Only send magic when flashing bootrom */ - if (p->precious) - c.arg[2] = START_FLASH_MAGIC; - else - c.arg[2] = 0; - - SendCommand(&c); - WaitForAck(); - } else { - fprintf(stderr, "Warning: Your bootloader does not understand the new START_FLASH command\n"); - fprintf(stderr, " It is recommended that you update your bootloader\n\n"); - exit(0); - } - - LoadFlashFromFile(filename, p->start, p->end); - return 1; + UsbCommand ack; + ReceiveCommand(&ack); + if (ack.cmd != CMD_ACK) { + printf("Error: Unexpected reply 0x%04" PRIx64 " (expected ACK)\n", ack.cmd); + return -1; + } + return 0; } -unsigned int GetProxmarkState(void) +// Go into flashing mode +int flash_start_flashing(int enable_bl_writes,char *serial_port_name) { - unsigned int state = 0; + uint32_t state; + + if (enter_bootloader(serial_port_name) < 0) + return -1; + + if (get_proxmark_state(&state) < 0) + return -1; + + if (state & DEVICE_INFO_FLAG_UNDERSTANDS_START_FLASH) { + // This command is stupid. Why the heck does it care which area we're + // flashing, as long as it's not the bootloader area? The mind boggles. + UsbCommand c = {CMD_START_FLASH}; + + if (enable_bl_writes) { + c.arg[0] = FLASH_START; + c.arg[1] = FLASH_END; + c.arg[2] = START_FLASH_MAGIC; + } else { + c.arg[0] = BOOTLOADER_END; + c.arg[1] = FLASH_END; + c.arg[2] = 0; + } + SendCommand(&c); + return wait_for_ack(); + } else { + fprintf(stderr, "Note: Your bootloader does not understand the new START_FLASH command\n"); + fprintf(stderr, " It is recommended that you update your bootloader\n\n"); + } + + return 0; +} + +static int write_block(uint32_t address, uint8_t *data, uint32_t length) +{ + uint8_t block_buf[BLOCK_SIZE]; + memset(block_buf, 0xFF, BLOCK_SIZE); + memcpy(block_buf, data, length); UsbCommand c; - c.cmd = CMD_DEVICE_INFO; + c.cmd = CMD_FINISH_WRITE; + c.arg[0] = address; + memcpy(c.d.asBytes, block_buf, length); SendCommand(&c); - - UsbCommand resp; - ReceiveCommand(&resp); - /* Three cases: - * 1. The old bootrom code will ignore CMD_DEVICE_INFO, but respond with an ACK - * 2. The old os code will respond with CMD_DEBUG_PRINT_STRING and "unknown command" - * 3. The new bootrom and os codes will respond with CMD_DEVICE_INFO and flags - */ - - switch (resp.cmd) { - case CMD_ACK: - state = DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM; - break; - case CMD_DEBUG_PRINT_STRING: - state = DEVICE_INFO_FLAG_CURRENT_MODE_OS; - break; - case CMD_DEVICE_INFO: - state = resp.arg[0]; - break; - default: - fprintf(stderr, "Couldn't get proxmark state, bad response type: 0x%04X\n", resp.cmd); - exit(-1); - break; - } - - return state; + return wait_for_ack(); } -unsigned int EnterFlashState(void) +// Write a file's segments to Flash +int flash_write(flash_file_t *ctx) { - unsigned int state = GetProxmarkState(); - - if (state & DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM) { - /* Already in flash state, we're done. */ - return state; - } - - if (state & DEVICE_INFO_FLAG_CURRENT_MODE_OS) { - fprintf(stderr,"Entering flash-mode...\n"); - UsbCommand c; - memset(&c, 0, sizeof (c)); - - if ((state & DEVICE_INFO_FLAG_BOOTROM_PRESENT) && (state & DEVICE_INFO_FLAG_OSIMAGE_PRESENT)) { - /* New style handover: Send CMD_START_FLASH, which will reset the board and - * enter the bootrom on the next boot. - */ - c.cmd = CMD_START_FLASH; - SendCommand(&c); - fprintf(stderr,"(You don't have to do anything. Press and release the button only if you want to abort)\n"); - fprintf(stderr,"Waiting for Proxmark to reappear on USB... "); - } else { - /* Old style handover: Ask the user to press the button, then reset the board */ - c.cmd = CMD_HARDWARE_RESET; - SendCommand(&c); - fprintf(stderr,"(Press and hold down button NOW if your bootloader requires it)\n"); - fprintf(stderr,"Waiting for Proxmark to reappear on USB... "); - } - - CloseProxmark(); - sleep(1); - - while (!OpenProxmark(0)) { sleep(1); } - fprintf(stderr,"Found.\n"); - - return GetProxmarkState(); - } - - return 0; + fprintf(stderr, "Writing segments for file: %s\n", ctx->filename); + for (int i = 0; i < ctx->num_segs; i++) { + flash_seg_t *seg = &ctx->segments[i]; + + uint32_t length = seg->length; + uint32_t blocks = (length + BLOCK_SIZE - 1) / BLOCK_SIZE; + uint32_t end = seg->start + length; + + fprintf(stderr, " 0x%08x..0x%08x [0x%x / %d blocks]", + seg->start, end - 1, length, blocks); + + int block = 0; + uint8_t *data = seg->data; + uint32_t baddr = seg->start; + + while (length) { + uint32_t block_size = length; + if (block_size > BLOCK_SIZE) + block_size = BLOCK_SIZE; + + if (write_block(baddr, data, block_size) < 0) { + fprintf(stderr, " ERROR\n"); + fprintf(stderr, "Error writing block %d of %d\n", block, blocks); + return -1; + } + + data += block_size; + baddr += block_size; + length -= block_size; + block++; + fprintf(stderr, "."); + } + fprintf(stderr, " OK\n"); + } + return 0; } -/* On first call, have *offset = -1, *length = 0; */ -int find_next_area(const char *str, int *offset, int *length) +// free a file context +void flash_free(flash_file_t *ctx) { - if (*str == '\0') return 0; - if ((*offset >= 0) && str[*offset + *length] == '\0') return 0; - *offset += 1 + *length; - - char *next_comma = strchr(str + *offset, ','); - if (next_comma == NULL) { - *length = strlen(str) - *offset; - } else { - *length = next_comma-(str+*offset); - } - return 1; + if (!ctx) + return; + if (ctx->segments) { + for (int i = 0; i < ctx->num_segs; i++) + free(ctx->segments[i].data); + free(ctx->segments); + ctx->segments = NULL; + ctx->num_segs = 0; + } } -void do_flash(char **argv) -{ - unsigned int state = EnterFlashState(); - - if (!(state & DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM)) { - fprintf(stderr, "Proxmark would not enter flash state, abort\n"); - exit(-1); - } - - int offset=-1, length=0; - int current_area = 0; - while (find_next_area(argv[1], &offset, &length)) { - struct partition *p = NULL; - for (int i = 0; i<sizeof(partitions)/sizeof(partitions[0]); ++i) { - if (strncmp(partitions[i].name, argv[1] + offset, length) == 0) { - /* Check if the name matches the bootrom partition, and if so, require "bootrom" to - * be written in full. The other names may be abbreviated. - */ - if(!partitions[i].precious || (strlen(partitions[i].name) == length)) - p = &partitions[i]; - break; - } - } - - if (p == NULL) { - fprintf(stderr, "Warning: area name '"); - fwrite(argv[1]+offset, length, 1, stderr); - fprintf(stderr, "' unknown, ignored\n"); - } else { - fprintf(stderr, "Flashing %s from %s\n", p->name, argv[2+current_area]); - PrepareFlash(p, argv[2+current_area], state); - } - current_area++; - } +// just reset the unit +int flash_stop_flashing(void) { + UsbCommand c = {CMD_HARDWARE_RESET}; + SendCommand(&c); + msleep(100); + return 0; }