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_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 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         task->task_id = ao_num_tasks;
  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
  69         /* Save current context */
  70         _asm
  71                 /* Push ACC first, as when restoring the context it must be restored
  72                  * last (it is used to set the IE register). */
  73                 push    ACC
  74                 /* Store the IE register then enable interrupts. */
  75                 push    _IEN0
  76                 setb    _EA
  77                 push    DPL
  78                 push    DPH
  79                 push    b
  80                 push    ar2
  81                 push    ar3
  82                 push    ar4
  83                 push    ar5
  84                 push    ar6
  85                 push    ar7
  86                 push    ar0
  87                 push    ar1
  88                 push    PSW
  89         _endasm;
  90         PSW = 0;
  91         _asm
  92                 push    _bp
  93         _endasm;
  94
  95         if (ao_cur_task_index != AO_NO_TASK_INDEX)
  96         {
  97         uint8_t stack_len;
  98         __data uint8_t *  stack_ptr;
  99         __xdata uint8_t * save_ptr;
 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                 do
 106                         *save_ptr++ = *stack_ptr++;
 107                 while (--stack_len);
 108         }
 109
 110         /* Empty the stack; might as well let interrupts have the whole thing */
 111         SP = AO_STACK_START - 1;
 112
 113         /* Find a task to run. If there isn't any runnable task,
 114          * this loop will run forever, which is just fine
 115          */
 116         for (;;) {
 117                 ++ao_cur_task_index;
 118                 if (ao_cur_task_index == ao_num_tasks)
 119                         ao_cur_task_index = 0;
 120                 ao_cur_task = ao_tasks[ao_cur_task_index];
 121                 if (ao_cur_task->wchan == NULL)
 122                         break;
 123         }
 124
 125         {
 126                 uint8_t stack_len;
 127                 __data uint8_t *  stack_ptr;
 128                 __xdata uint8_t * save_ptr;
 129
 130                 /* Restore the old stack */
 131                 stack_len = ao_cur_task->stack_count;
 132                 SP = AO_STACK_START - 1 + stack_len;
 133
 134                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 135                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 136                 do
 137                         *stack_ptr++ = *save_ptr++;
 138                 while (--stack_len);
 139         }
 140
 141         _asm
 142                 pop             _bp
 143                 pop             PSW
 144                 pop             ar1
 145                 pop             ar0
 146                 pop             ar7
 147                 pop             ar6
 148                 pop             ar5
 149                 pop             ar4
 150                 pop             ar3
 151                 pop             ar2
 152                 pop             b
 153                 pop             DPH
 154                 pop             DPL
 155                 /* The next byte of the stack is the IE register.  Only the global
 156                 enable bit forms part of the task context.  Pop off the IE then set
 157                 the global enable bit to match that of the stored IE register. */
 158                 pop             ACC
 159                 JB              ACC.7,0098$
 160                 CLR             _EA
 161                 LJMP    0099$
 162         0098$:
 163                 SETB            _EA
 164         0099$:
 165                 /* Finally pop off the ACC, which was the first register saved. */
 166                 pop             ACC
 167                 ret
 168         _endasm;
 169 }
 170
 171 int
 172 ao_sleep(__xdata void *wchan)
 173 {
 174         __critical {
 175         ao_cur_task->wchan = wchan;
 176         }
 177         ao_yield();
 178 }
 179
 180 int
 181 ao_wakeup(__xdata void *wchan)
 182 {
 183         uint8_t i;
 184
 185         for (i = 0; i < ao_num_tasks; i++)
 186                 if (ao_tasks[i]->wchan == wchan)
 187                         ao_tasks[i]->wchan = NULL;
 188 }
 189
 190 void
 191 ao_start_scheduler(void)
 192 {
 193
 194         ao_cur_task_index = AO_NO_TASK_INDEX;
 195         ao_cur_task = NULL;
 196         ao_yield();
 197 }