- 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);
- SendCommand(&c);
- WaitForAck();
+ 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;
+}
projects / proxmark3-svn / blobdiff