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