Микроконтроллеры AVR

Переношу асмовские наработки на си. Очередные затруднения. Есть очередь таймеров в ОЗУ. Блоки по 3 байта. Первый байт-статус. Два - счетчик. Подобные конструкции у меня часты. Также есть наработки с памятью программ. Не сочтите за халявщика, но сейчас я вперся. После ваших советов, в примерах из инета, не буду говорить, какие источники, примерщики по сравнению с вами сплошные дилетанты. Иногда даже я вижу дикость, не знаю в чем, но вижу :) Для дальнейшей работы и изучения хотел бы попросить примеры, как мне реализовать асмовскую подпрограмму: //======================================================================== Service_Timers: // Служба таймеров. sbrs FLAGS,SYS_TICK_FLG // Флаг очередного отсчета системного таймера установлен? ret // Нет, выход. cbr FLAGS,1<<SYS_TICK_FLG // Установлен, сброс флага. ldy ST_CNT_QUEUE // Указатель на начало очереди системных таймеров. clr CNT Service_Timers_ST_Cycle: ld r16,Y // Считывание байта-статуса системного таймера. sbrs r16,ST_UNLOCK_FLG // Таймер заблокирован? rjmp Process_Next_System_Timer // Да, переход к обработке следующего таймера. ldd r24,Y+1 // Считывание младшего байта таймера. ldd r25,Y+2 // Считывание старшего байта таймера. sbiw r24,1 // Уменьшаем значение таймера на единицу. std Y+1,r24 // Сохранение значения таймера. std Y+2,r25 // Сохранение значения таймера. brne Process_Next_System_Timer // Если время не вышло, то переход к обработке следующего таймера. sbr r16,1<<ST_TIME_OUT_FLG // Установка флага завершения работы таймера. Save_ST_Flags: st Y,r16 // Запись статуса таймера. Process_Next_System_Timer: adiw YL,3 // Смещение указателя на следущий таймер. inc CNT cpi CNT,ST_QUANTITY // Значение счетчика таймеров равно количеству таймеров? brlo Service_Timers_ST_Cycle // Меньше, обрабатываем следующие таймеры. Service_Timers_End: ret //======================================================================== Хидер: //======================================================================== //--------------------- Флаги системных таймеров ------------------------- #define SYS_TICK_FLG 0 //------------------------------------------------------------------------ //------------------------------------------------------------------------ #define ST_UNLOCK_FLG 0 // Флаг активности таймера. #define ST_TIME_OUT_FLG 1 // Время вышло. //======================================================================== //======================================================================== extern u08 sys_tick; //======================================================================== #define ST_QUANTITY 3 struct {u08 st_flags; u16 st_cnt} Sys_Timers_Queue [ST_QUANTITY]; Сишный файл: void service_timers(void) { if (sys_tick & (1<<SYS_TICK_FLG) == 1) { sys_tick &= ~(1<<SYS_TICK_FLG); volatile u08 cnt; for(cnt=0; cnt != ST_QUANTITY+1; cnt++); { if(Sys_Timers_Queue[cnt].st_flags & (1<<ST_UNLOCK_FLG) == 0) continue; else (st_flags |= (1<<ST_TIME_OUT_FLG)); } } } Работа с памятью программ: .macro Get_Keys_Func ldz @0 rcall _Get_Keys_Func .endmacro _Get_Keys_Func: clr r17 addw Z, r16, r17 shiftlwz lpm r16, Z+ lpm ZL, Z mov ZH, r16 Get_Keys_Func_Cycle: ser r16 lpm r17, Z+ cp r17, r16 lpm r17, Z cpc r17, r16 breq Get_Keys_Func_End sbiw ZL, 1 lpm r17, Z+ cp FSM_STATE, r17 breq Get_Keys_Func_Equal adiw ZL, 3 rjmp Get_Keys_Func_Cycle Get_Keys_Func_Equal: lpm FSM_STATE, Z+ ser r16 sts KEYS_BUF, r16 lpm r17, Z+ lpm r16, Z+ movw ZH:ZL, r17:r16 in XL, SPL in XH, SPH adiw XL, 2 out SPH, XH out SPL, XL ijmp Get_Keys_Func_End: ret //======================================================================== Proc_Game_Save_Val: std Y„_PROC_GAME, FSM_STATE std Y„_PROC_GAME_FLAGS, FSM_FLAGS ret //======================================================================== //======================================================================== Proc_Game: pushiwl Proc_Game_Save_Val // Стек на сохранение переменных ldy _PROC_GAME // Указатель на переменные. ldd FSM_STATE, Y+DISP_PROC_GAME // Считывание состояния автомата. ldd FSM_FLAGS, Y+DISP_PROC_GAME_FLAGS Proc_Game_Check_Keys: lds r16, KEYS_BUF cpi r16, 0xFF breq Proc_FSM_Game Get_Keys_Func Handlers_Keys_Proc_Game Proc_FSM_Game: Proc_FSM Tab_FSM_PROC_GAME // Индексный переход. Состояние автомата - индекс обработчиков состояний. Handler_Key_Left: // CURR_STATE, NEXT_STATE, FUNC key_func _PROC_GAME_RUN_1, _PROC_GAME_RUN_1, Shift_Car_Left end_array // 0xFF, 0xFF