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