mirror of
https://github.com/polhenarejos/pico-keys-sdk
synced 2026-05-27 08:35:12 +02:00
Adding first release of Pico CCID. It should be the core for different types of smart cards.
This commit is contained in:
Pol Henarejos
294
src/fs/file.c
Normal file
294
src/fs/file.c
Normal file
@@ -0,0 +1,294 @@
|
||||
/*
|
||||
* This file is part of the Pico CCID distribution (https://github.com/polhenarejos/pico-ccid).
|
||||
* Copyright (c) 2022 Pol Henarejos.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, version 3.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "file.h"
|
||||
#include "tusb.h"
|
||||
#include "ccid2040.h"
|
||||
#include <string.h>
|
||||
|
||||
extern const uintptr_t end_data_pool;
|
||||
extern const uintptr_t start_data_pool;
|
||||
extern int flash_write_data_to_file(file_t *file, const uint8_t *data, uint16_t len);
|
||||
extern int flash_program_halfword (uintptr_t addr, uint16_t data);
|
||||
extern int flash_program_word (uintptr_t addr, uint32_t data);
|
||||
extern int flash_program_uintptr (uintptr_t addr, uintptr_t data);
|
||||
extern int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len);
|
||||
extern uintptr_t flash_read_uintptr(uintptr_t addr);
|
||||
extern uint16_t flash_read_uint16(uintptr_t addr);
|
||||
extern uint8_t flash_read_uint8(uintptr_t addr);
|
||||
extern uint8_t *flash_read(uintptr_t addr);
|
||||
extern void low_flash_available();
|
||||
|
||||
//puts FCI in the RAPDU
|
||||
void process_fci(const file_t *pe) {
|
||||
uint8_t *p = res_APDU;
|
||||
uint8_t buf[64];
|
||||
res_APDU_size = 0;
|
||||
res_APDU[res_APDU_size++] = 0x6f;
|
||||
res_APDU[res_APDU_size++] = 0x00; //computed later
|
||||
|
||||
res_APDU[res_APDU_size++] = 0x81;
|
||||
res_APDU[res_APDU_size++] = 2;
|
||||
if (pe->data) {
|
||||
if ((pe->type & FILE_DATA_FUNC) == FILE_DATA_FUNC) {
|
||||
uint16_t len = ((int (*)(const file_t *, int))(pe->data))(pe, 0);
|
||||
res_APDU[res_APDU_size++] = (len >> 8) & 0xff;
|
||||
res_APDU[res_APDU_size++] = len & 0xff;
|
||||
}
|
||||
else {
|
||||
res_APDU[res_APDU_size++] = pe->data[1];
|
||||
res_APDU[res_APDU_size++] = pe->data[0];
|
||||
}
|
||||
}
|
||||
else {
|
||||
memset(res_APDU+res_APDU_size, 0, 2);
|
||||
res_APDU_size += 2;
|
||||
}
|
||||
|
||||
res_APDU[res_APDU_size++] = 0x82;
|
||||
res_APDU[res_APDU_size++] = 1;
|
||||
res_APDU[res_APDU_size] = 0;
|
||||
if (pe->type == FILE_TYPE_INTERNAL_EF)
|
||||
res_APDU[res_APDU_size++] |= 0x08;
|
||||
else if (pe->type == FILE_TYPE_WORKING_EF)
|
||||
res_APDU[res_APDU_size++] |= pe->ef_structure & 0x7;
|
||||
else if (pe->type == FILE_TYPE_DF)
|
||||
res_APDU[res_APDU_size++] |= 0x38;
|
||||
|
||||
res_APDU[res_APDU_size++] = 0x83;
|
||||
res_APDU[res_APDU_size++] = 2;
|
||||
put_uint16_t(pe->fid, res_APDU+res_APDU_size);
|
||||
res_APDU_size += 2;
|
||||
res_APDU[1] = res_APDU_size-2;
|
||||
}
|
||||
|
||||
#define MAX_DYNAMIC_FILES 64
|
||||
uint16_t dynamic_files = 0;
|
||||
file_t dynamic_file[MAX_DYNAMIC_FILES];
|
||||
|
||||
bool card_terminated = false;
|
||||
|
||||
bool is_parent(const file_t *child, const file_t *parent) {
|
||||
if (child == parent)
|
||||
return true;
|
||||
if (child == MF)
|
||||
return false;
|
||||
return is_parent(&file_entries[child->parent], parent);
|
||||
}
|
||||
|
||||
file_t *get_parent(file_t *f) {
|
||||
return &file_entries[f->parent];
|
||||
}
|
||||
|
||||
file_t *search_by_name(uint8_t *name, uint16_t namelen) {
|
||||
for (file_t *p = file_entries; p != file_last; p++) {
|
||||
if (p->name && *p->name == apdu.cmd_apdu_data_len && memcmp(p->name+1, name, namelen) == 0) {
|
||||
return p;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
file_t *search_by_fid(const uint16_t fid, const file_t *parent, const uint8_t sp) {
|
||||
|
||||
for (file_t *p = file_entries; p != file_last; p++) {
|
||||
if (p->fid != 0x0000 && p->fid == fid) {
|
||||
if (!parent || (parent && is_parent(p, parent))) {
|
||||
if (!sp || sp == SPECIFY_ANY || (((sp & SPECIFY_EF) && (p->type & FILE_TYPE_INTERNAL_EF)) || ((sp & SPECIFY_DF) && p->type == FILE_TYPE_DF)))
|
||||
return p;
|
||||
}
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
uint8_t make_path_buf(const file_t *pe, uint8_t *buf, uint8_t buflen, const file_t *top) {
|
||||
if (!buflen)
|
||||
return 0;
|
||||
if (pe == top) //MF or relative DF
|
||||
return 0;
|
||||
put_uint16_t(pe->fid, buf);
|
||||
return make_path_buf(&file_entries[pe->parent], buf+2, buflen-2, top)+2;
|
||||
}
|
||||
|
||||
uint8_t make_path(const file_t *pe, const file_t *top, uint8_t *path) {
|
||||
uint8_t buf[MAX_DEPTH*2], *p = path;
|
||||
put_uint16_t(pe->fid, buf);
|
||||
uint8_t depth = make_path_buf(&file_entries[pe->parent], buf+2, sizeof(buf)-2, top)+2;
|
||||
for (int d = depth-2; d >= 0; d -= 2) {
|
||||
memcpy(p, buf+d, 2);
|
||||
p += 2;
|
||||
}
|
||||
return depth;
|
||||
}
|
||||
|
||||
file_t *search_by_path(const uint8_t *pe_path, uint8_t pathlen, const file_t *parent) {
|
||||
uint8_t path[MAX_DEPTH*2];
|
||||
if (pathlen > sizeof(path)) {
|
||||
return NULL;
|
||||
}
|
||||
for (file_t *p = file_entries; p != file_last; p++) {
|
||||
uint8_t depth = make_path(p, parent, path);
|
||||
if (pathlen == depth && memcmp(path, pe_path, depth) == 0)
|
||||
return p;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
file_t *currentEF = NULL;
|
||||
file_t *currentDF = NULL;
|
||||
const file_t *selected_applet = NULL;
|
||||
bool isUserAuthenticated = false;
|
||||
|
||||
bool authenticate_action(const file_t *ef, uint8_t op) {
|
||||
uint8_t acl = ef->acl[op];
|
||||
if (acl == 0x0)
|
||||
return true;
|
||||
else if (acl == 0xff)
|
||||
return false;
|
||||
else if (acl == 0x90 || acl & 0x9F == 0x10) {
|
||||
// PIN required.
|
||||
if(isUserAuthenticated) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void initialize_chain(file_chain_t **chain) {
|
||||
file_chain_t *next;
|
||||
for (file_chain_t *f = *chain; f; f = next) {
|
||||
next = f->next;
|
||||
free(f);
|
||||
}
|
||||
*chain = NULL;
|
||||
}
|
||||
|
||||
void initialize_flash(bool hard) {
|
||||
if (hard) {
|
||||
const uint8_t empty[8] = { 0 };
|
||||
flash_program_block(end_data_pool, empty, sizeof(empty));
|
||||
low_flash_available();
|
||||
}
|
||||
for (file_t *f = file_entries; f != file_last; f++) {
|
||||
if ((f->type & FILE_DATA_FLASH) == FILE_DATA_FLASH)
|
||||
f->data = NULL;
|
||||
}
|
||||
dynamic_files = 0;
|
||||
}
|
||||
|
||||
void scan_flash() {
|
||||
initialize_flash(false); //soft initialization
|
||||
if (*(uintptr_t *)end_data_pool == 0xffffffff && *(uintptr_t *)(end_data_pool+sizeof(uintptr_t)) == 0xffffffff)
|
||||
{
|
||||
printf("First initialization (or corrupted!)\r\n");
|
||||
const uint8_t empty[8] = { 0 };
|
||||
flash_program_block(end_data_pool, empty, sizeof(empty));
|
||||
//low_flash_available();
|
||||
//wait_flash_finish();
|
||||
}
|
||||
printf("SCAN\r\n");
|
||||
|
||||
uintptr_t base = flash_read_uintptr(end_data_pool);
|
||||
for (uintptr_t base = flash_read_uintptr(end_data_pool); base >= start_data_pool; base = flash_read_uintptr(base)) {
|
||||
if (base == 0x0) //all is empty
|
||||
break;
|
||||
|
||||
uint16_t fid = flash_read_uint16(base+sizeof(uintptr_t)+sizeof(uintptr_t));
|
||||
printf("[%x] scan fid %x, len %d\r\n",base,fid,flash_read_uint16(base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t)));
|
||||
file_t *file = (file_t *)search_by_fid(fid, NULL, SPECIFY_EF);
|
||||
if (file)
|
||||
file->data = (uint8_t *)(base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t));
|
||||
if (flash_read_uintptr(base) == 0x0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t *file_read(const uint8_t *addr) {
|
||||
return flash_read((uintptr_t)addr);
|
||||
}
|
||||
uint16_t file_read_uint16(const uint8_t *addr) {
|
||||
return flash_read_uint16((uintptr_t)addr);
|
||||
}
|
||||
uint8_t file_read_uint8(const uint8_t *addr) {
|
||||
return flash_read_uint8((uintptr_t)addr);
|
||||
}
|
||||
|
||||
file_t *search_dynamic_file(uint16_t fid) {
|
||||
for (int i = 0; i < dynamic_files; i++) {
|
||||
if (dynamic_file[i].fid == fid)
|
||||
return &dynamic_file[i];
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int delete_dynamic_file(file_t *f) {
|
||||
for (int i = 0; i < dynamic_files; i++) {
|
||||
if (dynamic_file[i].fid == f->fid) {
|
||||
for (int j = i+1; j < dynamic_files; j++)
|
||||
memcpy(&dynamic_file[j-1], &dynamic_file[j], sizeof(file_t));
|
||||
dynamic_files--;
|
||||
return CCID_OK;
|
||||
}
|
||||
}
|
||||
return CCID_ERR_FILE_NOT_FOUND;
|
||||
}
|
||||
|
||||
file_t *file_new(uint16_t fid) {
|
||||
file_t *f;
|
||||
if ((f = search_dynamic_file(fid)))
|
||||
return f;
|
||||
if (dynamic_files == MAX_DYNAMIC_FILES)
|
||||
return NULL;
|
||||
f = &dynamic_file[dynamic_files];
|
||||
dynamic_files++;
|
||||
file_t file = {
|
||||
.fid = fid,
|
||||
.parent = 5,
|
||||
.name = NULL,
|
||||
.type = FILE_TYPE_WORKING_EF,
|
||||
.ef_structure = FILE_EF_TRANSPARENT,
|
||||
.data = NULL,
|
||||
.acl = {0}
|
||||
};
|
||||
memcpy(f, &file, sizeof(file_t));
|
||||
//memset((uint8_t *)f->acl, 0x90, sizeof(f->acl));
|
||||
return f;
|
||||
}
|
||||
|
||||
file_chain_t *add_file_to_chain(file_t *file, file_chain_t **chain) {
|
||||
if (search_file_chain(file->fid, *chain))
|
||||
return NULL;
|
||||
file_chain_t *fc = (file_chain_t *)malloc(sizeof(file_chain_t));
|
||||
fc->file = file;
|
||||
fc->next = *chain;
|
||||
*chain = fc;
|
||||
return fc;
|
||||
}
|
||||
|
||||
file_t *search_file_chain(uint16_t fid, file_chain_t *chain) {
|
||||
for (file_chain_t *fc = chain; fc; fc = fc->next) {
|
||||
if (fid == fc->file->fid) {
|
||||
return fc->file;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
124
src/fs/file.h
Normal file
124
src/fs/file.h
Normal file
@@ -0,0 +1,124 @@
|
||||
/*
|
||||
* This file is part of the Pico CCID distribution (https://github.com/polhenarejos/pico-ccid).
|
||||
* Copyright (c) 2022 Pol Henarejos.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, version 3.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _FILE_H_
|
||||
#define _FILE_H_
|
||||
|
||||
#include <stdlib.h>
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#define FILE_TYPE_UNKNOWN 0x00
|
||||
#define FILE_TYPE_DF 0x04
|
||||
#define FILE_TYPE_INTERNAL_EF 0x03
|
||||
#define FILE_TYPE_WORKING_EF 0x01
|
||||
#define FILE_TYPE_BSO 0x10
|
||||
#define FILE_PERSISTENT 0x20
|
||||
#define FILE_DATA_FLASH 0x40
|
||||
#define FILE_DATA_FUNC 0x80
|
||||
|
||||
/* EF structures */
|
||||
#define FILE_EF_UNKNOWN 0x00
|
||||
#define FILE_EF_TRANSPARENT 0x01
|
||||
#define FILE_EF_LINEAR_FIXED 0x02
|
||||
#define FILE_EF_LINEAR_FIXED_TLV 0x03
|
||||
#define FILE_EF_LINEAR_VARIABLE 0x04
|
||||
#define FILE_EF_LINEAR_VARIABLE_TLV 0x05
|
||||
#define FILE_EF_CYCLIC 0x06
|
||||
#define FILE_EF_CYCLIC_TLV 0x07
|
||||
|
||||
#define ACL_OP_DELETE_SELF 0x00
|
||||
#define ACL_OP_CREATE_DF 0x01
|
||||
#define ACL_OP_CREATE_EF 0x02
|
||||
#define ACL_OP_DELETE_CHILD 0x03
|
||||
#define ACL_OP_WRITE 0x04
|
||||
#define ACL_OP_UPDATE_ERASE 0x05
|
||||
#define ACL_OP_READ_SEARCH 0x06
|
||||
|
||||
#define SPECIFY_EF 0x1
|
||||
#define SPECIFY_DF 0x2
|
||||
#define SPECIFY_ANY 0x3
|
||||
|
||||
#define EF_DKEK 0x108F
|
||||
#define EF_PRKDFS 0x6040
|
||||
#define EF_PUKDFS 0x6041
|
||||
#define EF_CDFS 0x6042
|
||||
#define EF_AODFS 0x6043
|
||||
#define EF_DODFS 0x6044
|
||||
#define EF_SKDFS 0x6045
|
||||
#define EF_DEVOPS 0x100E
|
||||
|
||||
#define MAX_DEPTH 4
|
||||
|
||||
typedef struct file
|
||||
{
|
||||
const uint16_t fid;
|
||||
const uint8_t parent; //entry number in the whole table!!
|
||||
const uint8_t *name;
|
||||
const uint8_t type;
|
||||
const uint8_t ef_structure;
|
||||
uint8_t *data; //should include 2 bytes len at begining
|
||||
const uint8_t acl[7];
|
||||
} __attribute__((packed)) file_t;
|
||||
|
||||
typedef struct file_chain
|
||||
{
|
||||
file_t *file;
|
||||
struct file_chain *next;
|
||||
} file_chain_t;
|
||||
|
||||
extern file_t *currentEF;
|
||||
extern file_t *currentDF;
|
||||
extern const file_t *selected_applet;
|
||||
|
||||
extern const file_t *MF;
|
||||
extern const file_t *file_last;
|
||||
extern const file_t *file_openpgp;
|
||||
extern const file_t *file_sc_hsm;
|
||||
extern bool card_terminated;
|
||||
extern file_t *file_pin1;
|
||||
extern file_t *file_retries_pin1;
|
||||
extern file_t *file_sopin;
|
||||
extern file_t *file_retries_sopin;
|
||||
|
||||
extern file_t *search_by_fid(const uint16_t fid, const file_t *parent, const uint8_t sp);
|
||||
extern file_t *search_by_name(uint8_t *name, uint16_t namelen);
|
||||
extern file_t *search_by_path(const uint8_t *pe_path, uint8_t pathlen, const file_t *parent);
|
||||
extern bool authenticate_action(const file_t *ef, uint8_t op);
|
||||
extern void process_fci(const file_t *pe);
|
||||
extern void scan_flash();
|
||||
extern void initialize_flash(bool);
|
||||
|
||||
extern file_t file_entries[];
|
||||
|
||||
extern uint8_t *file_read(const uint8_t *addr);
|
||||
extern uint16_t file_read_uint16(const uint8_t *addr);
|
||||
extern uint8_t file_read_uint8(const uint8_t *addr);
|
||||
extern file_t *file_new(uint16_t);
|
||||
file_t *get_parent(file_t *f);
|
||||
|
||||
extern uint16_t dynamic_files;
|
||||
extern file_t dynamic_file[];
|
||||
extern file_t *search_dynamic_file(uint16_t);
|
||||
extern int delete_dynamic_file(file_t *f);
|
||||
|
||||
extern file_chain_t *add_file_to_chain(file_t *file, file_chain_t **chain);
|
||||
extern file_t *search_file_chain(uint16_t fid, file_chain_t *chain);
|
||||
extern bool isUserAuthenticated;
|
||||
|
||||
#endif
|
||||
|
||||
133
src/fs/flash.c
Normal file
133
src/fs/flash.c
Normal file
@@ -0,0 +1,133 @@
|
||||
/*
|
||||
* This file is part of the Pico CCID distribution (https://github.com/polhenarejos/pico-ccid).
|
||||
* Copyright (c) 2022 Pol Henarejos.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, version 3.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/flash.h"
|
||||
#include "ccid2040.h"
|
||||
#include "tusb.h"
|
||||
#include "file.h"
|
||||
|
||||
/*
|
||||
* ------------------------------------------------------
|
||||
* | |
|
||||
* | next_addr | prev_addr | fid | data (len + payload) |
|
||||
* | |
|
||||
* ------------------------------------------------------
|
||||
*/
|
||||
#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES >> 1) // DATA starts at the mid of flash
|
||||
#define FLASH_DATA_HEADER_SIZE (sizeof(uintptr_t)+sizeof(uint32_t))
|
||||
#define FLASH_PERMANENT_REGION (4*FLASH_SECTOR_SIZE) // 4 sectors (16kb) of permanent memory
|
||||
|
||||
//To avoid possible future allocations, data region starts at the end of flash and goes upwards to the center region
|
||||
|
||||
const uintptr_t start_data_pool = (XIP_BASE + FLASH_TARGET_OFFSET);
|
||||
const uintptr_t end_data_pool = (XIP_BASE + PICO_FLASH_SIZE_BYTES)-FLASH_DATA_HEADER_SIZE-FLASH_PERMANENT_REGION; //This is a fixed value. DO NOT CHANGE
|
||||
#define FLASH_ADDR_DATA_STORAGE_START start_data_pool
|
||||
|
||||
extern int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len);
|
||||
extern int flash_program_halfword (uintptr_t addr, uint16_t data);
|
||||
extern int flash_program_uintptr(uintptr_t, uintptr_t);
|
||||
extern uintptr_t flash_read_uintptr(uintptr_t addr);
|
||||
extern uint16_t flash_read_uint16(uintptr_t addr);
|
||||
|
||||
extern void low_flash_available();
|
||||
|
||||
uintptr_t allocate_free_addr(uint16_t size) {
|
||||
if (size > FLASH_SECTOR_SIZE)
|
||||
return 0x0; //ERROR
|
||||
size_t real_size = size+sizeof(uint16_t)+sizeof(uintptr_t)+sizeof(uint16_t)+sizeof(uintptr_t); //len+len size+next address+fid+prev_addr size
|
||||
uintptr_t next_base = 0x0;
|
||||
for (uintptr_t base = end_data_pool; base >= start_data_pool; base = next_base) {
|
||||
uintptr_t addr_alg = base & -FLASH_SECTOR_SIZE; //start address of sector
|
||||
uintptr_t potential_addr = base-real_size;
|
||||
next_base = flash_read_uintptr(base);
|
||||
//printf("nb %x %x %x %x\r\n",base,next_base,addr_alg,potential_addr);
|
||||
//printf("fid %x\r\n",flash_read_uint16(next_base+sizeof(uintptr_t)));
|
||||
if (next_base == 0x0) { //we are at the end
|
||||
//now we check if we fit in the current sector
|
||||
if (addr_alg <= potential_addr) //it fits in the current sector
|
||||
{
|
||||
flash_program_uintptr(potential_addr, 0x0);
|
||||
flash_program_uintptr(potential_addr+sizeof(uintptr_t), base);
|
||||
flash_program_uintptr(base, potential_addr);
|
||||
return potential_addr;
|
||||
}
|
||||
else if (addr_alg-FLASH_SECTOR_SIZE >= start_data_pool) { //check whether it fits in the next sector, so we take addr_aligned as the base
|
||||
potential_addr = addr_alg-real_size;
|
||||
flash_program_uintptr(potential_addr, 0x0);
|
||||
flash_program_uintptr(potential_addr+sizeof(uintptr_t), base);
|
||||
flash_program_uintptr(base, potential_addr);
|
||||
return potential_addr;
|
||||
}
|
||||
return 0x0;
|
||||
}
|
||||
//we check if |base-(next_addr+size_next_addr)| > |base-potential_addr| only if fid != 1xxx (not size blocked)
|
||||
else if (addr_alg <= potential_addr && base-(next_base+flash_read_uint16(next_base+sizeof(uintptr_t)+sizeof(uintptr_t)+sizeof(uint16_t))+2*sizeof(uint16_t)+2*sizeof(uintptr_t)) > base-potential_addr && flash_read_uint16(next_base+sizeof(uintptr_t)) & 0x1000 != 0x1000) {
|
||||
flash_program_uintptr(potential_addr, next_base);
|
||||
flash_program_uintptr(potential_addr+sizeof(uintptr_t), base);
|
||||
flash_program_uintptr(base, potential_addr);
|
||||
return potential_addr;
|
||||
}
|
||||
}
|
||||
return 0x0; //probably never reached
|
||||
}
|
||||
|
||||
int flash_clear_file(file_t *file) {
|
||||
uintptr_t base_addr = (uintptr_t)(file->data-sizeof(uintptr_t)-sizeof(uint16_t)-sizeof(uintptr_t));
|
||||
uintptr_t prev_addr = flash_read_uintptr(base_addr+sizeof(uintptr_t));
|
||||
uintptr_t next_addr = flash_read_uintptr(base_addr);
|
||||
//printf("nc %x->%x %x->%x\r\n",prev_addr,flash_read_uintptr(prev_addr),base_addr,next_addr);
|
||||
flash_program_uintptr(prev_addr, next_addr);
|
||||
flash_program_halfword((uintptr_t)file->data, 0);
|
||||
if (next_addr > 0)
|
||||
flash_program_uintptr(next_addr+sizeof(uintptr_t), prev_addr);
|
||||
//printf("na %x->%x\r\n",prev_addr,flash_read_uintptr(prev_addr));
|
||||
return CCID_OK;
|
||||
}
|
||||
|
||||
int flash_write_data_to_file(file_t *file, const uint8_t *data, uint16_t len) {
|
||||
if (!file)
|
||||
return CCID_ERR_NULL_PARAM;
|
||||
if (len > FLASH_SECTOR_SIZE)
|
||||
return CCID_ERR_NO_MEMORY;
|
||||
if (file->data) { //already in flash
|
||||
uint16_t size_file_flash = flash_read_uint16((uintptr_t)file->data);
|
||||
if (len <= size_file_flash) { //it fits, no need to move it
|
||||
flash_program_halfword((uintptr_t)file->data, len);
|
||||
if (data)
|
||||
flash_program_block((uintptr_t)file->data+sizeof(uint16_t), data, len);
|
||||
return CCID_OK;
|
||||
}
|
||||
else { //we clear the old file
|
||||
flash_clear_file(file);
|
||||
}
|
||||
}
|
||||
uintptr_t new_addr = allocate_free_addr(len);
|
||||
//printf("na %x\r\n",new_addr);
|
||||
if (new_addr == 0x0)
|
||||
return CCID_ERR_NO_MEMORY;
|
||||
file->data = (uint8_t *)new_addr+sizeof(uintptr_t)+sizeof(uint16_t)+sizeof(uintptr_t); //next addr+fid+prev addr
|
||||
flash_program_halfword(new_addr+sizeof(uintptr_t)+sizeof(uintptr_t), file->fid);
|
||||
flash_program_halfword((uintptr_t)file->data, len);
|
||||
if (data)
|
||||
flash_program_block((uintptr_t)file->data+sizeof(uint16_t), data, len);
|
||||
return CCID_OK;
|
||||
}
|
||||
247
src/fs/low_flash.c
Normal file
247
src/fs/low_flash.c
Normal file
@@ -0,0 +1,247 @@
|
||||
/*
|
||||
* This file is part of the Pico CCID distribution (https://github.com/polhenarejos/pico-ccid).
|
||||
* Copyright (c) 2022 Pol Henarejos.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, version 3.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/flash.h"
|
||||
#include "hardware/sync.h"
|
||||
#include "pico/mutex.h"
|
||||
#include "pico/sem.h"
|
||||
#include "pico/multicore.h"
|
||||
#include "ccid2040.h"
|
||||
#include <string.h>
|
||||
|
||||
#define TOTAL_FLASH_PAGES 4
|
||||
|
||||
typedef struct page_flash {
|
||||
uint8_t page[FLASH_SECTOR_SIZE];
|
||||
uintptr_t address;
|
||||
bool ready;
|
||||
bool erase;
|
||||
size_t page_size; //this param is for easy erase. It allows to erase with a single call. IT DOES NOT APPLY TO WRITE
|
||||
} page_flash_t;
|
||||
|
||||
static page_flash_t flash_pages[TOTAL_FLASH_PAGES];
|
||||
|
||||
static mutex_t mtx_flash;
|
||||
static semaphore_t sem_wait;
|
||||
|
||||
static uint8_t ready_pages = 0;
|
||||
|
||||
bool flash_available = false;
|
||||
static bool locked_out = false;
|
||||
|
||||
|
||||
//this function has to be called from the core 0
|
||||
void do_flash()
|
||||
{
|
||||
if (mutex_try_enter(&mtx_flash, NULL) == true) {
|
||||
if (locked_out == true && flash_available == true && ready_pages > 0) {
|
||||
//printf(" DO_FLASH AVAILABLE\r\n");
|
||||
for (int r = 0; r < TOTAL_FLASH_PAGES; r++) {
|
||||
if (flash_pages[r].ready == true) {
|
||||
//printf("WRITTING %X\r\n",flash_pages[r].address-XIP_BASE);
|
||||
while (multicore_lockout_start_timeout_us(1000) == false);
|
||||
//printf("WRITTING %X\r\n",flash_pages[r].address-XIP_BASE);
|
||||
uint32_t ints = save_and_disable_interrupts();
|
||||
flash_range_erase(flash_pages[r].address-XIP_BASE, FLASH_SECTOR_SIZE);
|
||||
flash_range_program(flash_pages[r].address-XIP_BASE, flash_pages[r].page, FLASH_SECTOR_SIZE);
|
||||
restore_interrupts (ints);
|
||||
while (multicore_lockout_end_timeout_us(1000) == false);
|
||||
//printf("WRITEN %X !\r\n",flash_pages[r].address);
|
||||
|
||||
flash_pages[r].ready = false;
|
||||
ready_pages--;
|
||||
}
|
||||
else if (flash_pages[r].erase == true) {
|
||||
while (multicore_lockout_start_timeout_us(1000) == false);
|
||||
//printf("WRITTING\r\n");
|
||||
flash_range_erase(flash_pages[r].address-XIP_BASE, flash_pages[r].page_size ? ((int)(flash_pages[r].page_size/FLASH_SECTOR_SIZE))*FLASH_SECTOR_SIZE : FLASH_SECTOR_SIZE);
|
||||
while (multicore_lockout_end_timeout_us(1000) == false);
|
||||
flash_pages[r].erase = false;
|
||||
ready_pages--;
|
||||
}
|
||||
}
|
||||
flash_available = false;
|
||||
if (ready_pages != 0) {
|
||||
DEBUG_INFO("ERROR: DO FLASH DOES NOT HAVE ZERO PAGES");
|
||||
}
|
||||
}
|
||||
mutex_exit(&mtx_flash);
|
||||
}
|
||||
sem_release(&sem_wait);
|
||||
}
|
||||
|
||||
//this function has to be called from the core 0
|
||||
void low_flash_init() {
|
||||
mutex_init(&mtx_flash);
|
||||
sem_init(&sem_wait, 0, 1);
|
||||
memset(flash_pages, 0, sizeof(page_flash_t)*TOTAL_FLASH_PAGES);
|
||||
}
|
||||
|
||||
void low_flash_init_core1() {
|
||||
mutex_enter_blocking(&mtx_flash);
|
||||
multicore_lockout_victim_init();
|
||||
locked_out = true;
|
||||
mutex_exit(&mtx_flash);
|
||||
}
|
||||
|
||||
void wait_flash_finish() {
|
||||
sem_acquire_blocking(&sem_wait); //blocks until released
|
||||
//wake up
|
||||
sem_acquire_blocking(&sem_wait); //decrease permits
|
||||
}
|
||||
|
||||
void low_flash_available() {
|
||||
mutex_enter_blocking(&mtx_flash);
|
||||
flash_available = true;
|
||||
mutex_exit(&mtx_flash);
|
||||
}
|
||||
|
||||
page_flash_t *find_free_page(uintptr_t addr) {
|
||||
uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE;
|
||||
page_flash_t *p = NULL;
|
||||
for (int r = 0; r < TOTAL_FLASH_PAGES; r++)
|
||||
{
|
||||
if ((!flash_pages[r].ready && !flash_pages[r].erase) || flash_pages[r].address == addr_alg) //first available
|
||||
{
|
||||
p = &flash_pages[r];
|
||||
if (!flash_pages[r].ready && !flash_pages[r].erase)
|
||||
{
|
||||
memcpy(p->page, (uint8_t *)addr_alg, FLASH_SECTOR_SIZE);
|
||||
ready_pages++;
|
||||
p->address = addr_alg;
|
||||
p->ready = true;
|
||||
}
|
||||
return p;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len) {
|
||||
uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE;
|
||||
page_flash_t *p = NULL;
|
||||
|
||||
if (!data || len == 0)
|
||||
return CCID_ERR_NULL_PARAM;
|
||||
|
||||
mutex_enter_blocking(&mtx_flash);
|
||||
if (ready_pages == TOTAL_FLASH_PAGES) {
|
||||
mutex_exit(&mtx_flash);
|
||||
DEBUG_INFO("ERROR: ALL FLASH PAGES CACHED\r\n");
|
||||
return CCID_ERR_NO_MEMORY;
|
||||
}
|
||||
if (!(p = find_free_page(addr)))
|
||||
{
|
||||
mutex_exit(&mtx_flash);
|
||||
DEBUG_INFO("ERROR: FLASH CANNOT FIND A PAGE (rare error)\r\n");
|
||||
return CCID_ERR_MEMORY_FATAL;
|
||||
}
|
||||
memcpy(&p->page[addr&(FLASH_SECTOR_SIZE-1)], data, len);
|
||||
//printf("Flash: modified page %X with data %x at [%x] (top page %X)\r\n",addr_alg,data,addr&(FLASH_SECTOR_SIZE-1),addr);
|
||||
mutex_exit(&mtx_flash);
|
||||
return CCID_OK;
|
||||
}
|
||||
|
||||
int flash_program_halfword (uintptr_t addr, uint16_t data) {
|
||||
return flash_program_block(addr, (const uint8_t *)&data, sizeof(uint16_t));
|
||||
}
|
||||
|
||||
int flash_program_word (uintptr_t addr, uint32_t data) {
|
||||
return flash_program_block(addr, (const uint8_t *)&data, sizeof(uint32_t));
|
||||
}
|
||||
|
||||
int flash_program_uintptr (uintptr_t addr, uintptr_t data) {
|
||||
return flash_program_block(addr, (const uint8_t *)&data, sizeof(uintptr_t));
|
||||
}
|
||||
|
||||
uint8_t *flash_read(uintptr_t addr) {
|
||||
uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE;
|
||||
mutex_enter_blocking(&mtx_flash);
|
||||
if (ready_pages > 0) {
|
||||
for (int r = 0; r < TOTAL_FLASH_PAGES; r++)
|
||||
{
|
||||
if (flash_pages[r].ready && flash_pages[r].address == addr_alg) {
|
||||
uint8_t *v = &flash_pages[r].page[addr&(FLASH_SECTOR_SIZE-1)];
|
||||
mutex_exit(&mtx_flash);
|
||||
return v;
|
||||
}
|
||||
}
|
||||
}
|
||||
uint8_t *v = (uint8_t *)addr;
|
||||
mutex_exit(&mtx_flash);
|
||||
return v;
|
||||
}
|
||||
|
||||
uintptr_t flash_read_uintptr(uintptr_t addr) {
|
||||
uint8_t *p = flash_read(addr);
|
||||
uintptr_t v = 0x0;
|
||||
for (int i = 0; i < sizeof(uintptr_t); i++) {
|
||||
v |= (uintptr_t)p[i]<<(8*i);
|
||||
}
|
||||
return v;
|
||||
}
|
||||
uint16_t flash_read_uint16(uintptr_t addr) {
|
||||
uint8_t *p = flash_read(addr);
|
||||
uint16_t v = 0x0;
|
||||
for (int i = 0; i < sizeof(uint16_t); i++) {
|
||||
v |= p[i]<<(8*i);
|
||||
}
|
||||
return v;
|
||||
}
|
||||
uint8_t flash_read_uint8(uintptr_t addr) {
|
||||
return *flash_read(addr);
|
||||
}
|
||||
|
||||
int flash_erase_page (uintptr_t addr, size_t page_size) {
|
||||
uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE;
|
||||
page_flash_t *p = NULL;
|
||||
|
||||
mutex_enter_blocking(&mtx_flash);
|
||||
if (ready_pages == TOTAL_FLASH_PAGES) {
|
||||
mutex_exit(&mtx_flash);
|
||||
DEBUG_INFO("ERROR: ALL FLASH PAGES CACHED\r\n");
|
||||
return CCID_ERR_NO_MEMORY;
|
||||
}
|
||||
if (!(p = find_free_page(addr))) {
|
||||
DEBUG_INFO("ERROR: FLASH CANNOT FIND A PAGE (rare error)\r\n");
|
||||
mutex_exit(&mtx_flash);
|
||||
return CCID_ERR_MEMORY_FATAL;
|
||||
}
|
||||
p->erase = true;
|
||||
p->ready = false;
|
||||
p->page_size = page_size;
|
||||
mutex_exit(&mtx_flash);
|
||||
|
||||
return CCID_OK;
|
||||
}
|
||||
|
||||
bool flash_check_blank(const uint8_t *p_start, size_t size)
|
||||
{
|
||||
const uint8_t *p;
|
||||
|
||||
for (p = p_start; p < p_start + size; p++) {
|
||||
if (*p != 0xff)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
Reference in New Issue
Block a user