git.gag.com Git - fw/altos/blob - src-avr/ao_task.c

   1 /*
   2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include "ao.h"
  19
  20 #define AO_NO_TASK_INDEX        0xff
  21
  22 __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS];
  23 __data uint8_t ao_num_tasks;
  24 __data uint8_t ao_cur_task_index;
  25 __xdata struct ao_task *__data ao_cur_task;
  26
  27 #ifdef AVR
  28
  29 #define PUSH8(stack, val)       (*((stack)--) = (val))
  30
  31 static void
  32 ao_init_stack(__xdata struct ao_task *task, void (*start)(void))
  33 {
  34         uint8_t         *sp = task->stack + AO_STACK_SIZE - 1;
  35         uint16_t        a = (uint16_t) start;
  36         int             i;
  37
  38         /* Return address */
  39         PUSH8(sp, a);
  40         PUSH8(sp, (a >> 8));
  41
  42         /* Clear register values */
  43         i = 32;
  44         while (i--)
  45                 PUSH8(sp, 0);
  46
  47         /* SREG with interrupts enabled */
  48         PUSH8(sp, 0x80);
  49         task->sp = sp;
  50 }
  51 #else
  52 static void
  53 ao_init_stack(__xdata struct ao_task *task, void (*start)(void))
  54 {
  55         uint8_t __xdata *stack = task->stack;
  56         /*
  57          * Construct a stack frame so that it will 'return'
  58          * to the start of the task
  59          */
  60
  61         *stack++ = ((uint16_t) start);
  62         *stack++ = ((uint16_t) start) >> 8;
  63
  64         /* and the stuff saved by ao_switch */
  65         *stack++ = 0;           /* acc */
  66         *stack++ = 0x80;        /* IE */
  67         *stack++ = 0;           /* DPL */
  68         *stack++ = 0;           /* DPH */
  69         *stack++ = 0;           /* B */
  70         *stack++ = 0;           /* R2 */
  71         *stack++ = 0;           /* R3 */
  72         *stack++ = 0;           /* R4 */
  73         *stack++ = 0;           /* R5 */
  74         *stack++ = 0;           /* R6 */
  75         *stack++ = 0;           /* R7 */
  76         *stack++ = 0;           /* R0 */
  77         *stack++ = 0;           /* R1 */
  78         *stack++ = 0;           /* PSW */
  79         *stack++ = 0;           /* BP */
  80         task->stack_count = stack - task->stack;
  81 }
  82 #endif
  83
  84 void
  85 ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant
  86 {
  87         uint8_t task_id;
  88         uint8_t t;
  89         if (ao_num_tasks == AO_NUM_TASKS)
  90                 ao_panic(AO_PANIC_NO_TASK);
  91         for (task_id = 1; task_id != 0; task_id++) {
  92                 for (t = 0; t < ao_num_tasks; t++)
  93                         if (ao_tasks[t]->task_id == task_id)
  94                                 break;
  95                 if (t == ao_num_tasks)
  96                         break;
  97         }
  98         ao_tasks[ao_num_tasks++] = task;
  99         task->task_id = task_id;
 100         task->name = name;
 101         task->wchan = NULL;
 102         ao_init_stack(task, start);
 103 }
 104
 105 /* Task switching function. This must not use any stack variables */
 106 void
 107 ao_yield(void) __naked
 108 {
 109 #ifdef AVR
 110         asm("push r31" "\n\t" "push r30");
 111         asm("push r29" "\n\t" "push r28" "\n\t" "push r27" "\n\t" "push r26" "\n\t" "push r25");
 112         asm("push r24" "\n\t" "push r23" "\n\t" "push r22" "\n\t" "push r21" "\n\t" "push r20");
 113         asm("push r19" "\n\t" "push r18" "\n\t" "push r17" "\n\t" "push r16" "\n\t" "push r15");
 114         asm("push r14" "\n\t" "push r13" "\n\t" "push r12" "\n\t" "push r11" "\n\t" "push r10");
 115         asm("push r9" "\n\t" "push r8" "\n\t" "push r7" "\n\t" "push r6" "\n\t" "push r5");
 116         asm("push r4" "\n\t" "push r3" "\n\t" "push r2" "\n\t" "push r1" "\n\t" "push r0");
 117         cli();
 118         asm("in r0, __SREG__" "\n\t" "push r0");
 119         sei();
 120 #else
 121         /* Save current context */
 122         _asm
 123                 /* Push ACC first, as when restoring the context it must be restored
 124                  * last (it is used to set the IE register). */
 125                 push    ACC
 126                 /* Store the IE register then enable interrupts. */
 127                 push    _IEN0
 128                 setb    _EA
 129                 push    DPL
 130                 push    DPH
 131                 push    b
 132                 push    ar2
 133                 push    ar3
 134                 push    ar4
 135                 push    ar5
 136                 push    ar6
 137                 push    ar7
 138                 push    ar0
 139                 push    ar1
 140                 push    PSW
 141         _endasm;
 142         PSW = 0;
 143         _asm
 144                 push    _bp
 145         _endasm;
 146 #endif
 147
 148         if (ao_cur_task_index == AO_NO_TASK_INDEX)
 149                 ao_cur_task_index = ao_num_tasks-1;
 150         else
 151         {
 152 #ifdef AVR
 153                 uint8_t sp_l, sp_h;
 154                 asm("in %0,__SP_L__" : "=&r" (sp_l) );
 155                 asm("in %0,__SP_H__" : "=&r" (sp_h) );
 156                 ao_cur_task->sp = (uint8_t *) ((uint16_t) sp_l | ((uint16_t) sp_h << 8));
 157 #else
 158                 uint8_t stack_len;
 159                 __data uint8_t *stack_ptr;
 160                 __xdata uint8_t *save_ptr;
 161                 /* Save the current stack */
 162                 stack_len = SP - (AO_STACK_START - 1);
 163                 ao_cur_task->stack_count = stack_len;
 164                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 165                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 166                 do
 167                         *save_ptr++ = *stack_ptr++;
 168                 while (--stack_len);
 169 #endif
 170         }
 171
 172 #ifndef AVR
 173         /* Empty the stack; might as well let interrupts have the whole thing */
 174         SP = AO_STACK_START - 1;
 175 #endif
 176
 177         /* Find a task to run. If there isn't any runnable task,
 178          * this loop will run forever, which is just fine
 179          */
 180         {
 181                 __pdata uint8_t ao_next_task_index = ao_cur_task_index;
 182                 for (;;) {
 183                         ++ao_next_task_index;
 184                         if (ao_next_task_index == ao_num_tasks)
 185                                 ao_next_task_index = 0;
 186
 187                         ao_cur_task = ao_tasks[ao_next_task_index];
 188                         if (ao_cur_task->wchan == NULL) {
 189                                 ao_cur_task_index = ao_next_task_index;
 190                                 break;
 191                         }
 192
 193                         /* Check if the alarm is set for a time which has passed */
 194                         if (ao_cur_task->alarm &&
 195                             (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) {
 196                                 ao_cur_task_index = ao_next_task_index;
 197                                 break;
 198                         }
 199
 200                         /* Enter lower power mode when there isn't anything to do */
 201                         if (ao_next_task_index == ao_cur_task_index)
 202 #ifdef AVR
 203                                 sleep_cpu();
 204 #else
 205                                 PCON = PCON_IDLE;
 206 #endif
 207                 }
 208         }
 209
 210 #ifdef AVR
 211         {
 212                 uint8_t sp_l, sp_h;
 213                 sp_l = (uint16_t) ao_cur_task->sp;
 214                 sp_h = ((uint16_t) ao_cur_task->sp) >> 8;
 215                 cli();
 216                 asm("out __SP_H__,%0" : : "r" (sp_h) );
 217                 asm("out __SP_L__,%0" : : "r" (sp_l) );
 218                 asm("pop r0"    "\n\t"
 219                     "out __SREG__, r0");
 220                 asm("pop r0" "\n\t" "pop r1" "\n\t" "pop r2" "\n\t" "pop r3" "\n\t" "pop r4");
 221                 asm("pop r5" "\n\t" "pop r6" "\n\t" "pop r7" "\n\t" "pop r8" "\n\t" "pop r9");
 222                 asm("pop r10" "\n\t" "pop r11" "\n\t" "pop r12" "\n\t" "pop r13" "\n\t" "pop r14");
 223                 asm("pop r15" "\n\t" "pop r16" "\n\t" "pop r17" "\n\t" "pop r18" "\n\t" "pop r19");
 224                 asm("pop r20" "\n\t" "pop r21" "\n\t" "pop r22" "\n\t" "pop r23" "\n\t" "pop r24");
 225                 asm("pop r25" "\n\t" "pop r26" "\n\t" "pop r27" "\n\t" "pop r28" "\n\t" "pop r29");
 226                 asm("pop r30" "\n\t" "pop r31");
 227                 asm("ret");
 228         }
 229 #else
 230         {
 231                 uint8_t stack_len;
 232                 __data uint8_t *stack_ptr;
 233                 __xdata uint8_t *save_ptr;
 234
 235                 /* Restore the old stack */
 236                 stack_len = ao_cur_task->stack_count;
 237                 SP = AO_STACK_START - 1 + stack_len;
 238
 239                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 240                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 241                 do
 242                         *stack_ptr++ = *save_ptr++;
 243                 while (--stack_len);
 244         }
 245
 246         _asm
 247                 pop             _bp
 248                 pop             PSW
 249                 pop             ar1
 250                 pop             ar0
 251                 pop             ar7
 252                 pop             ar6
 253                 pop             ar5
 254                 pop             ar4
 255                 pop             ar3
 256                 pop             ar2
 257                 pop             b
 258                 pop             DPH
 259                 pop             DPL
 260                 /* The next byte of the stack is the IE register.  Only the global
 261                 enable bit forms part of the task context.  Pop off the IE then set
 262                 the global enable bit to match that of the stored IE register. */
 263                 pop             ACC
 264                 JB              ACC.7,0098$
 265                 CLR             _EA
 266                 LJMP    0099$
 267         0098$:
 268                 SETB            _EA
 269         0099$:
 270                 /* Finally pop off the ACC, which was the first register saved. */
 271                 pop             ACC
 272                 ret
 273         _endasm;
 274 #endif
 275 }
 276
 277 uint8_t
 278 ao_sleep(__xdata void *wchan)
 279 {
 280         cli();
 281         __critical {
 282                 ao_cur_task->wchan = wchan;
 283         }
 284         sei();
 285         ao_yield();
 286         ao_cur_task->alarm = 0;
 287         if (ao_cur_task->wchan) {
 288                 ao_cur_task->wchan = NULL;
 289                 return 1;
 290         }
 291         return 0;
 292 }
 293
 294 void
 295 ao_wakeup(__xdata void *wchan)
 296 {
 297         uint8_t i;
 298
 299         for (i = 0; i < ao_num_tasks; i++)
 300                 if (ao_tasks[i]->wchan == wchan)
 301                         ao_tasks[i]->wchan = NULL;
 302 }
 303
 304 void
 305 ao_alarm(uint16_t delay)
 306 {
 307         /* Make sure we sleep *at least* delay ticks, which means adding
 308          * one to account for the fact that we may be close to the next tick
 309          */
 310         if (!(ao_cur_task->alarm = ao_time() + delay + 1))
 311                 ao_cur_task->alarm = 1;
 312 }
 313
 314 void
 315 ao_exit(void) __critical
 316 {
 317         uint8_t i;
 318         ao_num_tasks--;
 319         for (i = ao_cur_task_index; i < ao_num_tasks; i++)
 320                 ao_tasks[i] = ao_tasks[i+1];
 321         ao_cur_task_index = AO_NO_TASK_INDEX;
 322         ao_yield();
 323         /* we'll never get back here */
 324 }
 325
 326 void
 327 ao_task_info(void)
 328 {
 329         uint8_t i;
 330         __xdata struct ao_task *task;
 331
 332         for (i = 0; i < ao_num_tasks; i++) {
 333                 task = ao_tasks[i];
 334                 printf("%12s: wchan %04x\n",
 335                        task->name,
 336                        (int16_t) task->wchan);
 337         }
 338 }
 339
 340 void
 341 ao_start_scheduler(void)
 342 {
 343         ao_cur_task_index = AO_NO_TASK_INDEX;
 344         ao_cur_task = NULL;
 345         ao_yield();
 346 }