git.gag.com Git - fw/altos/blob - 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; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #include "ao.h"
  20
  21 #define AO_NO_TASK      0xff
  22
  23 __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS];
  24 __data uint8_t ao_num_tasks;
  25 __data uint8_t ao_cur_task_id;
  26 __xdata struct ao_task *__data ao_cur_task;
  27
  28 void
  29 ao_add_task(__xdata struct ao_task * task, void (*start)(void))
  30 {
  31         uint8_t __xdata *stack;
  32         if (ao_num_tasks == AO_NUM_TASKS)
  33                 ao_panic(AO_ERROR_NO_TASK);
  34         ao_tasks[ao_num_tasks++] = task;
  35         /*
  36          * Construct a stack frame so that it will 'return'
  37          * to the start of the task
  38          */
  39         stack = task->stack;
  40
  41         *stack++ = ((uint16_t) start);
  42         *stack++ = ((uint16_t) start) >> 8;
  43
  44         /* and the stuff saved by ao_switch */
  45         *stack++ = 0;           /* acc */
  46         *stack++ = 0x80;        /* IE */
  47         *stack++ = 0;           /* DPL */
  48         *stack++ = 0;           /* DPH */
  49         *stack++ = 0;           /* B */
  50         *stack++ = 0;           /* R2 */
  51         *stack++ = 0;           /* R3 */
  52         *stack++ = 0;           /* R4 */
  53         *stack++ = 0;           /* R5 */
  54         *stack++ = 0;           /* R6 */
  55         *stack++ = 0;           /* R7 */
  56         *stack++ = 0;           /* R0 */
  57         *stack++ = 0;           /* R1 */
  58         *stack++ = 0;           /* PSW */
  59         *stack++ = 0;           /* BP */
  60         task->stack_count = stack - task->stack;
  61         task->wchan = NULL;
  62 }
  63
  64 /* Task switching function. This must not use any stack variables */
  65 void
  66 ao_yield(void) _naked
  67 {
  68         static uint8_t __data stack_len;
  69         static __data uint8_t * __data stack_ptr;
  70         static __xdata uint8_t * __data save_ptr;
  71
  72         /* Save current context */
  73         _asm
  74                 /* Push ACC first, as when restoring the context it must be restored
  75                  * last (it is used to set the IE register). */
  76                 push    ACC
  77                 /* Store the IE register then enable interrupts. */
  78                 push    _IEN0
  79                 setb    _EA
  80                 push    DPL
  81                 push    DPH
  82                 push    b
  83                 push    ar2
  84                 push    ar3
  85                 push    ar4
  86                 push    ar5
  87                 push    ar6
  88                 push    ar7
  89                 push    ar0
  90                 push    ar1
  91                 push    PSW
  92         _endasm;
  93         PSW = 0;
  94         _asm
  95                 push    _bp
  96         _endasm;
  97
  98         if (ao_cur_task_id != AO_NO_TASK)
  99         {
 100                 /* Save the current stack */
 101                 stack_len = SP - (AO_STACK_START - 1);
 102                 ao_cur_task->stack_count = stack_len;
 103                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 104                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 105                 while (stack_len--)
 106                         *save_ptr++ = *stack_ptr++;
 107         }
 108
 109         /* Empty the stack; might as well let interrupts have the whole thing */
 110         SP = AO_STACK_START - 1;
 111
 112         /* Find a task to run. If there isn't any runnable task,
 113          * this loop will run forever, which is just fine
 114          */
 115         for (;;) {
 116                 ++ao_cur_task_id;
 117                 if (ao_cur_task_id == ao_num_tasks)
 118                         ao_cur_task_id = 0;
 119                 ao_cur_task = ao_tasks[ao_cur_task_id];
 120                 if (ao_cur_task->wchan == NULL)
 121                         break;
 122         }
 123
 124         /* Restore the old stack */
 125         stack_len = ao_cur_task->stack_count;
 126         SP = AO_STACK_START - 1 + stack_len;
 127
 128         stack_ptr = (uint8_t __data *) AO_STACK_START;
 129         save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 130         while (stack_len--)
 131                 *stack_ptr++ = *save_ptr++;
 132
 133         _asm
 134                 pop             _bp
 135                 pop             PSW
 136                 pop             ar1
 137                 pop             ar0
 138                 pop             ar7
 139                 pop             ar6
 140                 pop             ar5
 141                 pop             ar4
 142                 pop             ar3
 143                 pop             ar2
 144                 pop             b
 145                 pop             DPH
 146                 pop             DPL
 147                 /* The next byte of the stack is the IE register.  Only the global
 148                 enable bit forms part of the task context.  Pop off the IE then set
 149                 the global enable bit to match that of the stored IE register. */
 150                 pop             ACC
 151                 JB              ACC.7,0098$
 152                 CLR             _EA
 153                 LJMP    0099$
 154         0098$:
 155                 SETB            _EA
 156         0099$:
 157                 /* Finally pop off the ACC, which was the first register saved. */
 158                 pop             ACC
 159                 ret
 160         _endasm;
 161 }
 162
 163 int
 164 ao_sleep(__xdata void *wchan)
 165 {
 166         ao_cur_task->wchan = wchan;
 167         ao_yield();
 168 }
 169
 170 int
 171 ao_wakeup(__xdata void *wchan)
 172 {
 173         uint8_t i;
 174
 175         for (i = 0; i < ao_num_tasks; i++)
 176                 if (ao_tasks[i]->wchan == wchan)
 177                         ao_tasks[i]->wchan = NULL;
 178 }
 179
 180 void
 181 ao_start_scheduler(void)
 182 {
 183         ao_cur_task_id = AO_NO_TASK;
 184         ao_cur_task = NULL;
 185         ao_yield();
 186 }