int sector_size_for_device(const char *device)
{
int fd = open(device, O_RDONLY);
int r;
if(fd < 0)
return -EINVAL;
r = sector_size(fd);
close(fd);
return r;
}
ssize_t write_blockwise(int fd, void *orig_buf, size_t count)
{
void *hangover_buf, *hangover_buf_base = NULL;
void *buf, *buf_base = NULL;
int r, hangover, solid, bsize, alignment;
ssize_t ret = -1;
if ((bsize = sector_size(fd)) < 0)
return bsize;
hangover = count % bsize;
solid = count - hangover;
alignment = get_alignment(fd);
if ((long)orig_buf & (alignment - 1)) {
buf = aligned_malloc(&buf_base, count, alignment);
if (!buf)
goto out;
memcpy(buf, orig_buf, count);
} else
buf = orig_buf;
r = write(fd, buf, solid);
if (r < 0 || r != solid)
goto out;
if (hangover) {
hangover_buf = aligned_malloc(&hangover_buf_base, bsize, alignment);
if (!hangover_buf)
goto out;
r = read(fd, hangover_buf, bsize);
if (r < 0 || r != bsize)
goto out;
r = lseek(fd, -bsize, SEEK_CUR);
if (r < 0)
goto out;
memcpy(hangover_buf, (char*)buf + solid, hangover);
r = write(fd, hangover_buf, bsize);
if (r < 0 || r != bsize)
goto out;
}
ret = count;
out:
free(hangover_buf_base);
if (buf != orig_buf)
free(buf_base);
return ret;
}
/*
* Combines llseek with blockwise write. write_blockwise can already deal with short writes
* but we also need a function to deal with short writes at the start. But this information
* is implicitly included in the read/write offset, which can not be set to non-aligned
* boundaries. Hence, we combine llseek with write.
*/
ssize_t write_lseek_blockwise(int fd, char *buf, size_t count, off_t offset) {
char *frontPadBuf;
void *frontPadBuf_base = NULL;
int r, bsize, frontHang;
size_t innerCount = 0;
ssize_t ret = -1;
if ((bsize = sector_size(fd)) < 0)
return bsize;
frontHang = offset % bsize;
if (lseek(fd, offset - frontHang, SEEK_SET) < 0)
goto out;
if (frontHang) {
frontPadBuf = aligned_malloc(&frontPadBuf_base,
bsize, get_alignment(fd));
if (!frontPadBuf)
goto out;
r = read(fd, frontPadBuf, bsize);
if (r < 0 || r != bsize)
goto out;
innerCount = bsize - frontHang;
if (innerCount > count)
innerCount = count;
memcpy(frontPadBuf + frontHang, buf, innerCount);
if (lseek(fd, offset - frontHang, SEEK_SET) < 0)
goto out;
r = write(fd, frontPadBuf, bsize);
if (r < 0 || r != bsize)
goto out;
buf += innerCount;
count -= innerCount;
}
ret = count ? write_blockwise(fd, buf, count) : 0;
if (ret >= 0)
ret += innerCount;
out:
free(frontPadBuf_base);
return ret;
}
bool mf_read_tag_internal(mf_tag_t* tag,
const mf_tag_t* keys, mf_key_type_t key_type) {
mifare_param mp;
static mf_tag_t buffer_tag;
clear_tag(&buffer_tag);
int error = 0;
printf("Reading: ["); fflush(stdout);
// Read the card from end to begin
for (int block_it = (int)block_count(size) - 1; block_it >= 0; --block_it) {
size_t block = (size_t)block_it;
// Authenticate everytime we reach a trailer block
if (is_trailer_block(block)) {
// Try to authenticate for the current sector
uint8_t* key = key_from_tag(keys, key_type, block);
if (!mf_authenticate(block, key, key_type)) {
// Progress indication and error report
printf("0x%02zx", block_to_sector(block));
if (block != 3) printf(".");
fflush(stdout);
block_it -= (int)sector_size(block) - 1; // Skip the rest of the sector blocks
error = 1;
}
else {
// Try to read the trailer (only to *read* the access bits)
if (nfc_initiator_mifare_cmd(device, MC_READ, (uint8_t)block, &mp)) {
// Copy the keys over to our tag buffer
key_to_tag(&buffer_tag, keys->amb[block].mbt.abtKeyA, MF_KEY_A, block);
key_to_tag(&buffer_tag, keys->amb[block].mbt.abtKeyB, MF_KEY_B, block);
// Store the retrieved access bits in the tag buffer
memcpy(buffer_tag.amb[block].mbt.abtAccessBits,
mp.mpd.abtData + 6, 4);
} else {
printf ("\nUnable to read trailer block: 0x%02zx.\n", block);
return false;
}
printf("."); fflush(stdout); // Progress indicator
}
}
else { // I.e. not a sector trailer
// Try to read out the block
if (!nfc_initiator_mifare_cmd(device, MC_READ, (uint8_t)block, &mp)) {
printf("\nUnable to read block: 0x%02zx.\n", block);
return false;
}
memcpy(buffer_tag.amb[block].mbd.abtData, mp.mpd.abtData, 0x10);
}
}
// Terminate progress indicator
if (error)
printf("] Auth errors in indicated sectors.\n");
else
printf("] Success!\n");
// Success! Copy the data
// todo: Or return static ptr?
memcpy(tag, &buffer_tag, MF_4K);
return true;
}
static int
measured_exec(const char *device, char *const *args)
{
struct disk_stats stats0;
struct disk_stats stats1;
char stat_path[PATH_MAX];
uint64_t time0;
uint64_t time1;
uint32_t bytes;
uint32_t milli_io;
float milli_tot;
float seconds;
int status;
pid_t pid;
uint16_t sector_sz;
if (find_device_stat(device, stat_path) == -1)
return -1;
if ((sector_sz = sector_size(device)) == -1) {
fprintf(stderr, "%s: failed to determine sector size: %s\n",
prog, strerror(errno));
return -1;
}
if (drop_cache() == -1)
return -1;
/* collect opening data */
if (read_disk_stats(prog, stat_path, &stats0) == -1)
return -1;
time0 = clock_monotonic();
/* start child */
if ((pid = fork()) == -1) {
fprintf(stderr, "%s: fork failed: %s\n",
prog, strerror(errno));
return -1;
} else if (!pid) {
execvp(*args, args);
fprintf(stderr, "%s: exec failed: %s\n",
prog, strerror(errno));
exit(0xff);
}
/* wait for child */
status = 0;
for (;;) {
if (waitpid(pid, &status, 0) == -1) {
if (errno == EINTR) continue;
fprintf(stderr, "%s: wait failed: %s\n",
prog, strerror(errno));
return -1;
}
break;
}
/* collect closing data */
if (read_disk_stats(prog, stat_path, &stats1) == -1)
return -1;
time1 = clock_monotonic();
/* check for child error */
if (WIFSIGNALED(status)) {
fprintf(stderr, "%s: child terminated by signal %s\n",
prog, signame(WTERMSIG(status)));
return -1;
} else if (WEXITSTATUS(status)) {
if (WEXITSTATUS(status) != 0xff)
fprintf(stderr, "%s: child exited with %d\n",
prog, WEXITSTATUS(status));
return -1;
}
/* process data */
bytes = ((stats1.sectors_read + stats1.sectors_written) -
(stats0.sectors_read + stats0.sectors_written)) * sector_sz;
milli_io = stats1.milli_io - stats0.milli_io;
milli_tot = (time1 - time0) / 1000000.0f;
seconds = (time1 - time0) / 1000000000.0f;
/* print data */
printf("RATE_KBPS\t%.10e\n", bytes / (seconds * 1024));
printf("RATE_MBPS\t%.10e\n", bytes / (seconds * 1048576));
printf("TIME_S\t\t%.10e\n", seconds);
printf("UTIL\t\t%.10e\n", milli_io / milli_tot);
return 0;
}
static size_t lpc2_flash_writeDirect(struct KBlock *blk, block_idx_t idx, const void *_buf, size_t offset, size_t size)
{
ASSERT(offset == 0);
ASSERT(FLASH_PAGE_SIZE_BYTES == size);
Flash *fls = FLASH_CAST(blk);
if (!(fls->blk.priv.flags & KB_WRITE_ONCE))
ASSERT(sector_size(idx) <= FLASH_PAGE_SIZE_BYTES);
const uint8_t *buf = (const uint8_t *)_buf;
cpu_flags_t flags;
//Compute page address of current page.
uint32_t addr = idx * blk->blk_size;
uint32_t sector = addr_to_sector(addr);
// Compute the first page index in the sector to manage the status
int idx_sector = sector_addr(sector) / blk->blk_size;
LOG_INFO("Writing page[%ld]sector[%ld]idx[%d]\n", idx, sector, idx_sector);
IRQ_SAVE_DISABLE(flags);
IapCmd cmd;
IapRes res;
cmd.cmd = PREPARE_SECTOR_FOR_WRITE;
cmd.param[0] = cmd.param[1] = sector;
iap(&cmd, &res);
if (res.status != CMD_SUCCESS)
goto flash_error;
if ((fls->blk.priv.flags & KB_WRITE_ONCE) &&
bitarray_isRangeFull(&lpc2_bitx, idx_sector, erase_group[sector]))
{
kputs("blocchi pieni\n");
ASSERT(0);
goto flash_error;
}
bool erase = false;
if ((fls->blk.priv.flags & KB_WRITE_ONCE) &&
bitarray_isRangeEmpty(&lpc2_bitx, idx_sector, erase_group[sector]))
erase = true;
if (!(fls->blk.priv.flags & KB_WRITE_ONCE))
erase = true;
if (erase)
{
cmd.cmd = ERASE_SECTOR;
cmd.param[0] = cmd.param[1] = sector;
cmd.param[2] = CPU_FREQ / 1000;
iap(&cmd, &res);
if (res.status != CMD_SUCCESS)
goto flash_error;
}
LOG_INFO("Writing page [%ld], addr [%ld] in sector[%ld]\n", idx, addr, sector);
cmd.cmd = PREPARE_SECTOR_FOR_WRITE;
cmd.param[0] = cmd.param[1] = sector;
iap(&cmd, &res);
if (res.status != CMD_SUCCESS)
goto flash_error;
if (fls->blk.priv.flags & KB_WRITE_ONCE)
{
if (bitarray_test(&lpc2_bitx, idx))
{
ASSERT(0);
goto flash_error;
}
else
bitarray_set(&lpc2_bitx, idx);
}
cmd.cmd = COPY_RAM_TO_FLASH;
cmd.param[0] = addr;
cmd.param[1] = (uint32_t)buf;
cmd.param[2] = FLASH_PAGE_SIZE_BYTES;
cmd.param[3] = CPU_FREQ / 1000;
iap(&cmd, &res);
if (res.status != CMD_SUCCESS)
goto flash_error;
IRQ_RESTORE(flags);
LOG_INFO("Done\n");
return blk->blk_size;
flash_error:
LOG_ERR("%ld\n", res.status);
fls->hw->status |= FLASH_WR_ERR;
return 0;
}
