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; 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      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_id;
  25 __xdata struct ao_task *__data ao_cur_task;
  26
  27 void
  28 ao_add_task(__xdata struct ao_task * task, void (*start)(void))
  29 {
  30         uint8_t __xdata *stack;
  31         if (ao_num_tasks == AO_NUM_TASKS)
  32                 ao_panic(AO_PANIC_NO_TASK);
  33         ao_tasks[ao_num_tasks++] = task;
  34         /*
  35          * Construct a stack frame so that it will 'return'
  36          * to the start of the task
  37          */
  38         stack = task->stack;
  39
  40         *stack++ = ((uint16_t) start);
  41         *stack++ = ((uint16_t) start) >> 8;
  42
  43         /* and the stuff saved by ao_switch */
  44         *stack++ = 0;           /* acc */
  45         *stack++ = 0x80;        /* IE */
  46         *stack++ = 0;           /* DPL */
  47         *stack++ = 0;           /* DPH */
  48         *stack++ = 0;           /* B */
  49         *stack++ = 0;           /* R2 */
  50         *stack++ = 0;           /* R3 */
  51         *stack++ = 0;           /* R4 */
  52         *stack++ = 0;           /* R5 */
  53         *stack++ = 0;           /* R6 */
  54         *stack++ = 0;           /* R7 */
  55         *stack++ = 0;           /* R0 */
  56         *stack++ = 0;           /* R1 */
  57         *stack++ = 0;           /* PSW */
  58         *stack++ = 0;           /* BP */
  59         task->stack_count = stack - task->stack;
  60         task->wchan = NULL;
  61 }
  62
  63 /* Task switching function. This must not use any stack variables */
  64 void
  65 ao_yield(void) _naked
  66 {
  67         static uint8_t __data stack_len;
  68         static __data uint8_t * __data stack_ptr;
  69         static __xdata uint8_t * __data save_ptr;
  70
  71         /* Save current context */
  72         _asm
  73                 /* Push ACC first, as when restoring the context it must be restored
  74                  * last (it is used to set the IE register). */
  75                 push    ACC
  76                 /* Store the IE register then enable interrupts. */
  77                 push    _IEN0
  78                 setb    _EA
  79                 push    DPL
  80                 push    DPH
  81                 push    b
  82                 push    ar2
  83                 push    ar3
  84                 push    ar4
  85                 push    ar5
  86                 push    ar6
  87                 push    ar7
  88                 push    ar0
  89                 push    ar1
  90                 push    PSW
  91         _endasm;
  92         PSW = 0;
  93         _asm
  94                 push    _bp
  95         _endasm;
  96
  97         if (ao_cur_task_id != AO_NO_TASK)
  98         {
  99                 /* Save the current stack */
 100                 stack_len = SP - (AO_STACK_START - 1);
 101                 ao_cur_task->stack_count = stack_len;
 102                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 103                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 104                 while (stack_len--)
 105                         *save_ptr++ = *stack_ptr++;
 106         }
 107
 108         /* Empty the stack; might as well let interrupts have the whole thing */
 109         SP = AO_STACK_START - 1;
 110
 111         /* Find a task to run. If there isn't any runnable task,
 112          * this loop will run forever, which is just fine
 113          */
 114         for (;;) {
 115                 ++ao_cur_task_id;
 116                 if (ao_cur_task_id == ao_num_tasks)
 117                         ao_cur_task_id = 0;
 118                 ao_cur_task = ao_tasks[ao_cur_task_id];
 119                 if (ao_cur_task->wchan == NULL)
 120                         break;
 121         }
 122
 123         /* Restore the old stack */
 124         stack_len = ao_cur_task->stack_count;
 125         SP = AO_STACK_START - 1 + stack_len;
 126
 127         stack_ptr = (uint8_t __data *) AO_STACK_START;
 128         save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 129         while (stack_len--)
 130                 *stack_ptr++ = *save_ptr++;
 131
 132         _asm
 133                 pop             _bp
 134                 pop             PSW
 135                 pop             ar1
 136                 pop             ar0
 137                 pop             ar7
 138                 pop             ar6
 139                 pop             ar5
 140                 pop             ar4
 141                 pop             ar3
 142                 pop             ar2
 143                 pop             b
 144                 pop             DPH
 145                 pop             DPL
 146                 /* The next byte of the stack is the IE register.  Only the global
 147                 enable bit forms part of the task context.  Pop off the IE then set
 148                 the global enable bit to match that of the stored IE register. */
 149                 pop             ACC
 150                 JB              ACC.7,0098$
 151                 CLR             _EA
 152                 LJMP    0099$
 153         0098$:
 154                 SETB            _EA
 155         0099$:
 156                 /* Finally pop off the ACC, which was the first register saved. */
 157                 pop             ACC
 158                 ret
 159         _endasm;
 160 }
 161
 162 int
 163 ao_sleep(__xdata void *wchan)
 164 {
 165         __critical {
 166         ao_cur_task->wchan = wchan;
 167         }
 168         ao_yield();
 169 }
 170
 171 int
 172 ao_wakeup(__xdata void *wchan)
 173 {
 174         uint8_t i;
 175
 176         for (i = 0; i < ao_num_tasks; i++)
 177                 if (ao_tasks[i]->wchan == wchan)
 178                         ao_tasks[i]->wchan = NULL;
 179 }
 180
 181 void
 182 ao_start_scheduler(void)
 183 {
 184
 185         ao_cur_task_id = AO_NO_TASK;
 186         ao_cur_task = NULL;
 187         ao_yield();
 188 }