BeRTOS
|
00001 00055 #include "stepper_at91.h" 00056 00057 #include "cfg/cfg_stepper.h" 00058 #include <cfg/macros.h> 00059 #include <cfg/debug.h> 00060 00061 #include <cpu/types.h> 00062 #include <cpu/irq.h> 00063 00064 #include <io/arm.h> 00065 00066 00067 /* 00068 * Delay to set C compare to clear output 00069 * on select TIO output 00070 */ 00071 #define STEPPER_DELAY_ON_COMPARE_C 20 00072 00073 /* 00074 * Forward declaration for interrupt handler 00075 */ 00076 static ISR_PROTO(stepper_tc0_irq); 00077 static ISR_PROTO(stepper_tc1_irq); 00078 static ISR_PROTO(stepper_tc2_irq); 00079 00081 static struct TimerCounter stepper_timers[CONFIG_TC_STEPPER_MAX_NUM] = 00082 { 00083 { //Timer Counter settings for TIOA0 output pin 00084 .timer_id = TC0_ID, 00085 .blk_ctrl_set = TC_NONEXC0, 00086 .chl_mode_reg = &TC0_CMR, 00087 .chl_ctrl_reg = &TC0_CCR, 00088 .comp_effect_mask = TC_ACPA_MASK, 00089 .comp_effect_set = TC_ACPA_SET_OUTPUT, 00090 .comp_effect_clear = TC_ACPA_CLEAR_OUTPUT, 00091 .comp_effect_c_mask = TC_ACPC_MASK, 00092 .comp_effect_c_clear = TC_ACPC_CLEAR_OUTPUT, 00093 .ext_event_set = TC_EEVT_XC0, 00094 .comp_reg = &TC0_RA, 00095 .comp_c_reg = &TC0_RC, 00096 .count_val_reg = &TC0_CV, 00097 .irq_enable_reg = &TC0_IER, 00098 .irq_disable_reg = &TC0_IDR, 00099 .irq_set_mask = BV(TC_CPAS), 00100 .irq_mask_reg = &TC0_IMR, 00101 .isr = stepper_tc0_irq, 00102 .status_reg = &TC0_SR, 00103 .tio_pin = TIOA0, 00104 .callback = NULL, 00105 .motor = NULL, 00106 }, 00107 { //Timer Counter settings for TIOB0 output pin 00108 .timer_id = TC0_ID, 00109 .blk_ctrl_set = TC_NONEXC0, 00110 .chl_mode_reg = &TC0_CMR, 00111 .chl_ctrl_reg = &TC0_CCR, 00112 .comp_reg = &TC0_RB, 00113 .comp_c_reg = &TC0_RC, 00114 .count_val_reg = &TC0_CV, 00115 .comp_effect_mask = TC_BCPB_MASK, 00116 .comp_effect_set = TC_BCPB_SET_OUTPUT, 00117 .comp_effect_clear = TC_BCPB_CLEAR_OUTPUT, 00118 .comp_effect_c_mask = TC_BCPC_MASK, 00119 .comp_effect_c_clear = TC_BCPC_CLEAR_OUTPUT, 00120 .ext_event_set = TC_EEVT_XC0, 00121 .irq_enable_reg = &TC0_IER, 00122 .irq_disable_reg = &TC0_IDR, 00123 .irq_set_mask = BV(TC_CPBS), 00124 .irq_mask_reg = &TC0_IMR, 00125 .isr = stepper_tc0_irq, 00126 .status_reg = &TC0_SR, 00127 .tio_pin = TIOB0, 00128 .callback = NULL, 00129 .motor = NULL, 00130 }, 00131 { //Timer Counter settings for TIOA1 output pin 00132 .timer_id = TC1_ID, 00133 .blk_ctrl_set = TC_NONEXC1, 00134 .chl_mode_reg = &TC1_CMR, 00135 .chl_ctrl_reg = &TC1_CCR, 00136 .comp_reg = &TC1_RA, 00137 .comp_c_reg = &TC1_RC, 00138 .count_val_reg = &TC1_CV, 00139 .comp_effect_mask = TC_ACPA_MASK, 00140 .comp_effect_set = TC_ACPA_SET_OUTPUT, 00141 .comp_effect_clear = TC_ACPA_CLEAR_OUTPUT, 00142 .comp_effect_c_mask = TC_ACPC_MASK, 00143 .comp_effect_c_clear = TC_ACPC_CLEAR_OUTPUT, 00144 .ext_event_set = TC_EEVT_XC1, 00145 .irq_enable_reg = &TC1_IER, 00146 .irq_disable_reg = &TC1_IDR, 00147 .irq_set_mask = BV(TC_CPAS), 00148 .irq_mask_reg = &TC1_IMR, 00149 .isr = stepper_tc1_irq, 00150 .status_reg = &TC1_SR, 00151 .tio_pin = TIOA1, 00152 .callback = NULL, 00153 .motor = NULL, 00154 }, 00155 { //Timer Counter settings for TIOB1 output pin 00156 .timer_id = TC1_ID, 00157 .blk_ctrl_set = TC_NONEXC1, 00158 .chl_mode_reg = &TC1_CMR, 00159 .chl_ctrl_reg = &TC1_CCR, 00160 .comp_reg = &TC1_RB, 00161 .comp_c_reg = &TC1_RC, 00162 .count_val_reg = &TC1_CV, 00163 .comp_effect_mask = TC_BCPB_MASK, 00164 .comp_effect_set = TC_BCPB_SET_OUTPUT, 00165 .comp_effect_clear = TC_BCPB_CLEAR_OUTPUT, 00166 .comp_effect_c_mask = TC_BCPC_MASK, 00167 .comp_effect_c_clear = TC_BCPC_CLEAR_OUTPUT, 00168 .ext_event_set = TC_EEVT_XC1, 00169 .irq_enable_reg = &TC1_IER, 00170 .irq_disable_reg = &TC1_IDR, 00171 .irq_set_mask = BV(TC_CPBS), 00172 .irq_mask_reg = &TC1_IMR, 00173 .isr = stepper_tc1_irq, 00174 .status_reg = &TC1_SR, 00175 .tio_pin = TIOB1, 00176 .callback = NULL, 00177 .motor = NULL, 00178 }, 00179 { //Timer Counter settings for TIOA2 output pin 00180 .timer_id = TC2_ID, 00181 .blk_ctrl_set = TC_NONEXC2, 00182 .chl_mode_reg = &TC2_CMR, 00183 .chl_ctrl_reg = &TC2_CCR, 00184 .comp_reg = &TC2_RA, 00185 .comp_c_reg = &TC2_RC, 00186 .count_val_reg = &TC2_CV, 00187 .comp_effect_mask = TC_ACPA_MASK, 00188 .comp_effect_set = TC_ACPA_SET_OUTPUT, 00189 .comp_effect_clear = TC_ACPA_CLEAR_OUTPUT, 00190 .comp_effect_c_mask = TC_ACPC_MASK, 00191 .comp_effect_c_clear = TC_ACPC_CLEAR_OUTPUT, 00192 .ext_event_set = TC_EEVT_XC2, 00193 .irq_enable_reg = &TC2_IER, 00194 .irq_disable_reg = &TC2_IDR, 00195 .irq_set_mask = BV(TC_CPAS), 00196 .irq_mask_reg = &TC2_IMR, 00197 .isr = stepper_tc2_irq, 00198 .status_reg = &TC2_SR, 00199 .tio_pin = TIOA2, 00200 .callback = NULL, 00201 .motor = NULL, 00202 }, 00203 { //Timer Counter settings for TIOB2 output pin 00204 .timer_id = TC2_ID, 00205 .blk_ctrl_set = TC_NONEXC2, 00206 .chl_mode_reg = &TC2_CMR, 00207 .chl_ctrl_reg = &TC2_CCR, 00208 .comp_reg = &TC2_RB, 00209 .comp_c_reg = &TC2_RC, 00210 .count_val_reg = &TC2_CV, 00211 .comp_effect_mask = TC_BCPB_MASK, 00212 .comp_effect_set = TC_BCPB_SET_OUTPUT, 00213 .comp_effect_clear = TC_BCPB_CLEAR_OUTPUT, 00214 .comp_effect_c_mask = TC_BCPC_MASK, 00215 .comp_effect_c_clear = TC_BCPC_CLEAR_OUTPUT, 00216 .ext_event_set = TC_EEVT_XC2, 00217 .irq_enable_reg = &TC2_IER, 00218 .irq_disable_reg = &TC2_IDR, 00219 .irq_set_mask = BV(TC_CPBS), 00220 .irq_mask_reg = &TC2_IMR, 00221 .isr = stepper_tc2_irq, 00222 .status_reg = &TC2_SR, 00223 .tio_pin = TIOB2, 00224 .callback = NULL, 00225 .motor = NULL, 00226 } 00227 }; 00228 00232 INLINE void stepper_tc_tio_irq(struct TimerCounter * t) 00233 { 00234 // 00235 *t->chl_mode_reg &= ~t->comp_effect_c_mask; 00236 *t->chl_mode_reg |= t->comp_effect_c_clear; 00237 00238 /* 00239 * Cleat TIO output on c register compare. 00240 * This generate an pulse with variable lenght, this 00241 * depend to delay that interrupt is realy service. 00242 */ 00243 *t->comp_c_reg = *t->count_val_reg + STEPPER_DELAY_ON_COMPARE_C; 00244 00245 //Call the associate callback 00246 t->callback(t->motor); 00247 00248 *t->chl_mode_reg &= ~t->comp_effect_c_mask; 00249 } 00250 00251 00252 /* 00253 * Interrupt handler for timer counter TCKL0 00254 */ 00255 DECLARE_ISR(stepper_tc0_irq) 00256 { 00257 /* 00258 * Warning: when we read the status_reg register, we reset it. 00259 * That mean if is occur an interrupt event we can read only 00260 * the last that has been occur. To not miss an interrupt event 00261 * we save the status_reg register and then we read it. 00262 */ 00263 uint32_t status_reg = TC0_SR & TC0_IMR; 00264 00265 if (status_reg & BV(TC_CPAS)) 00266 stepper_tc_tio_irq(&stepper_timers[TC_TIOA0]); 00267 00268 if (status_reg & BV(TC_CPBS)) 00269 stepper_tc_tio_irq(&stepper_timers[TC_TIOB0]); 00270 00271 /* Inform hw that we have served the IRQ */ 00272 AIC_EOICR = 0; 00273 00274 } 00275 00276 /* 00277 * Interrupt handler for timer counter TCKL1 00278 */ 00279 DECLARE_ISR(stepper_tc1_irq) 00280 { 00281 /* 00282 * Warning: when we read the status_reg register, we reset it. 00283 * That mean if is occur an interrupt event we can read only 00284 * the last that has been occur. To not miss an interrupt event 00285 * we save the status_reg register and then we read it. 00286 */ 00287 uint32_t status_reg = TC1_SR & TC1_IMR; 00288 00289 if (status_reg & BV(TC_CPAS)) 00290 stepper_tc_tio_irq(&stepper_timers[TC_TIOA1]); 00291 00292 if (status_reg & BV(TC_CPBS)) 00293 stepper_tc_tio_irq(&stepper_timers[TC_TIOB1]); 00294 00295 00296 /* Inform hw that we have served the IRQ */ 00297 AIC_EOICR = 0; 00298 } 00299 00300 00301 /* 00302 * Interrupt handler for timer counter TCKL2 00303 */ 00304 DECLARE_ISR(stepper_tc2_irq) 00305 { 00306 00307 /* 00308 * Warning: when we read the status_reg register, we reset it. 00309 * That mean if is occur an interrupt event we can read only 00310 * the last that has been occur. To not miss an interrupt event 00311 * we save the status_reg register and then we read it. 00312 */ 00313 uint32_t status_reg = TC2_SR & TC2_IMR; 00314 00315 if (status_reg & BV(TC_CPAS)) 00316 stepper_tc_tio_irq(&stepper_timers[TC_TIOA2]); 00317 00318 if (status_reg & BV(TC_CPBS)) 00319 stepper_tc_tio_irq(&stepper_timers[TC_TIOB2]); 00320 00321 /* Inform hw that we have served the IRQ */ 00322 AIC_EOICR = 0; 00323 00324 } 00325 00332 void stepper_tc_setup(int index, stepper_isr_t callback, struct Stepper *motor) 00333 { 00334 ASSERT(index < CONFIG_TC_STEPPER_MAX_NUM); 00335 00336 motor->timer = &stepper_timers[index]; 00337 00338 //Disable PIO controller and enable TIO function 00339 TIO_PIO_PDR = BV(motor->timer->tio_pin); 00340 TIO_PIO_ABSR = BV(motor->timer->tio_pin); 00341 00342 /* 00343 * Sets timer counter in waveform mode. 00344 * We set as default: 00345 * - Waveform mode 00 (see datasheet for more detail.) 00346 * - Master clock prescaler to STEPPER_MCK_PRESCALER 00347 * - Set none external event 00348 * - Clear pin output on comp_reg 00349 * - None effect on reg C compare 00350 */ 00351 *motor->timer->chl_mode_reg = BV(TC_WAVE); 00352 *motor->timer->chl_mode_reg |= motor->timer->ext_event_set; 00353 *motor->timer->chl_mode_reg &= ~TC_WAVSEL_MASK; 00354 *motor->timer->chl_mode_reg |= TC_WAVSEL_UP; 00355 *motor->timer->chl_mode_reg |= STEPPER_MCK_PRESCALER; 00356 *motor->timer->chl_mode_reg |= motor->timer->comp_effect_clear; 00357 *motor->timer->chl_mode_reg &= ~motor->timer->comp_effect_c_mask; 00358 00359 //Reset comp_reg and C compare register 00360 *motor->timer->comp_reg = 0; 00361 *motor->timer->comp_c_reg = 0; 00362 00363 //Register interrupt vector 00364 cpu_flags_t flags; 00365 IRQ_SAVE_DISABLE(flags); 00366 00367 /* 00368 * Warning: To guarantee a correct management of interrupt event, we must 00369 * trig the interrupt on level sensitive. This becouse, we have only a common 00370 * line for interrupt request, and if we have at the same time two interrupt 00371 * request could be that the is service normaly but the second will never 00372 * been detected and interrupt will stay active but never serviced. 00373 */ 00374 AIC_SVR(motor->timer->timer_id) = motor->timer->isr; 00375 AIC_SMR(motor->timer->timer_id) = AIC_SRCTYPE_INT_LEVEL_SENSITIVE; 00376 AIC_IECR = BV(motor->timer->timer_id); 00377 00378 // Disable interrupt on select timer counter 00379 stepper_tc_irq_disable(motor->timer); 00380 00381 IRQ_RESTORE(flags); 00382 00383 //Register callback 00384 motor->timer->callback = callback; 00385 motor->timer->motor = motor; 00386 } 00387 00391 void stepper_tc_init(void) 00392 { 00393 STEPPER_STROBE_INIT; 00394 00395 ASSERT(CONFIG_NUM_STEPPER_MOTORS <= CONFIG_TC_STEPPER_MAX_NUM); 00396 00397 /* 00398 * Enable timer counter: 00399 * - power on all timer counter 00400 * - disable all interrupt 00401 * - disable all external event/timer source 00402 */ 00403 for (int i = 0; i < CONFIG_TC_STEPPER_MAX_NUM; i++) 00404 { 00405 PMC_PCER = BV(stepper_timers[i].timer_id); 00406 *stepper_timers[i].irq_disable_reg = 0xFFFFFFFF; 00407 TC_BMR = stepper_timers[i].blk_ctrl_set; 00408 } 00409 00410 /* 00411 * Enable timer counter and start it. 00412 */ 00413 for (int i = 0; i < CONFIG_TC_STEPPER_MAX_NUM; i++) 00414 *stepper_timers[i].chl_ctrl_reg = (BV(TC_CLKEN) | BV(TC_SWTRG)); 00415 00416 } 00417