void segv_sigaction(int signum, siginfo_t* sinfo, void * p){
// Check signal
if (SIGSEGV != signum) {
printf ("Bad handler for bad signal...\n");
exit (1);
}
// Check if fault is from managed segment
if(sinfo->si_addr < start_addr || sinfo->si_addr >= size*PAGESIZE+start_addr){
printf("Segfault somewhere else... good luck...\n");
printf(" Hint: run the debugger and set a breakpoint for this line\n");
exit(1);
}
// Check cause of fault
if(sinfo->si_code != SEGV_ACCERR){
printf("Object not mapped...\n");
printf(" Hint: run the debugger and set a breakpoint for this line\n");
exit(1);
}
// Find out virtual page in fault
page_virt pv=PAGEOF(sinfo->si_addr);
// If not invalid then setDirtyBit
// Allow read, but wait until page has been loaded...
if(mmu_array[pv].pp != INVALID){
if(!mmu_array[pv].accessed){
struct timeval delay;
struct timeval now;
gettimeofday(&now,NULL);
timeradd(&delay,&delay_load,&delay);
timersub(&delay,&now,&delay);
if(delay.tv_sec >= 0){
int UNUSED r;
struct timespec s;
TV2TS(delay,s);
//do{
// r=nanosleep(&s,&s);
// }while(r < 0 && errno == EINTR);
}
mmu_setAccessedBit(pv);
}else{
mmu_setDirtyBit(pv);
}
}else{
// Else call page fault handler
stats_faults++;
page_fault_handler(pv);
}
// Check that translation has been added
if(mmu_array[pv].pp == INVALID){
printf("Page fault handler did not update MMU...\n");
exit(1);
}
}
static enum armv2_status load_section( struct armv2 *cpu, uint32_t start, uint32_t end, FILE *f, ssize_t *size_out ) {
uint32_t section_length = 0;
ssize_t read_bytes = 0;
ssize_t size = *size_out;
if(0 != INPAGE(start)) {
//We only want to start loading sections in at page boundaries
LOG("Error starting section off page boundary\n");
return ARMV2STATUS_INVALID_PAGE;
}
uint32_t page_num = PAGEOF(start);
read_bytes = fread(§ion_length, sizeof(section_length), 1, f);
if(read_bytes != 1) {
LOG("Error reading opening length\n");
return ARMV2STATUS_IO_ERROR;
}
section_length = htonl( section_length );
size -= sizeof(section_length);
if( section_length > (end - start) ) {
LOG("Error, section length of %08x would take us past end %08x\n", section_length, end );
return ARMV2STATUS_IO_ERROR;
}
if( section_length > size ) {
LOG("Error, not enough data for section length 0x%x (0x%zx bytes remaining)\n", section_length, size);
return ARMV2STATUS_IO_ERROR;
}
size -= section_length;
while(section_length > 0) {
size_t to_read = section_length > PAGE_SIZE ? PAGE_SIZE : section_length;
read_bytes = fread(cpu->page_tables[page_num++]->memory, 1, to_read,f);
if(read_bytes != to_read) {
if(read_bytes != section_length) {
LOG("Error %d %zd %zd %d\n",page_num, read_bytes, size, section_length);
return ARMV2STATUS_IO_ERROR;
}
}
section_length -= to_read;
}
*size_out = size;
return ARMV2STATUS_OK;
}
bool FLASHDRVR::write_page(const unsigned addr, const unsigned len,
const unsigned *data, const bool verify_write) {
DEVBUS::BUSW buf[SZPAGE];
assert(len > 0);
assert(len <= PGLEN);
assert(PAGEOF(addr)==PAGEOF(addr+len-1));
if (len <= 0)
return true;
// Write the page
m_fpga->writeio(R_ICONTROL, ISPIF_DIS);
m_fpga->clear();
m_fpga->writeio(R_ICONTROL, ISPIF_EN);
printf("Writing page: 0x%08x - 0x%08x\n", addr, addr+len-1);
m_fpga->writeio(R_QSPI_EREG, DISABLEWP);
m_fpga->writei(addr, len, data);
// If we're in high speed mode and we want to verify the write, then
// we can skip waiting for the write to complete by issueing a read
// command immediately. As soon as the write completes the read will
// begin sending commands back. This allows us to recover the lost
// time between the interrupt and the next command being received.
if ((!HIGH_SPEED)||(!verify_write)) {
flwait();
} if (verify_write) {
// NOW VERIFY THE PAGE
m_fpga->readi(addr, len, buf);
for(unsigned i=0; i<len; i++) {
if (buf[i] != data[i]) {
printf("\nVERIFY FAILS[%d]: %08x\n", i, i+addr);
printf("\t(Flash[%d]) %08x != %08x (Goal[%08x])\n",
i, buf[i], data[i], i+addr);
return false;
}
}
} return true;
}
enum armv2_status map_memory(struct armv2 *cpu, uint32_t device_num, uint32_t start, uint32_t end) {
uint32_t page_pos = 0;
uint32_t page_start = PAGEOF(start);
uint32_t page_end = PAGEOF(end);
struct hardware_mapping hw_mapping = {0};
if(NULL == cpu || end <= start) {
return ARMV2STATUS_INVALID_ARGS;
}
if(device_num >= cpu->num_hardware_devices) {
return ARMV2STATUS_NO_SUCH_DEVICE;
}
if(NULL == cpu->hardware_devices[device_num]) {
return ARMV2STATUS_INVALID_CPUSTATE;
}
if(start&PAGE_MASK ||
end &PAGE_MASK ||
page_start == 0 ||
page_end == 0 ||
page_start >= NUM_PAGE_TABLES ||
page_end >= NUM_PAGE_TABLES
//page_start == INTERRUPT_PAGE_NUM ||
//page_end == INTERRUPT_PAGE_NUM ) {
) {
return ARMV2STATUS_INVALID_ARGS;
}
//First we need to know if all of the requested memory is available for mapping. That means
//it must not have been mapped already, and it may not be the zero page
for(page_pos = page_start; page_pos < page_end; page_pos++) {
struct page_info *page;
if(page_pos >= NUM_PAGE_TABLES) { // || page_pos == INTERRUPT_PAGE_NUM) {
return ARMV2STATUS_MEMORY_ERROR;
}
page = cpu->page_tables[page_pos];
if(page == NULL) {
//That's OK, that means this page is currently completely unmapped. We can make a page just for this
continue;
}
if(page->read_callback || page->write_callback || page->read_byte_callback || page->write_byte_callback) {
return ARMV2STATUS_ALREADY_MAPPED;
}
}
hw_mapping.device = cpu->hardware_devices[device_num];
//If we get here then the entire range is free, so we can go ahead and fill it in
for(page_pos = page_start; page_pos < page_end; page_pos++) {
struct page_info *page = cpu->page_tables[page_pos];
if(NULL == page) {
//we need a new page
page = calloc(1, sizeof(struct page_info));
if(NULL == page) {
//I don't think I'm leaving anything untidied up by returning here
return ARMV2STATUS_MEMORY_ERROR;
}
page->flags = (PERM_READ|PERM_EXECUTE|PERM_WRITE);
page->memory = NULL;
if(hw_mapping.device) {
page->mapped_device = hw_mapping.device->extra;
}
cpu->page_tables[page_pos] = page;
}
//Already checked everything's OK, and we're single threaded, so this should be ok I think...
/* LOG("Setting page_pos %x to callbacks %p %p flags %x\n", */
/* page_pos, */
/* hw_mapping.device->read_callback, */
/* hw_mapping.device->write_callback, */
/* hw_mapping.device->read_byte_callback, */
/* hw_mapping.device->write_byte_callback); */
page->read_callback = hw_mapping.device->read_callback;
page->write_callback = hw_mapping.device->write_callback;
page->read_byte_callback = hw_mapping.device->read_byte_callback;
page->write_byte_callback = hw_mapping.device->write_byte_callback;
}
hw_mapping.start = start;
hw_mapping.end = end;
hw_mapping.flags = 0; //maybe use these later
add_mapping(&(cpu->hw_mappings),&hw_mapping);
return ARMV2STATUS_OK;
}
bool FLASHDRVR::write(const unsigned addr, const unsigned len,
const unsigned *data, const bool verify) {
// Work through this one sector at a time.
// If this buffer is equal to the sector value(s), go on
// If not, erase the sector
// m_fpga->writeio(R_QSPI_CREG, 2);
// m_fpga->readio(R_VERSION); // Read something innocuous
// m_fpga->writeio(R_QSPI_SREG, 0);
// m_fpga->readio(R_VERSION); // Read something innocuous
for(unsigned s=SECTOROF(addr); s<SECTOROF(addr+len+SECTORSZ-1); s+=SECTORSZ) {
// printf("IN LOOP, s=%08x\n", s);
// Do we need to erase?
bool need_erase = false;
unsigned newv = 0; // (s<addr)?addr:s;
{
DEVBUS::BUSW *sbuf = new DEVBUS::BUSW[SECTORSZ];
const DEVBUS::BUSW *dp;
unsigned base,ln;
base = (addr>s)?addr:s;
ln=((addr+len>s+SECTORSZ)?(s+SECTORSZ):(addr+len))-base;
m_fpga->readi(base, ln, sbuf);
dp = &data[base-addr];
for(unsigned i=0; i<ln; i++) {
if ((sbuf[i]&dp[i]) != dp[i]) {
printf("\nNEED-ERASE @0x%08x ... %08x != %08x (Goal)\n",
i+base-addr, sbuf[i], dp[i]);
need_erase = true;
newv = i+base;
break;
} else if ((sbuf[i] != dp[i])&&(newv == 0)) {
// if (newv == 0)
// printf("MEM[%08x] = %08x (!= %08x (Goal))\n",
// i+base, sbuf[i], dp[i]);
newv = i+base;
}
}
}
if (newv == 0)
continue; // This sector already matches
// Just erase anyway
if ((need_erase)&&(!erase_sector(s, verify))) {
printf("SECTOR ERASE FAILED!\n");
return false;
} else if (!need_erase)
printf("NO ERASE NEEDED\n");
else {
printf("ERASING SECTOR %08x\n", s);
newv = (s<addr) ? addr : s;
}
for(unsigned p=newv; (p<s+SECTORSZ)&&(p<addr+len); p=PAGEOF(p+PGLEN))
if (!write_page(p, (p+PGLEN<addr+len)
?((PAGEOF(p)!=PAGEOF(p+PGLEN-1))?(PAGEOF(p+PGLEN-1)-p):PGLEN)
:(addr+len-p), &data[p-addr]), verify) {
printf("WRITE-PAGE FAILED!\n");
return false;
}
}
m_fpga->writeio(R_QSPI_EREG, 0); // Re-enable write protection
return true;
}