135 lines
No EOL
2.4 KiB
C
135 lines
No EOL
2.4 KiB
C
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include "drive.h"
|
|
|
|
void memcpy(void *to, const void *from, uint32_t n) {
|
|
uint32_t *tp = to;
|
|
const uint32_t *fp = from;
|
|
while (n >= 4) {
|
|
*(tp++) = *(fp++);
|
|
n -= 4;
|
|
}
|
|
uint8_t *tpp = (uint8_t *)tp, *fpp = (uint8_t *)fp;
|
|
while (n--)
|
|
*(tpp++) = *(fpp++);
|
|
}
|
|
|
|
void fmcpy(void *to, const struct drive *d, drive_file_id_t f, uint32_t from, uint32_t n) {
|
|
uint8_t buf[512];
|
|
d->load_sector(d, f, from >> 9, buf);
|
|
uint16_t from_low = from & 511;
|
|
if (!((from_low + n) & ~511)) {
|
|
memcpy(to, buf + from_low, n);
|
|
return;
|
|
}
|
|
|
|
uint32_t i = 512 - from_low;
|
|
memcpy(to, buf + from_low, i);
|
|
|
|
n -= i;
|
|
uint32_t fsi = (from + i) >> 9;
|
|
|
|
while (n & ~511) {
|
|
d->load_sector(d, f, fsi, buf);
|
|
memcpy(to + i, buf, 512);
|
|
i += 512;
|
|
n -= 512;
|
|
++fsi;
|
|
}
|
|
|
|
d->load_sector(d, f, fsi, buf);
|
|
memcpy(to + i, buf, n);
|
|
}
|
|
|
|
void u32_dec(uint32_t n, char *b) {
|
|
if (!n) {
|
|
*(uint16_t *)b = (uint16_t)'0';
|
|
return;
|
|
}
|
|
bool zero = false;
|
|
for (uint32_t m = 1000000000; m; m /= 10) {
|
|
uint8_t d = (n / m) % 10;
|
|
if (zero)
|
|
*(b++) = d + '0';
|
|
else if (d) {
|
|
zero = true;
|
|
*(b++) = d + '0';
|
|
}
|
|
}
|
|
*b = '\0';
|
|
}
|
|
|
|
void u16_dec(uint16_t n, char *b) {
|
|
if (!n) {
|
|
*(uint16_t *)b = (uint16_t)'0';
|
|
return;
|
|
}
|
|
bool zero = false;
|
|
for (uint32_t m = 10000; m; m /= 10) {
|
|
uint8_t d = (n / m) % 10;
|
|
if (zero)
|
|
*(b++) = d + '0';
|
|
else if (d) {
|
|
zero = true;
|
|
*(b++) = d + '0';
|
|
}
|
|
}
|
|
*b = '\0';
|
|
}
|
|
|
|
void u8_dec(uint8_t n, char *b) {
|
|
if (!n) {
|
|
*(uint16_t *)b = (uint16_t)'0';
|
|
return;
|
|
}
|
|
bool zero = false;
|
|
for (uint32_t m = 100; m; m /= 10) {
|
|
uint8_t d = (n / m) % 10;
|
|
if (zero)
|
|
*(b++) = d + '0';
|
|
else if (d) {
|
|
zero = true;
|
|
*(b++) = d + '0';
|
|
}
|
|
}
|
|
*b = '\0';
|
|
}
|
|
|
|
void u32_hex(uint32_t n, char *b) {
|
|
uint8_t m = 28;
|
|
while (1) {
|
|
uint8_t d = (n >> m) & 0xf;
|
|
*(b++) = d >= 10 ? 'a' + d - 10 : '0' + d;
|
|
if (!m) {
|
|
*b = '\0';
|
|
return;
|
|
}
|
|
m -= 4;
|
|
}
|
|
}
|
|
|
|
void u16_hex(uint16_t n, char *b) {
|
|
uint8_t m = 12;
|
|
while (1) {
|
|
uint8_t d = (n >> m) & 0xf;
|
|
*(b++) = d >= 10 ? 'a' + d - 10 : '0' + d;
|
|
if (!m) {
|
|
*b = '\0';
|
|
return;
|
|
}
|
|
m -= 4;
|
|
}
|
|
}
|
|
|
|
void u8_hex(uint8_t n, char *b) {
|
|
uint8_t m = 4;
|
|
while (1) {
|
|
uint8_t d = (n >> m) & 0xf;
|
|
*(b++) = d >= 10 ? 'a' + d - 10 : '0' + d;
|
|
if (!m) {
|
|
*b = '\0';
|
|
return;
|
|
}
|
|
m -= 4;
|
|
}
|
|
} |