git.gag.com Git - fw/altos/blob - src/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) __reentrant
  29 {
  30         uint8_t __xdata *stack;
  31         uint8_t task_id;
  32         uint8_t t;
  33         if (ao_num_tasks == AO_NUM_TASKS)
  34                 ao_panic(AO_PANIC_NO_TASK);
  35         for (task_id = 1; task_id != 0; task_id++) {
  36                 for (t = 0; t < ao_num_tasks; t++)
  37                         if (ao_tasks[t]->task_id == task_id)
  38                                 break;
  39                 if (t == ao_num_tasks)
  40                         break;
  41         }
  42         ao_tasks[ao_num_tasks++] = task;
  43         task->task_id = task_id;
  44         task->name = name;
  45         /*
  46          * Construct a stack frame so that it will 'return'
  47          * to the start of the task
  48          */
  49         stack = task->stack;
  50
  51         *stack++ = ((uint16_t) start);          /* 0 */
  52         *stack++ = ((uint16_t) start) >> 8;     /* 1 */
  53
  54         /* and the stuff saved by ao_switch */
  55         *stack++ = 0;                           /* 2 acc */
  56         *stack++ = 0x80;                        /* 3 IE */
  57
  58         /*  4 DPL
  59          *  5 DPH
  60          *  6 B
  61          *  7 R2
  62          *  8 R3
  63          *  9 R4
  64          * 10 R5
  65          * 11 R6
  66          * 12 R7
  67          * 13 R0
  68          * 14 R1
  69          * 15 PSW
  70          * 16 BP
  71          */
  72         for (t = 0; t < 13; t++)
  73                 *stack++ = 0;
  74
  75         task->stack_count = 17;
  76         task->wchan = NULL;
  77 }
  78
  79 /* Task switching function. This must not use any stack variables */
  80 void
  81 ao_yield(void) __naked
  82 {
  83
  84         /* Save current context */
  85         _asm
  86                 /* Push ACC first, as when restoring the context it must be restored
  87                  * last (it is used to set the IE register). */
  88                 push    ACC
  89                 /* Store the IE register then enable interrupts. */
  90                 push    _IEN0
  91                 setb    _EA
  92                 push    DPL
  93                 push    DPH
  94                 push    b
  95                 push    ar2
  96                 push    ar3
  97                 push    ar4
  98                 push    ar5
  99                 push    ar6
 100                 push    ar7
 101                 push    ar0
 102                 push    ar1
 103                 push    PSW
 104         _endasm;
 105         PSW = 0;
 106         _asm
 107                 push    _bp
 108         _endasm;
 109
 110         if (ao_cur_task_index == AO_NO_TASK_INDEX)
 111                 ao_cur_task_index = ao_num_tasks-1;
 112         else
 113         {
 114                 uint8_t stack_len;
 115                 __data uint8_t *stack_ptr;
 116                 __xdata uint8_t *save_ptr;
 117                 /* Save the current stack */
 118                 stack_len = SP - (AO_STACK_START - 1);
 119                 ao_cur_task->stack_count = stack_len;
 120                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 121                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 122                 do
 123                         *save_ptr++ = *stack_ptr++;
 124                 while (--stack_len);
 125         }
 126
 127         /* Empty the stack; might as well let interrupts have the whole thing */
 128         SP = AO_STACK_START - 1;
 129
 130         /* Find a task to run. If there isn't any runnable task,
 131          * this loop will run forever, which is just fine
 132          */
 133         {
 134                 __pdata uint8_t ao_next_task_index = ao_cur_task_index;
 135                 for (;;) {
 136                         ++ao_next_task_index;
 137                         if (ao_next_task_index == ao_num_tasks)
 138                                 ao_next_task_index = 0;
 139
 140                         ao_cur_task = ao_tasks[ao_next_task_index];
 141                         if (ao_cur_task->wchan == NULL) {
 142                                 ao_cur_task_index = ao_next_task_index;
 143                                 break;
 144                         }
 145
 146                         /* Check if the alarm is set for a time which has passed */
 147                         if (ao_cur_task->alarm &&
 148                             (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) {
 149                                 ao_cur_task_index = ao_next_task_index;
 150                                 break;
 151                         }
 152
 153                         /* Enter lower power mode when there isn't anything to do */
 154                         if (ao_next_task_index == ao_cur_task_index)
 155                                 PCON = PCON_IDLE;
 156                 }
 157         }
 158
 159         {
 160                 uint8_t stack_len;
 161                 __data uint8_t *stack_ptr;
 162                 __xdata uint8_t *save_ptr;
 163
 164                 /* Restore the old stack */
 165                 stack_len = ao_cur_task->stack_count;
 166                 SP = AO_STACK_START - 1 + stack_len;
 167
 168                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 169                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 170                 do
 171                         *stack_ptr++ = *save_ptr++;
 172                 while (--stack_len);
 173         }
 174
 175         _asm
 176                 pop             _bp
 177                 pop             PSW
 178                 pop             ar1
 179                 pop             ar0
 180                 pop             ar7
 181                 pop             ar6
 182                 pop             ar5
 183                 pop             ar4
 184                 pop             ar3
 185                 pop             ar2
 186                 pop             b
 187                 pop             DPH
 188                 pop             DPL
 189                 /* The next byte of the stack is the IE register.  Only the global
 190                 enable bit forms part of the task context.  Pop off the IE then set
 191                 the global enable bit to match that of the stored IE register. */
 192                 pop             ACC
 193                 JB              ACC.7,0098$
 194                 CLR             _EA
 195                 LJMP    0099$
 196         0098$:
 197                 SETB            _EA
 198         0099$:
 199                 /* Finally pop off the ACC, which was the first register saved. */
 200                 pop             ACC
 201                 ret
 202         _endasm;
 203 }
 204
 205 uint8_t
 206 ao_sleep(__xdata void *wchan)
 207 {
 208         __critical {
 209                 ao_cur_task->wchan = wchan;
 210         }
 211         ao_yield();
 212         ao_cur_task->alarm = 0;
 213         if (ao_cur_task->wchan) {
 214                 ao_cur_task->wchan = NULL;
 215                 return 1;
 216         }
 217         return 0;
 218 }
 219
 220 void
 221 ao_wakeup(__xdata void *wchan)
 222 {
 223         uint8_t i;
 224
 225         for (i = 0; i < ao_num_tasks; i++)
 226                 if (ao_tasks[i]->wchan == wchan)
 227                         ao_tasks[i]->wchan = NULL;
 228 }
 229
 230 void
 231 ao_alarm(uint16_t delay)
 232 {
 233         /* Make sure we sleep *at least* delay ticks, which means adding
 234          * one to account for the fact that we may be close to the next tick
 235          */
 236         if (!(ao_cur_task->alarm = ao_time() + delay + 1))
 237                 ao_cur_task->alarm = 1;
 238 }
 239
 240 void
 241 ao_exit(void) __critical
 242 {
 243         uint8_t i;
 244         ao_num_tasks--;
 245         for (i = ao_cur_task_index; i < ao_num_tasks; i++)
 246                 ao_tasks[i] = ao_tasks[i+1];
 247         ao_cur_task_index = AO_NO_TASK_INDEX;
 248         ao_yield();
 249         /* we'll never get back here */
 250 }
 251
 252 void
 253 ao_task_info(void)
 254 {
 255         uint8_t i;
 256         uint8_t pc_loc;
 257         __xdata struct ao_task *task;
 258
 259         for (i = 0; i < ao_num_tasks; i++) {
 260                 task = ao_tasks[i];
 261                 pc_loc = task->stack_count - 17;
 262                 printf("%12s: wchan %04x pc %04x\n",
 263                        task->name,
 264                        (int16_t) task->wchan,
 265                        (task->stack[pc_loc]) | (task->stack[pc_loc+1] << 8));
 266         }
 267 }
 268
 269 void
 270 ao_start_scheduler(void)
 271 {
 272         ao_cur_task_index = AO_NO_TASK_INDEX;
 273         ao_cur_task = NULL;
 274         ao_yield();
 275 }