void
__impure_init (void)
{
// Initialize both impure data structures
_REENT_INIT_PTR (_impure_ptr);
_REENT_INIT_PTR (_exception_impure_ptr);
// Set current to standard impure pointer
_current_impure_ptr = _impure_ptr;
} /* __impure_init () */
struct _reent *
__getreent (void)
{
struct _reent *r = t_reentp;
if (!r) {
t_reentp = r = &t_reent;
_REENT_INIT_PTR(r);
}
return r;
}
struct _reent *
__create_reent (void)
{
struct _reent* reent = (struct _reent*) calloc(1, sizeof(struct _cereent));
if (!reent)
return NULL;
_REENT_INIT_PTR((reent));
__init_cereent((struct _cereent*)reent);
return reent;
}
/**
* \b archThreadContextInit
*
* Architecture-specific thread context initialisation routine.
*
* This function must set up a thread's context ready for restoring
* and running the thread via archFirstThreadRestore() or
* archContextSwitch().
*
* The layout required to fill the correct register values is
* described in archContextSwitch(). Note that not all registers
* are restored by archContextSwitch() and archFirstThreadRestore()
* as this port takes advantage of the fact that not all registers
* must be stored by ARM gcc C subroutines. This means that we don't
* provide start values for those registers, as they are "don't cares".
*
* @param[in] tcb_ptr Pointer to the TCB of the thread being created
* @param[in] stack_top Pointer to the top of the new thread's stack
* @param[in] entry_point Pointer to the thread entry point function
* @param[in] entry_param Parameter to be passed to the thread entry point
*
* @return None
*/
void archThreadContextInit (ATOM_TCB *tcb_ptr, void *stack_top, void (*entry_point)(uint32_t), uint32_t entry_param)
{
uint32_t *stack_ptr;
/** Start at stack top */
tcb_ptr->sp_save_ptr = stack_ptr = stack_top;
/**
* After restoring all of the context registers, the thread restore
* routines will return to the address of the calling routine on the
* stack. In this case (the first time a thread is run) we "return"
* to the entry point for the thread. That is, we store the thread
* entry point in the place that return will look for the return
* address: the stack.
*
* Note that we are using the thread_shell() routine to start all
* threads, so we actually store the address of thread_shell()
* here. Other ports may store the real thread entry point here
* and call it directly from the thread restore routines.
*
* Because we are filling the stack from top to bottom, this goes
* on the stack first (at the top).
*/
*stack_ptr-- = (uint32_t)thread_shell;
/**
* Store starting register values for R4-R11
*/
*stack_ptr-- = (uint32_t) 0x00001111; /* R11 */
*stack_ptr-- = (uint32_t) 0x00001010; /* R10 */
*stack_ptr-- = (uint32_t) 0x00000909; /* R9 */
*stack_ptr-- = (uint32_t) 0x00000808; /* R8 */
*stack_ptr-- = (uint32_t) 0x00000707; /* R7 */
*stack_ptr-- = (uint32_t) 0x00000606; /* R6 */
*stack_ptr-- = (uint32_t) 0x00000505; /* R5 */
*stack_ptr = (uint32_t) 0x00000404; /* R4 */
/**
* All thread context has now been initialised. Save the current
* stack pointer to the thread's TCB so it knows where to start
* looking when the thread is started.
*/
tcb_ptr->sp_save_ptr = stack_ptr;
/**
* At thread startup (and every time we switch back to a thread)
* we set the global reentrancy pointer to this thread's reent struct
*/
_REENT_INIT_PTR (&(tcb_ptr->port_priv.reent));
}
int __libc_start_hook(lwp_cntrl *curr_thr,lwp_cntrl *start_thr)
{
struct _reent *ptr;
ptr = (struct _reent*)calloc(1,sizeof(struct _reent));
if(!ptr) abort();
#ifdef __GNUC__
_REENT_INIT_PTR((ptr));
#endif
start_thr->libc_reent = ptr;
return 1;
}
Thread threadCreate(ThreadFunc entrypoint, void* arg, size_t stack_size, int prio, int affinity, bool detached)
{
size_t stackoffset = (sizeof(struct Thread_tag)+7)&~7;
size_t allocsize = stackoffset + ((stack_size+7)&~7);
size_t tlssize = __tls_end-__tls_start;
size_t tlsloadsize = __tdata_lma_end-__tdata_lma;
size_t tbsssize = tlssize-tlsloadsize;
// Guard against overflow
if (allocsize < stackoffset) return NULL;
if ((allocsize-stackoffset) < stack_size) return NULL;
if ((allocsize+tlssize) < allocsize) return NULL;
Thread t = (Thread)memalign(8,allocsize+tlssize);
if (!t) return NULL;
t->ep = entrypoint;
t->arg = arg;
t->detached = detached;
t->finished = false;
t->stacktop = (u8*)t + allocsize;
if (tlsloadsize)
memcpy(t->stacktop, __tdata_lma, tlsloadsize);
if (tbsssize)
memset((u8*)t->stacktop+tlsloadsize, 0, tbsssize);
// Set up child thread's reent struct, inheriting standard file handles
_REENT_INIT_PTR(&t->reent);
struct _reent* cur = getThreadVars()->reent;
t->reent._stdin = cur->_stdin;
t->reent._stdout = cur->_stdout;
t->reent._stderr = cur->_stderr;
Result rc;
rc = svcCreateThread(&t->handle, _thread_begin, (u32)t, (u32*)t->stacktop, prio, affinity);
if (R_FAILED(rc))
{
free(t);
return NULL;
}
return t;
}
ThreadControlBlock::ThreadControlBlock(internal::Stack&& stack, const uint8_t priority,
const SchedulingPolicy schedulingPolicy, ThreadGroupControlBlock* const threadGroupControlBlock,
SignalsReceiver*, RunnableThread& owner) :
ThreadListNode{priority},
ownedProtocolMutexList_{},
stack_{std::move(stack)},
list_{},
owner_{owner},
priorityInheritanceMutexControlBlock_{},
threadGroupControlBlock_{threadGroupControlBlock},
unblockFunctor_{},
roundRobinQuantum_{},
schedulingPolicy_{schedulingPolicy},
state_{ThreadState::created}
{
_REENT_INIT_PTR(&reent_);
const InterruptMaskingLock interruptMaskingLock;
sequenceNumber_ = nextSequenceNumber++;
}
ThreadControlBlock::ThreadControlBlock(architecture::Stack&& stack, const uint8_t priority,
const SchedulingPolicy schedulingPolicy, ThreadGroupControlBlock* const threadGroupControlBlock,
SignalsReceiver* const signalsReceiver, Thread& owner) :
ThreadListNode{priority},
stack_{std::move(stack)},
owner_{owner},
ownedProtocolMutexList_{},
priorityInheritanceMutexControlBlock_{},
list_{},
threadGroupControlBlock_{threadGroupControlBlock},
unblockFunctor_{},
signalsReceiverControlBlock_
{
signalsReceiver != nullptr ? &signalsReceiver->signalsReceiverControlBlock_ : nullptr
},
roundRobinQuantum_{},
schedulingPolicy_{schedulingPolicy},
state_{ThreadState::New}
{
_REENT_INIT_PTR(&reent_);
}
Thread* Thread_Create(Process* pProcess, size_t pEntryPoint, size_t pStackSize, uint8_t pPriority)
{
Thread* thread = (Thread*)calloc(1, sizeof(Thread));
thread->Process = pProcess;
thread->EntryPoint = pEntryPoint;
thread->Stack = (uint8_t*)calloc(1, pStackSize);
_REENT_INIT_PTR(&thread->Reentrant);
thread->Reentrant._stdin = stdin;
thread->Reentrant._stdout = stdout;
thread->Reentrant._stderr = stderr;
thread->Reentrant.__sdidinit = TRUE;
thread->Reentrant._new._reent._unused_rand = (uint32_t)thread;
Log_WriteLine(LOGLEVEL__Memory, "Memory: Thread_Create @ 0x%x, Stack @ 0x%x", (unsigned int)thread, (unsigned int)thread->Stack);
thread->StackSize = pStackSize;
thread->Priority = pPriority;
thread->SavedRegisterState.useresp = (uint32_t)(thread->Stack + thread->StackSize);
thread->SavedRegisterState.eip = pEntryPoint;
thread->SavedRegisterState.cs = 0x1B;
thread->SavedRegisterState.ds = 0x23;
thread->SavedRegisterState.ss = 0x23;
thread->SavedRegisterState.eflags = 0x200; // Interrupts enabled, maybe need 0x202
ThreadScheduler_Add(thread);
return thread;
}
int main(void)
{
uint8_t system_state=0, i2c_resets=0, si446x_resets=0;//used to track button press functionality and any errors
uint8_t sensors=0;
uint32_t repetition_counter=0; //Used to detect any I2C lockup
uint8_t L3GD20_Data_Buffer_old[8]; //Used to test for noise in the gyro data (indicating that it is working)
uint8_t UplinkFlags=0,CutFlags=0;
uint16_t UplinkBytes=0; //Counters and flags for telemetry
uint32_t last_telemetry=0,cutofftime=0,indtest=0,badgyro=0,permission_time=0,countdown_time=0,last_cuttest=0;
uint16_t sentence_counter=0;
uint8_t silab;
//Cutdown config stuff here, atm uses hardcoded polygon defined in polygon.h
static const int32_t Geofence[UK_GEOFENCE_POINTS*2]=UK_GEOFENCE;
RTC_t RTC_time;
_REENT_INIT_PTR(&my_reent);
_impure_ptr = &my_reent;
SystemInit(); //Sets up the clk
setup_gpio(); //Initialised pins, and detects boot source
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); //Watchdog stopped during JTAG halt
RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE);/* Enable PWR and BKP clocks */
PWR_BackupAccessCmd(ENABLE);/* Allow access to BKP Domain */
uint16_t shutdown_lock=BKP_ReadBackupRegister(BKP_DR3); //Holds the shutdown lock setting
uint16_t reset_counter=BKP_ReadBackupRegister(BKP_DR2); //The number of consecutive failed reboot cycles
PWR_BackupAccessCmd(DISABLE);
if(RCC->CSR&RCC_CSR_IWDGRSTF && shutdown_lock!=SHUTDOWNLOCK_MAGIC) {//Watchdog reset, turn off
RCC->CSR|=RCC_CSR_RMVF; //Reset the reset flags
shutdown();
}
if(USB_SOURCE==bootsource) {
RCC->CFGR &= ~(uint32_t)RCC_CFGR_PPRE1_DIV16;
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV4;//Swap the ABP1 bus to run at 12mhz rather than 4 if we booted from USB, makes USB fast enough
}
SysTick_Configuration(); //Start up system timer at 100Hz for uSD card functionality
Watchdog_Config(WATCHDOG_TIMEOUT); //Set the watchdog
Watchdog_Reset(); //Reset watchdog as soon as possible incase it is still running at power on
rtc_init(); //Real time clock initialise - (keeps time unchanged if set)
Usarts_Init();
ISR_Config();
rprintfInit(__usart_send_char); //Printf over the bluetooth
if(USB_SOURCE==bootsource) {
Set_System(); //This actually just inits the storage layer
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
uint32_t nojack=0x000FFFFF; //Countdown timer - a few hundered ms of 0v on jack detect forces a shutdown
while (bDeviceState != CONFIGURED) { //Wait for USB config - timeout causes shutdown
if((Millis>10000 && bDeviceState == UNCONNECTED)|| !nojack) //No USB cable - shutdown (Charger pin will be set to open drain, cant be disabled without usb)
shutdown();
if(GET_VBUS_STATE) //Jack detect resets the countdown
nojack=0x0FFFFF;
nojack--;
Watchdog_Reset(); //Reset watchdog here, if we are stalled here the Millis timeout should catch us
}
PWR_BackupAccessCmd(ENABLE); /* Allow access to BKP Domain */
BKP_WriteBackupRegister(BKP_DR3,0x0000);//Wipe the shutdown lock setting
PWR_BackupAccessCmd(DISABLE);
while(1) {
if(!(Millis%1000) && bDeviceState == SUSPENDED) {
Delay(100);
if(!GET_VBUS_STATE)
shutdown();
}
Watchdog_Reset();
__WFI(); //Sleep mode
}
}
if(!GET_PWR_STATE && !(CoreDebug->DHCSR&0x00000001) && shutdown_lock!=SHUTDOWNLOCK_MAGIC) {//Check here to make sure the power button is still pressed, if not, sleep if no debug and not in always on flight mode
shutdown(); //This means a glitch on the supply line, or a power glitch results in sleep
}
// check to see if battery has enough charge to start
ADC_Configuration(); //We leave this a bit later to allow stabilisation
{
uint32_t t=Millis;
while(Millis<(t+100)){__WFI();} //Sensor+inst amplifier takes about 100ms to stabilise after power on
}
if(Battery_Voltage<BATTERY_STARTUP_LIMIT) { //We will have to turn off
if(reset_counter<10)
shutdown();
}
Watchdog_Reset(); //Card Init can take a second or two
// system has passed battery level check and so file can be opened
{//Context
uint8_t silabs_header[5]={};
uint8_t* silabs_header_=NULL; //Pointer to the array (if all goes ok)
if((f_err_code = f_mount(0, &FATFS_Obj)))Usart_Send_Str((char*)"FatFs mount error\r\n");//This should only error if internal error
else { //FATFS initialised ok, try init the card, this also sets up the SPI1
if(!(f_err_code=f_open(&FATFS_logfile,(const TCHAR*)"time.txt",FA_OPEN_EXISTING|FA_READ|FA_WRITE))){//Try to open time file get system time
if(!f_stat((const TCHAR *)"time.txt",&FATFS_info)) {//Get file info
if(FATFS_info.fsize<5) { //Empty file
RTC_time.year=(FATFS_info.fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
RTC_time.month=(FATFS_info.fdate>>5)&0x000F;
RTC_time.mday=FATFS_info.fdate&0x001F;
RTC_time.hour=(FATFS_info.ftime>>11)&0x001F;
RTC_time.min=(FATFS_info.ftime>>5)&0x003F;
RTC_time.sec=(FATFS_info.ftime<<1)&0x003E;
rtc_settime(&RTC_time);
rprintfInit(__fat_print_char);//printf to the open file
printf("RTC set to %d/%d/%d %d:%d:%d\n",RTC_time.mday,RTC_time.month,RTC_time.year,\
RTC_time.hour,RTC_time.min,RTC_time.sec);
}
}
f_close(&FATFS_logfile); //Close the time.txt file
}
// load settings if file exists
Watchdog_Reset(); //Card Init can take a second or two
if(!f_open(&FATFS_logfile,(const TCHAR *)"settings.dat",FA_OPEN_EXISTING | FA_READ)) {
UINT br;
int8_t rtc_correction;
f_read(&FATFS_logfile, (void*)(&rtc_correction),sizeof(rtc_correction),&br);
//Use the setting to apply correction to the RTC
if(br && (rtc_correction<30) && (rtc_correction>-92) && rtc_correction ) {
PWR_BackupAccessCmd(ENABLE);/* Allow access to BKP Domain */
uint16_t tweaked_prescale = (0x0001<<15)-2;/* Try to run the RTC slightly too fast so it can be corrected either way */
RTC_WaitForSynchro(); /* Wait for RTC registers synchronization */
if( RTC->PRLL != tweaked_prescale ) {/*Note that there is a 0.5ppm offset here (correction 0==0.5ppm slow)*/
RTC_SetPrescaler(tweaked_prescale); /* RTC period = RTCCLK/RTC_PR = (32.768 KHz)/(32767-2+1) */
RTC_WaitForLastTask();
}
BKP_SetRTCCalibrationValue((uint8_t) ((int16_t)31-(21*(int16_t)rtc_correction)/(int16_t)20) );
BKP_RTCOutputConfig(BKP_RTCOutputSource_None);/* Ensure any output is disabled here */
/* Lock access to BKP Domain */
PWR_BackupAccessCmd(DISABLE);
}
else if(br && ((uint8_t)rtc_correction==0x91) ) {/* 0x91 magic flag sets the RTC clock output on */
PWR_BackupAccessCmd(ENABLE);/* Allow access to BKP Domain */
BKP_RTCOutputConfig(BKP_RTCOutputSource_CalibClock);/* Output a 512Hz reference clock on the TAMPER pin*/
PWR_BackupAccessCmd(DISABLE);
}
if(br) {
f_read(&FATFS_logfile, (void*)(&shutdown_lock),sizeof(shutdown_lock),&br);/*This needs to be set with the same magic flag value*/
if(br==2) {
PWR_BackupAccessCmd(ENABLE);/* Allow access to BKP Domain */
BKP_WriteBackupRegister(BKP_DR3,shutdown_lock);//Wipe the shutdown lock setting
PWR_BackupAccessCmd(DISABLE);
}
}
if(br==2) { //Read was successful, next try to read 5 bytes of packet header
f_read(&FATFS_logfile, (void*)(silabs_header),5,&br);
if(br!=5)
silabs_header_=silabs_header;
}
f_close(&FATFS_logfile); //Close the settings.dat file
}
#ifndef SINGLE_LOGFILE
rtc_gettime(&RTC_time); //Get the RTC time and put a timestamp on the start of the file
rprintfInit(__str_print_char); //Print to the string
printf("%02d-%02d-%02dT%02d-%02d-%02d-%s.csv",RTC_time.year,RTC_time.month,RTC_time.mday,RTC_time.hour,RTC_time.min,RTC_time.sec,"Log");//Timestamp name
rprintfInit(__usart_send_char); //Printf over the bluetooth
#endif
Watchdog_Reset(); //Card Init can take a second or two
if((f_err_code=f_open(&FATFS_logfile,(TCHAR*)LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Present
Delay(10000);
if((f_err_code=f_open(&FATFS_logfile,(TCHAR*)LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Try again
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
}
}
else {
Watchdog_Reset(); //Card Init can take a second or two
print_string[strlen(print_string)-4]=0x00;//Wipe the .csv off the string
strcat(print_string,"_gyro.wav");
if((f_err_code=f_open(&FATFS_wavfile_gyro,(TCHAR*)LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Present
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
}
else { //We have a mounted card
f_err_code=f_lseek(&FATFS_logfile, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_logfile) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_logfile);//Close the already opened file on error
else
file_opened=1; //So we know to close the file properly on shutdown
if(file_opened==1) {
Watchdog_Reset(); //Card Init can take a second or two
if (f_err_code || f_tell(&FATFS_wavfile_gyro) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_wavfile_gyro);//Close the already opened file on error
else
file_opened|=2; //So we know to close the file properly on shutdown
}
}
}
}
f_err_code|=write_wave_header(&FATFS_wavfile_gyro, 4, 100, 16);//4 channels, last channel is for the current rpm
Watchdog_Reset(); //Card Init can take a second or two
//Setup and test the silabs radio
silab=si446x_setup(silabs_header_);
if(silab!=0x44) { //Should return the device code
print_string[0]=0x00;
printf("Silabs: %02x\n",silab);
f_puts("Silabs detect error, got:",&FATFS_logfile);
f_puts(print_string,&FATFS_logfile);
shutdown_filesystem(ERR, file_opened);//So we log that something went wrong in the logfile
shutdown();
}
}//Context
static int elf_load_int(const char *path, task_t *task, char *argv[], char *envp[]) {
// Loads to a fixed address of 0x10000000 for now; not a HUGE deal
// since each (user mode) task has its own address space
assert(interrupts_enabled() == false); // TODO: get rid of the race condition from create_task, so that this isn't needed
assert(task != NULL);
struct task_mm *mm = task->mm;
assert(mm != NULL);
struct stat st;
int r;
if ((r = stat(path, &st)) != 0) {
assert(r < 0);
return r;
}
uint32 file_size = st.st_size;
unsigned char *data = kmalloc(file_size);
int retval = 0;
int fd = open(path, O_RDONLY);
if (fd < 0) {
printk("elf_load(): unable to open %s\n", path);
retval = fd;
goto err;
}
if ((r = read(fd, data, file_size)) != (int)file_size) {
printk("elf_load(): unable to read from %s; got %d bytes, requested %d\n", path, r, (int)file_size);
if (r < 0) {
retval = r;
goto err;
}
else {
panic("read() returned less than the expected file size, but not a negative value... why?");
retval = -EIO;
goto err;
}
}
close(fd);
elf_header_t *header = (elf_header_t *)data;
const unsigned char ELF_IDENT[] = {0x7f, 'E', 'L', 'F'};
if (memcmp(header->e_ident.ei_mag, &ELF_IDENT, 4) != 0) {
printk("Warning: file %s is not an ELF file; aborting execution\n", path);
retval = -ENOEXEC;
goto err;
}
// TODO SECURITY: don't trust anything from the file - users can EASILY execute
// "forged" ELF files!
if (header->e_ident.ei_class != ELFCLASS32 || header->e_ident.ei_data != ELFDATA2LSB || \
header->e_ident.ei_version != 1 || header->e_machine != EM_386 || header->e_type != ET_EXEC) {
printk("Warning: file %s is not a valid ELF file (invalid ELFCLASS, ELFDATA, version, machine or not ET_EXEC\n");
retval = -ENOEXEC;
goto err;
}
assert(header->e_entry >= 0x10000000);
assert(header->e_entry < 0x11000000);
if (task == current_task) {
// execve
assert(current_task->mm != NULL);
assert(current_task->mm->areas != NULL);
assert(current_task->mm->page_directory != NULL);
}
for (int i=0; i < header->e_phnum; i++) {
Elf32_Phdr *phdr = (Elf32_Phdr *)(data + header->e_phoff + header->e_phentsize * i);
if (phdr->p_type == PT_LOAD) {
// This is a segment to load!
// Should this be writable to the task?
bool writable = ((phdr->p_flags & PF_W) ? true : false);
#if ELF_DEBUG
printk("Segment #%u: copy %u bytes from 0x%08x (data + offset) to 0x%08x (virt in task page dir); read%s\n",
i, phdr->p_filesz, data + phdr->p_offset, phdr->p_vaddr, writable ? "-write" : "only");
#endif
if (i == 0)
assert(phdr->p_vaddr == 0x10000000);
else
assert(phdr->p_vaddr > 0x10000000);
uint32 start_addr = phdr->p_vaddr;
uint32 start_addr_aligned = (phdr->p_vaddr & 0xfffff000);
uint32 end_addr = start_addr + phdr->p_memsz;
if (!IS_PAGE_ALIGNED(end_addr)) {
end_addr &= ~(PAGE_SIZE - 1);
end_addr += PAGE_SIZE;
}
if (end_addr > task->mm->brk_start) {
uint32 new_brk = end_addr;
if (!IS_PAGE_ALIGNED(new_brk)) {
new_brk &= ~(PAGE_SIZE - 1);
new_brk += PAGE_SIZE;
}
task->mm->brk_start = new_brk;
task->mm->brk = new_brk;
}
// Allocate memory for this address in the task's address space, set for user mode
vmm_alloc_user(start_addr_aligned, end_addr, mm, writable);
// Switch to the new page directory, so that we can copy the data there
page_directory_t *old_dir = current_directory;
switch_page_directory(mm->page_directory);
// Okay, we should have the memory. Let's clear it (since PARTS may be left empty by the memcpy,
// e.g. the .bss section, and we do want zeroes to be there)
memset((void *)start_addr_aligned, 0, end_addr - start_addr_aligned);
// Copy the segment (e.g. .text + .rodata + .eh_frame, or .data + .bss) to the location
// DO NOT use start_addr_aligned here - we want the program to dictate the exact location
memcpy((void *)start_addr, data + phdr->p_offset, phdr->p_filesz);
switch_page_directory(old_dir);
}
else if (phdr->p_type == PT_GNU_STACK || phdr->p_type == PT_GNU_RELRO || phdr->p_type == PT_GNU_EH_FRAME) {
// Quietly ignore
}
else
printk("Warning: skipping unsupported ELF program header (#%u, p_type = 0x%x)\n", i, phdr->p_type);
}
// Set up the reentrancy structure for Newlib
// (It is initialized below, after switching to the new page directory.)
uint32 reent_size = sizeof(struct _reent);
if (reent_size & 0xfff) {
reent_size &= 0xfffff000;
reent_size += PAGE_SIZE;
}
vmm_alloc_user(task->mm->brk, task->mm->brk + reent_size, mm, PAGE_RW);
//assert(current_directory == kernel_directory);
page_directory_t *old_dir = current_directory;
switch_page_directory(task->mm->page_directory);
task->reent = (struct _reent *)task->mm->brk;
_REENT_INIT_PTR(task->reent);
task->mm->brk += reent_size;
task->mm->brk_start += reent_size;
assert(IS_PAGE_ALIGNED(task->mm->brk));
assert(task->mm->brk == task->mm->brk_start);
// The value brk has when the process starts;
// userspace may not decrease the brk point below this address
task->mm->initial_brk = task->mm->brk_start;
// Copy the argv data from the kernel heap to the task's address space
// This function updates argv to point to the new location.
uint32 argc = 0;
for (; argv[argc] != NULL; argc++) { }
copy_argv_env_to_task(&argv, argc, task);
uint32 envc = 0;
assert(envp != NULL);
for (; envp[envc] != NULL; envc++) { }
copy_argv_env_to_task(&envp, envc, task);
*((uint32 *)(USER_STACK_START - 0)) = (uint32)envp;
*((uint32 *)(USER_STACK_START - 4)) = (uint32)argv;
*((uint32 *)(USER_STACK_START - 8)) = (uint32)argc;
// Update the task's name
strlcpy((char *)task->name, argv[0], TASK_NAME_LEN);
if (old_dir != kernel_directory) {
// execve, stay with the new dir
}
else
switch_page_directory(old_dir);
#if ELF_DEBUG
printk("File has %u program headers (each %u bytes), %u section headers (each %u bytes)\n",
header->e_phnum, header->e_phentsize, header->e_shnum, header->e_shentsize);
printk("Program Header:\n");
for (int i=0; i < header->e_phnum; i++) {
Elf32_Phdr *phdr = (Elf32_Phdr *)(data + header->e_phoff + header->e_phentsize * i);
if (phdr->p_type == PT_LOAD) {
printk("LOAD offset 0x%08x vaddr 0x%08x alignment %u bytes\n", phdr->p_offset, phdr->p_vaddr, phdr->p_align);
unsigned int f = phdr->p_flags;
printk(" filesz 0x%08x memsz 0x%08x flags %c%c%c\n", phdr->p_filesz, phdr->p_memsz,
(f & PF_R ? 'r' : '-'), (f & PF_W ? 'w' : '-'), (f & PF_X ? 'x' : '-'));
}
else {
printk("unsupported program header (#%u), skipping\n", i);
}
}
// Find the string table
assert(header->e_shoff != 0); // we need a section header
Elf32_Shdr *string_table_hdr = (Elf32_Shdr *)(data + header->e_shoff + header->e_shentsize * header->e_shstrndx);
char *string_table = (char *)(data + string_table_hdr->sh_offset);
printk("Sections:\n");
printk("Idx Name Size VMA LMA File off Align\n");
for (int i=1; i < header->e_shnum; i++) { // skip #0, which is always empty
Elf32_Shdr *shdr = (Elf32_Shdr *)(data + header->e_shoff + header->e_shentsize * i);
char *name = (char *)&string_table[shdr->sh_name];
printk("%03d %12s %08x %08x %08x %08x %u\n", i, name, shdr->sh_size, shdr->sh_addr, shdr->sh_addr /* TODO: LMA */, shdr->sh_offset, shdr->sh_addralign);
unsigned int f = shdr->sh_flags;
printk(" ");
if (shdr->sh_type != SHT_NOBITS)
printk("CONTENTS, ");
if ((f & SHF_ALLOC))
printk("ALLOC, ");
if ((f & SHF_WRITE) == 0)
printk("READONLY, ");
if ((f & SHF_EXECINSTR))
printk("CODE\n");
else
printk("DATA\n");
}
#endif // ELF_DEBUG
// Try to find symbols, so we can get nice backtrace displays
Elf32_Sym *symhdr = NULL;
uint32 num_syms = 0;
const char *sym_string_table = NULL;
uint32 string_table_size = 0;
for (uint32 i=1; i < header->e_shnum; i++) { // skip #0, which is always empty
Elf32_Shdr *shdr = (Elf32_Shdr *)((uint32)data + header->e_shoff + (header->e_shentsize * i));
if (shdr->sh_type == SHT_SYMTAB) {
symhdr = (Elf32_Sym *)(data + shdr->sh_offset);
num_syms = shdr->sh_size / shdr->sh_entsize;
Elf32_Shdr *string_table_hdr = (Elf32_Shdr *)((uint32)data + header->e_shoff + shdr->sh_link * header->e_shentsize);
string_table_size = string_table_hdr->sh_size;
sym_string_table = (char *)(data + string_table_hdr->sh_offset);
break;
}
}
// Load symbols for this file, so that we can display them in backtraces
if (!symhdr || !sym_string_table || num_syms < 1) {
printk("Warning: failed to load symbols for %s\n", path);
}
else {
// Clone the string table. Because load_symbols doesn't strdup() names
// for performance reasons, we need the string table to keep existing
// for as long as the task lives.
char *old_table = task->symbol_string_table;
task->symbol_string_table = kmalloc(string_table_size);
task->symbol_string_table_size = string_table_size;
memcpy(task->symbol_string_table, sym_string_table, string_table_size);
if (load_symbols(symhdr, task->symbol_string_table, &task->symbols, num_syms) != 0) {
printk("Warning: failed to load symbols for %s\n", path);
}
else if (old_table) {
// execve, so free the old one, or it'll leak
kfree(old_table);
}
}
// If we're still here: set the program entry point
// (This updates the value on the stack in task.c)
task->new_entry = (uint32)header->e_entry;
set_entry_point((task_t *)task, task->new_entry);
retval = 0;
/* fall through on success */
err:
kfree(data);
assert(retval <= 0);
return retval;
}
int main(void)
{
uint32_t ppg; //PPG channel
uint32_t data_counter=0; //used as data timestamp
uint8_t system_state=0; //used to track button press functionality
float sensor_data; //used for handling data passed back from sensors
RTC_t RTC_time;
_REENT_INIT_PTR(&my_reent);
_impure_ptr = &my_reent;
SystemInit(); //Sets up the clk
setup_gpio(); //Initialised pins, and detects boot source
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); //Watchdog stopped during JTAG halt
if(RCC->CSR&RCC_CSR_IWDGRSTF) { //Watchdog reset, turn off
RCC->CSR|=RCC_CSR_RMVF; //Reset the reset flags
shutdown();
}
SysTick_Configuration(); //Start up system timer at 100Hz for uSD card functionality
Watchdog_Config(WATCHDOG_TIMEOUT); //Set the watchdog
Watchdog_Reset(); //Reset watchdog as soon as possible incase it is still running at power on
rtc_init(); //Real time clock initialise - (keeps time unchanged if set)
Usarts_Init();
ISR_Config();
rprintfInit(__usart_send_char); //Printf over the bluetooth
if(USB_SOURCE==bootsource) {
Set_System(); //This actually just inits the storage layer
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
uint32_t nojack=0x000FFFFF; //Countdown timer - a few hundered ms of 0v on jack detect forces a shutdown
while (bDeviceState != CONFIGURED) { //Wait for USB config - timeout causes shutdown
if(Millis>10000 || !nojack) //No USB cable - shutdown (Charger pin will be set to open drain, cant be disabled without usb)
shutdown();
if(GET_VBUS_STATE) //Jack detect resets the countdown
nojack=0x0FFFFF;
nojack--;
Watchdog_Reset(); //Reset watchdog here, if we are stalled here the Millis timeout should catch us
}
USB_Configured_LED();
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
while(1) { //If running off USB (mounted as mass storage), stay in this loop - dont turn on anything
if(Millis%1000>500) //1Hz on/off flashing
switch_leds_on(); //Flash the LED(s)
else
switch_leds_off();
Watchdog_Reset();
}
}
if(!GET_PWR_STATE) //Check here to make sure the power button is still pressed, if not, sleep
shutdown(); //This means a glitch on the supply line, or a power glitch results in sleep
for(uint8_t n=0;n<PPG_CHANNELS;n++)
init_buffer(&(Buff[n]),PPG_BUFFER_SIZE);//Enough for ~0.25S of data
setup_pwm(); //Enable the PWM outputs on all three channels
Delay(100000); //Sensor+inst amplifier takes about 100ms to stabilise after power on
ADC_Configuration(); //We leave this a bit later to allow stabilisation
calibrate_sensor(); //Calibrate the offset on the diff pressure sensor
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
I2C_Config(); //Setup the I2C bus
uint8_t sensors_=detect_sensors(); //Search for connected sensors
sensor_data=GET_BATTERY_VOLTAGE; //Have to flush adc for some reason
Delay(10000);
if(!(sensors_&~(1<<PRESSURE_HOSE))||GET_BATTERY_VOLTAGE<BATTERY_STARTUP_LIMIT) {//We will have to turn off
Watchdog_Reset(); //LED flashing takes a while
if(abs(Reported_Pressure)>PRESSURE_MARGIN)
Set_Motor(-1); //If the is air backpressure, dump to rapidly drop to zero pressure before turnoff
if(file_opened)
f_close(&FATFS_logfile); //be sure to terminate file neatly
red_flash();
Delay(400000);
red_flash(); //Two flashes means battery abort -----------------ABORT 2
if(sensors_&~(1<<PRESSURE_HOSE))
shutdown();
Delay(400000);
red_flash(); //Three flashes means no sensors abort ------------ABORT 3
shutdown();
}
if((f_err_code = f_mount(0, &FATFS_Obj)))Usart_Send_Str((char*)"FatFs mount error\r\n");//This should only error if internal error
else { //FATFS initialised ok, try init the card, this also sets up the SPI1
if(!f_open(&FATFS_logfile,"time.txt",FA_OPEN_EXISTING | FA_READ | FA_WRITE)) {//Try and open a time file to get the system time
if(!f_stat((const TCHAR *)"time.txt",&FATFS_info)) {//Get file info
if(!FATFS_info.fsize) { //Empty file
RTC_time.year=(FATFS_info.fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
RTC_time.month=(FATFS_info.fdate>>5)&0x000F;
RTC_time.mday=FATFS_info.fdate&0x001F;
RTC_time.hour=(FATFS_info.ftime>>11)&0x001F;
RTC_time.min=(FATFS_info.ftime>>5)&0x003F;
RTC_time.sec=(FATFS_info.ftime<<1)&0x003E;
rtc_settime(&RTC_time);
rprintfInit(__fat_print_char);//printf to the open file
printf("RTC set to %d/%d/%d %d:%d:%d\n",RTC_time.mday,RTC_time.month,RTC_time.year,\
RTC_time.hour,RTC_time.min,RTC_time.sec);
}
}
f_close(&FATFS_logfile); //Close the time.txt file
}
#ifndef SINGLE_LOGFILE
rtc_gettime(&RTC_time); //Get the RTC time and put a timestamp on the start of the file
rprintfInit(__str_print_char); //Print to the string
printf("%d-%d-%dT%d-%d-%d.txt",RTC_time.year,RTC_time.month,RTC_time.mday,RTC_time.hour,RTC_time.min,RTC_time.sec);//Timestamp name
rprintfInit(__usart_send_char); //Printf over the bluetooth
#endif
if((f_err_code=f_open(&FATFS_logfile,LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Present
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
}
else { //We have a mounted card
f_err_code=f_lseek(&FATFS_logfile, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_logfile) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_logfile);//Close the already opened file on error
else
file_opened=1; //So we know to close the file properly on shutdown
}
}
/// Allocates a struct reent. Overrides the (weak) definition to put it to a
/// separate RAM segment and leave more heap space free.
/// @return a newly allocated struct reent. Cannot be freed.
struct _reent* allocate_reent(void)
{
struct _reent* data = usb_malloc(sizeof(struct _reent));
_REENT_INIT_PTR(data);
return data;
}
