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