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         *stack = task->stack + AO_STACK_SIZE - 1;
  35         uint16_t        a = (uint16_t) start;
  36         int             i;
  37
  38         /* Return address */
  39         PUSH8(stack, a);
  40         PUSH8(stack, (a >> 8));
  41
  42         /* Clear register values */
  43         i = 32;
  44         while (i--)
  45                 PUSH8(stack, 0);
  46
  47         /* SREG with interrupts enabled */
  48         PUSH8(stack, 0x80);
  49         task->stack_count = stack - task->stack;
  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         asm("in r0, __SREG__" "\n\t" "push r0");
 118         sei();
 119 #else
 120         /* Save current context */
 121         _asm
 122                 /* Push ACC first, as when restoring the context it must be restored
 123                  * last (it is used to set the IE register). */
 124                 push    ACC
 125                 /* Store the IE register then enable interrupts. */
 126                 push    _IEN0
 127                 setb    _EA
 128                 push    DPL
 129                 push    DPH
 130                 push    b
 131                 push    ar2
 132                 push    ar3
 133                 push    ar4
 134                 push    ar5
 135                 push    ar6
 136                 push    ar7
 137                 push    ar0
 138                 push    ar1
 139                 push    PSW
 140         _endasm;
 141         PSW = 0;
 142         _asm
 143                 push    _bp
 144         _endasm;
 145 #endif
 146
 147         if (ao_cur_task_index == AO_NO_TASK_INDEX)
 148                 ao_cur_task_index = ao_num_tasks-1;
 149         else
 150         {
 151 #ifdef AVR
 152                 uint8_t sp_l, sp_h;
 153                 uint8_t *sp;
 154                 asm("in %0,__SP_L__" : "=&r" (sp_l) );
 155                 asm("in %0,__SP_H__" : "=&r" (sp_h) );
 156                 sp = (uint8_t *) ((uint16_t) sp_l | ((uint16_t) sp_h << 8));
 157                 ao_cur_task->stack_count = sp - ao_cur_task->stack;
 158 #else
 159                 uint8_t stack_len;
 160                 __data uint8_t *stack_ptr;
 161                 __xdata uint8_t *save_ptr;
 162                 /* Save the current stack */
 163                 stack_len = SP - (AO_STACK_START - 1);
 164                 ao_cur_task->stack_count = stack_len;
 165                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 166                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 167                 do
 168                         *save_ptr++ = *stack_ptr++;
 169                 while (--stack_len);
 170 #endif
 171         }
 172
 173 #ifndef AVR
 174         /* Empty the stack; might as well let interrupts have the whole thing */
 175         SP = AO_STACK_START - 1;
 176 #endif
 177
 178         /* Find a task to run. If there isn't any runnable task,
 179          * this loop will run forever, which is just fine
 180          */
 181         {
 182                 __pdata uint8_t ao_next_task_index = ao_cur_task_index;
 183                 for (;;) {
 184                         ++ao_next_task_index;
 185                         if (ao_next_task_index == ao_num_tasks)
 186                                 ao_next_task_index = 0;
 187
 188                         ao_cur_task = ao_tasks[ao_next_task_index];
 189                         if (ao_cur_task->wchan == NULL) {
 190                                 ao_cur_task_index = ao_next_task_index;
 191                                 break;
 192                         }
 193
 194                         /* Check if the alarm is set for a time which has passed */
 195                         if (ao_cur_task->alarm &&
 196                             (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) {
 197                                 ao_cur_task_index = ao_next_task_index;
 198                                 break;
 199                         }
 200
 201 #ifdef AVR
 202 #else
 203                         /* Enter lower power mode when there isn't anything to do */
 204                         if (ao_next_task_index == ao_cur_task_index)
 205                                 PCON = PCON_IDLE;
 206 #endif
 207                 }
 208         }
 209
 210 #ifdef AVR
 211         {
 212                 uint8_t *sp = (ao_cur_task->stack + ao_cur_task->stack_count);
 213                 uint8_t sp_l, sp_h;
 214                 sp_l = (uint16_t) sp;
 215                 sp_h = ((uint16_t) sp) >> 8;
 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         __critical {
 281                 ao_cur_task->wchan = wchan;
 282         }
 283         ao_yield();
 284         ao_cur_task->alarm = 0;
 285         if (ao_cur_task->wchan) {
 286                 ao_cur_task->wchan = NULL;
 287                 return 1;
 288         }
 289         return 0;
 290 }
 291
 292 void
 293 ao_wakeup(__xdata void *wchan)
 294 {
 295         uint8_t i;
 296
 297         for (i = 0; i < ao_num_tasks; i++)
 298                 if (ao_tasks[i]->wchan == wchan)
 299                         ao_tasks[i]->wchan = NULL;
 300 }
 301
 302 void
 303 ao_alarm(uint16_t delay)
 304 {
 305         /* Make sure we sleep *at least* delay ticks, which means adding
 306          * one to account for the fact that we may be close to the next tick
 307          */
 308         if (!(ao_cur_task->alarm = ao_time() + delay + 1))
 309                 ao_cur_task->alarm = 1;
 310 }
 311
 312 void
 313 ao_exit(void) __critical
 314 {
 315         uint8_t i;
 316         ao_num_tasks--;
 317         for (i = ao_cur_task_index; i < ao_num_tasks; i++)
 318                 ao_tasks[i] = ao_tasks[i+1];
 319         ao_cur_task_index = AO_NO_TASK_INDEX;
 320         ao_yield();
 321         /* we'll never get back here */
 322 }
 323
 324 void
 325 ao_task_info(void)
 326 {
 327         uint8_t i;
 328         uint8_t pc_loc;
 329         __xdata struct ao_task *task;
 330
 331         for (i = 0; i < ao_num_tasks; i++) {
 332                 task = ao_tasks[i];
 333                 pc_loc = task->stack_count - 17;
 334                 printf("%12s: wchan %04x pc %04x\n",
 335                        task->name,
 336                        (int16_t) task->wchan,
 337                        (task->stack[pc_loc]) | (task->stack[pc_loc+1] << 8));
 338         }
 339 }
 340
 341 void
 342 ao_start_scheduler(void)
 343 {
 344         ao_cur_task_index = AO_NO_TASK_INDEX;
 345         ao_cur_task = NULL;
 346         ao_yield();
 347 }