void _or1k_support_reent_init() { int c; // TODO: move this to newlib? _or1k_support_reent_ptr = _sbrk_r(_impure_ptr, sizeof(struct _or1k_support_reent*) * NUMCORES); _or1k_support_reent_ptr[0] = &_or1k_support_reent_core0; // Allocate on other cores and copy for (c = 1; c < NUMCORES; c++) { _or1k_support_reent_ptr[c] = _sbrk_r(_impure_ptr, sizeof(struct _or1k_support_reent)); memset(_or1k_support_reent_ptr[c], 0,sizeof(struct _or1k_support_reent)); _or1k_support_reent_ptr[c]->or1k_interrupt_handler_table = _sbrk_r(_impure_ptr, 4 * 32); _or1k_support_reent_ptr[c]->or1k_interrupt_handler_data_ptr_table = _sbrk_r(_impure_ptr, 4 * 32); _or1k_support_reent_ptr[c]->or1k_exception_handler_table = _sbrk_r(_impure_ptr, 4 * 29); memcpy(_or1k_support_reent_ptr[c]->or1k_interrupt_handler_table, _or1k_support_reent_ptr[0]->or1k_interrupt_handler_table, 4*32); memcpy(_or1k_support_reent_ptr[c]->or1k_interrupt_handler_data_ptr_table, _or1k_support_reent_ptr[0]->or1k_interrupt_handler_data_ptr_table, 4*32); memcpy(_or1k_support_reent_ptr[c]->or1k_exception_handler_table, _or1k_support_reent_ptr[0]->or1k_exception_handler_table, 4*29); } }
/** Function sbrk_aligned * Algorithm: * Use sbrk() to obtain more memory and ensure it is CHUNK_ALIGN aligned * Optimise for the case that it is already aligned - only ask for extra * padding after we know we need it */ static void* sbrk_aligned(RARG malloc_size_t s) { char *p, *align_p; p = _sbrk_r(RCALL s); /* sbrk returns -1 if fail to allocate */ if (p == (void *)-1) return p; align_p = (char*)ALIGN_TO((unsigned long)p, CHUNK_ALIGN); if (align_p != p) { /* p is not aligned, ask for a few more bytes so that we have s * bytes reserved from align_p. */ p = _sbrk_r(RCALL align_p - p); if (p == (void *)-1) return p; } return align_p; }
void EventLoop() { CLOCK_ResetWatchdog(); unsigned int time; #ifdef HEAP_DEBUG static int heap = 0; int h = _sbrk_r(NULL, 0); if(h > heap) { printf("heap: %x\n", h); heap = h; } #endif #ifdef TIMING_DEBUG debug_timing(0, 0); #endif priority_ready &= ~(1 << MEDIUM_PRIORITY); if(PWR_CheckPowerSwitch()) { if(! (BATTERY_Check() & BATTERY_CRITICAL)) { CONFIG_SaveModelIfNeeded(); CONFIG_SaveTxIfNeeded(); } if(Transmitter.music_shutdown) { MUSIC_Play(MUSIC_SHUTDOWN); // We wait ~1sec for shutdown music finished time = CLOCK_getms()+700; while(CLOCK_getms()<time); } PWR_Shutdown(); } BUTTON_Handler(); TOUCH_Handler(); if (priority_ready & (1 << LOW_PRIORITY)) { priority_ready &= ~(1 << LOW_PRIORITY); PAGE_Event(); PROTOCOL_CheckDialogs(); TIMER_Update(); TELEMETRY_Alarm(); BATTERY_Check(); AUTODIMMER_Update(); #if DATALOG_ENABLED DATALOG_Update(); #endif GUI_RefreshScreen(); } #ifdef TIMING_DEBUG debug_timing(0, 1); #endif }
void _or1k_init() { #ifdef __OR1K_MULTICORE__ uint32_t c; // Initialize stacks _or1k_stack_core = _sbrk_r(0, sizeof(uint32_t*) * or1k_numcores()); _or1k_exception_stack_core = _sbrk_r(0, sizeof(uint32_t*) * or1k_numcores()); _or1k_stack_core[0] = _or1k_stack_top; _or1k_exception_stack_core[0] = _or1k_exception_stack_top; for (c = 1; c < or1k_numcores(); c++) { _or1k_stack_core[c] = _or1k_stack_core[c-1] - _or1k_stack_size; _or1k_exception_stack_core[c] = _or1k_exception_stack_core[c-1] - _or1k_exception_stack_size; } size_t exc_size = sizeof(void*) * or1k_numcores() * OR1K_NUM_EXCEPTIONS; _or1k_exception_handler_table = _sbrk_r(0, exc_size); size_t int_size = sizeof(void*) * or1k_numcores() * 32; size_t intdata_size = sizeof(void*) * or1k_numcores() * 32; _or1k_interrupt_handler_table = _sbrk_r(0, int_size); _or1k_interrupt_handler_data_ptr_table = _sbrk_r(0, intdata_size); #endif _or1k_reent_init(); #ifdef __OR1K_MULTICORE__ for (c = 0; c < or1k_numcores(); c++) { _or1k_exception_handler_table[c][6] = _or1k_interrupt_handler; } #else _or1k_exception_handler_table[6] = _or1k_interrupt_handler; #endif #ifdef __OR1K_MULTICORE__ _or1k_exception_level = _sbrk_r(0, 4 * or1k_numcores()); for (c = 0; c < or1k_numcores(); c++) { _or1k_exception_level[c] = 0; } #else _or1k_exception_level = 0; #endif }
void EventLoop() { CLOCK_ResetWatchdog(); #ifdef HEAP_DEBUG static int heap = 0; int h = _sbrk_r(NULL, 0); if(h > heap) { printf("heap: %x\n", h); heap = h; } #endif #ifdef TIMING_DEBUG debug_timing(0, 0); #endif priority_ready &= ~(1 << MEDIUM_PRIORITY); #if !HAS_HARD_POWER_OFF if(PWR_CheckPowerSwitch()) { if(! (BATTERY_Check() & BATTERY_CRITICAL)) { PAGE_Test(); CONFIG_SaveModelIfNeeded(); CONFIG_SaveTxIfNeeded(); } if(Transmitter.music_shutdown) { #if HAS_EXTENDED_AUDIO if(AUDIO_VoiceAvailable()) { MUSIC_Play(MUSIC_SHUTDOWN); while (CLOCK_getms() < audio_queue_time) { // Wait for voice to finished CLOCK_ResetWatchdog(); } } else { #else { // We wait ~1sec for shutdown buzzer music finished unsigned int time; MUSIC_Play(MUSIC_SHUTDOWN); time = CLOCK_getms()+700; while (CLOCK_getms() < time) { CLOCK_ResetWatchdog(); } #endif } } PWR_Shutdown(); } #endif BUTTON_Handler(); TOUCH_Handler(); INPUT_CheckChanges(); if (priority_ready & (1 << LOW_PRIORITY)) { priority_ready &= ~(1 << LOW_PRIORITY); PAGE_Event(); PROTOCOL_CheckDialogs(); TIMER_Update(); TELEMETRY_Alarm(); BATTERY_Check(); AUTODIMMER_Update(); #if HAS_DATALOG DATALOG_Update(); #endif #if HAS_VIDEO VIDEO_Update(); #endif #if HAS_EXTENDED_AUDIO AUDIO_CheckQueue(); #endif GUI_RefreshScreen(); #if HAS_HARD_POWER_OFF if (PAGE_ModelDoneEditing()) CONFIG_SaveModelIfNeeded(); CONFIG_SaveTxIfNeeded(); #endif } #ifdef TIMING_DEBUG debug_timing(0, 1); #endif } void TOUCH_Handler() { if(! HAS_TOUCH) return; u32 pen_down=0; static u32 pen_down_last=0; static u32 pen_down_long_at=0; struct touch t; if(SPITouch_IRQ()) { pen_down=1; t=SPITouch_GetCoords(); if (! pen_down_last) pen_down_long_at=CLOCK_getms()+500; } else { pen_down=0; } if(pen_down && (!pen_down_last)) { AUTODIMMER_Check(); GUI_CheckTouch(&t, 0); } if(!pen_down && pen_down_last) { GUI_TouchRelease(); } if(pen_down && pen_down_last) { if(CLOCK_getms()>pen_down_long_at) { GUI_CheckTouch(&t, 1); pen_down_long_at += 100; } } pen_down_last=pen_down; } #if HAS_VIDEO void VIDEO_Update() { static u8 video_enable = 0; static u32 check_standard_ms = 0; // Check if Video is turn on int enabled = MIXER_SourceAsBoolean(Model.videosrc); if (enabled != video_enable) { VIDEO_Enable(enabled); video_enable = enabled; if (enabled) { VIDEO_SetChannel(Model.videoch); VIDEO_Contrast(Model.video_contrast); VIDEO_Brightness(Model.video_brightness); check_standard_ms = CLOCK_getms() + 3000; } else check_standard_ms = 0; } if(video_enable && check_standard_ms > 0 && check_standard_ms < CLOCK_getms()) { u8 video_standard_current = VIDEO_GetStandard(); if((video_standard_current > 0) && (video_standard_current < 8)) { VIDEO_SetStandard(video_standard_current); check_standard_ms = 0; } else { check_standard_ms = CLOCK_getms() + 3000; } } if(video_enable) AUTODIMMER_Check(); } #endif //HAS_VIDEO #ifdef TIMING_DEBUG void debug_timing(u32 type, int startend) { static u32 last_time[2][100]; static u32 loop_time[4][101]; static u32 loop_pos[4] = {-1, -1, -1, -1}; static u32 max_last[2]; static u32 max_loop[4]; static int save_priority; if (type == 0) { if (! startend) save_priority = priority_ready; if (save_priority & (1 << MEDIUM_PRIORITY)) debug_timing(2, startend); if (save_priority & (1 << LOW_PRIORITY)) debug_timing(1, startend); return; } type--; if (! startend) { u32 t = CLOCK_getms(); loop_pos[type] = (loop_pos[type] + 1) % 100; if (type < 2) { last_time[type][loop_pos[type]] = t; if (t - last_time[type][(loop_pos[type] + 99) % 100] > max_last[type]) max_last[type] = t - last_time[type][(loop_pos[type] + 99) % 100]; } loop_time[type][100] = t; } else { loop_time[type][loop_pos[type]] = CLOCK_getms() - loop_time[type][100]; if (loop_time[type][loop_pos[type]] > max_loop[type]) max_loop[type] = loop_time[type][loop_pos[type]]; if (type == 0 && loop_pos[0] == 99) { unsigned avg_loop[4] = {0, 0, 0, 0}; unsigned avg_last[2] = {0, 0}; for(int i = 0; i < 99; i++) { for(int t = 0; t < 2; t++) { u32 delay = last_time[t][(i + loop_pos[t] + 2) % 100] - last_time[t][(i + loop_pos[t] + 1) % 100]; avg_last[t] += delay; } for(int t = 0; t < 4; t++) avg_loop[t] += loop_time[t][i]; } for(int t = 0; t < 4; t++) avg_loop[t] /= 99; avg_last[0] /= 99; avg_last[1] /= 99; printf("Avg: radio: %d mix: %d med: %d/%d low: %d/%d\n", avg_loop[3], avg_loop[2], avg_loop[1], avg_last[1], avg_loop[0], avg_last[0]); printf("Max: radio: %d mix: %d med: %d/%d low: %d/%d\n", max_loop[3], max_loop[2], max_loop[1], max_last[1], max_loop[0], max_last[0]); memset(max_loop, 0, sizeof(max_loop)); max_last[0] = 0; max_last[1] = 0; } } } #endif void debug_switches() { s32 data[INP_LAST]; for(int i = INP_HAS_CALIBRATION+1; i < INP_LAST; i++) { data[i] = CHAN_ReadRawInput(i); } while(1) { u32 changed = 0; for(int i = INP_HAS_CALIBRATION+1; i < INP_LAST; i++) { s32 val = CHAN_ReadRawInput(i); if (val != data[i]) { printf("%s=%d ", INPUT_SourceName(tempstring, i), val); data[i] = val; changed = 1; } } if (changed) { printf("\n"); } if(PWR_CheckPowerSwitch()) PWR_Shutdown(); } }