File size: 3,552 Bytes
d8afd6f
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
// Floppy image on raw flash partition "dosdisk"; writes are kept in a RAM overlay (lost at reset).
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "esp_partition.h"
#include "diskpart.h"

namespace {
struct Disk {
  const esp_partition_t * part;
  uint32_t size;
  uint32_t pos;
  uint8_t * pool;
  uint16_t  poolSlots, used;
  uint16_t * map;     // sector -> slot+1 (0 = not overlaid)
  uint32_t reads;
};

ssize_t dread(void * c, char * buf, size_t n)
{
  Disk * d = (Disk*)c;
  if (d->pos >= d->size) return 0;
  if (n > d->size - d->pos) n = d->size - d->pos;
  size_t done = 0;
  while (done < n) {
    uint32_t sec = (d->pos + done) / 512, off = (d->pos + done) % 512;
    size_t chunk = 512 - off; if (chunk > n - done) chunk = n - done;
    if (d->map[sec]) {
      memcpy(buf + done, d->pool + (uint32_t)(d->map[sec] - 1) * 512 + off, chunk);
    } else {
      // coalesce following non-overlaid sectors into one flash read
      size_t span = chunk;
      uint32_t s2 = sec + 1;
      while (done + span < n && !d->map[s2]) { size_t add = n - done - span; if (add > 512) add = 512; span += add; ++s2; }
      if (esp_partition_read(d->part, d->pos + done, buf + done, span) != ESP_OK) return -1;
      chunk = span;
    }
    done += chunk;
  }
  if (d->reads < 12) { printf("[disk] read off=%u len=%u\n", (unsigned)d->pos, (unsigned)n); }
  ++d->reads;
  d->pos += n;
  return n;
}

ssize_t dwrite(void * c, const char * buf, size_t n)
{
  Disk * d = (Disk*)c;
  if (d->pos >= d->size) return 0;
  if (n > d->size - d->pos) n = d->size - d->pos;
  size_t done = 0;
  while (done < n) {
    uint32_t sec = (d->pos + done) / 512, off = (d->pos + done) % 512;
    size_t chunk = 512 - off; if (chunk > n - done) chunk = n - done;
    if (!d->map[sec]) {
      if (d->used >= d->poolSlots) return done ? (d->pos += done, done) : -1;  // overlay full
      uint8_t * slot = d->pool + (uint32_t)d->used * 512;
      if (esp_partition_read(d->part, sec * 512, slot, 512) != ESP_OK) return -1;
      d->map[sec] = ++d->used;
    }
    memcpy(d->pool + (uint32_t)(d->map[sec] - 1) * 512 + off, buf + done, chunk);
    done += chunk;
  }
  d->pos += n;
  return n;
}

int dseek(void * c, off_t * off, int whence)
{
  Disk * d = (Disk*)c;
  int64_t p = *off;
  if (whence == SEEK_CUR) p += d->pos; else if (whence == SEEK_END) p += d->size;
  if (p < 0) return -1;
  d->pos = (uint32_t)p; *off = p;
  return 0;
}

int dclose(void * c) { Disk * d = (Disk*)c; free(d->map); free(d); return 0; }
}

FILE * openPartitionDisk(uint8_t * overlayMem, uint32_t overlaySize, uint64_t * diskSize)
{
  const esp_partition_t * p = esp_partition_find_first(ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_ANY, "dosdisk");
  if (!p) { printf("[disk] partition 'dosdisk' NOT FOUND\n"); return nullptr; }
  Disk * d = (Disk*)calloc(1, sizeof(Disk));
  d->part = p;
  d->size = p->size > 1474560 ? 1474560 : p->size;   // 1.44MB floppy
  d->pool = overlayMem;
  d->poolSlots = overlaySize / 512;
  d->map = (uint16_t*)calloc(d->size / 512, sizeof(uint16_t));
  uint8_t bs[512];
  esp_partition_read(p, 0, bs, 512);
  printf("[disk] partition dosdisk @0x%x size=0x%x, boot sector: %02X %02X %02X ... sig=%02X%02X\n",
         (unsigned)p->address, (unsigned)p->size, bs[0], bs[1], bs[2], bs[510], bs[511]);
  cookie_io_functions_t f = { dread, dwrite, dseek, dclose };
  FILE * fp = fopencookie(d, "r+", f);
  if (fp) setvbuf(fp, (char*)malloc(512), _IOFBF, 512);
  *diskSize = d->size;
  return fp;
}