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         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                 ao_cur_task_index = ao_num_tasks-1;
  98         else
  99         {
 100                 uint8_t stack_len;
 101                 __data uint8_t *stack_ptr;
 102                 __xdata uint8_t *save_ptr;
 103                 /* Save the current stack */
 104                 stack_len = SP - (AO_STACK_START - 1);
 105                 ao_cur_task->stack_count = stack_len;
 106                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 107                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 108                 do
 109                         *save_ptr++ = *stack_ptr++;
 110                 while (--stack_len);
 111         }
 112
 113         /* Empty the stack; might as well let interrupts have the whole thing */
 114         SP = AO_STACK_START - 1;
 115
 116         /* Find a task to run. If there isn't any runnable task,
 117          * this loop will run forever, which is just fine
 118          */
 119         {
 120                 __pdata uint8_t ao_next_task_index = ao_cur_task_index;
 121                 for (;;) {
 122                         ++ao_next_task_index;
 123                         if (ao_next_task_index == ao_num_tasks)
 124                                 ao_next_task_index = 0;
 125
 126                         ao_cur_task = ao_tasks[ao_next_task_index];
 127                         if (ao_cur_task->wchan == NULL) {
 128                                 ao_cur_task_index = ao_next_task_index;
 129                                 break;
 130                         }
 131
 132                         /* Check if the alarm is set for a time which has passed */
 133                         if (ao_cur_task->alarm &&
 134                             (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) {
 135                                 ao_cur_task_index = ao_next_task_index;
 136                                 break;
 137                         }
 138
 139                         /* Enter lower power mode when there isn't anything to do */
 140                         if (ao_next_task_index == ao_cur_task_index)
 141                                 PCON = PCON_IDLE;
 142                 }
 143         }
 144
 145         {
 146                 uint8_t stack_len;
 147                 __data uint8_t *stack_ptr;
 148                 __xdata uint8_t *save_ptr;
 149
 150                 /* Restore the old stack */
 151                 stack_len = ao_cur_task->stack_count;
 152                 SP = AO_STACK_START - 1 + stack_len;
 153
 154                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 155                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 156                 do
 157                         *stack_ptr++ = *save_ptr++;
 158                 while (--stack_len);
 159         }
 160
 161         _asm
 162                 pop             _bp
 163                 pop             PSW
 164                 pop             ar1
 165                 pop             ar0
 166                 pop             ar7
 167                 pop             ar6
 168                 pop             ar5
 169                 pop             ar4
 170                 pop             ar3
 171                 pop             ar2
 172                 pop             b
 173                 pop             DPH
 174                 pop             DPL
 175                 /* The next byte of the stack is the IE register.  Only the global
 176                 enable bit forms part of the task context.  Pop off the IE then set
 177                 the global enable bit to match that of the stored IE register. */
 178                 pop             ACC
 179                 JB              ACC.7,0098$
 180                 CLR             _EA
 181                 LJMP    0099$
 182         0098$:
 183                 SETB            _EA
 184         0099$:
 185                 /* Finally pop off the ACC, which was the first register saved. */
 186                 pop             ACC
 187                 ret
 188         _endasm;
 189 }
 190
 191 uint8_t
 192 ao_sleep(__xdata void *wchan)
 193 {
 194         __critical {
 195                 ao_cur_task->wchan = wchan;
 196         }
 197         ao_yield();
 198         if (ao_cur_task->wchan) {
 199                 ao_cur_task->wchan = NULL;
 200                 ao_cur_task->alarm = 0;
 201                 return 1;
 202         }
 203         ao_cur_task->alarm = 0;
 204         return 0;
 205 }
 206
 207 void
 208 ao_wakeup(__xdata void *wchan)
 209 {
 210         uint8_t i;
 211
 212         for (i = 0; i < ao_num_tasks; i++)
 213                 if (ao_tasks[i]->wchan == wchan)
 214                         ao_tasks[i]->wchan = NULL;
 215 }
 216
 217 void
 218 ao_alarm(uint16_t delay)
 219 {
 220         if (!(ao_cur_task->alarm = ao_time() + delay))
 221                 ao_cur_task->alarm = 1;
 222 }
 223
 224 void
 225 ao_wake_task(__xdata struct ao_task *task)
 226 {
 227         task->wchan = NULL;
 228 }
 229
 230 void
 231 ao_exit(void) __critical
 232 {
 233         uint8_t i;
 234         ao_num_tasks--;
 235         for (i = ao_cur_task_index; i < ao_num_tasks; i++)
 236                 ao_tasks[i] = ao_tasks[i+1];
 237         ao_cur_task_index = AO_NO_TASK_INDEX;
 238         ao_yield();
 239         /* we'll never get back here */
 240 }
 241
 242 void
 243 ao_task_info(void)
 244 {
 245         uint8_t i;
 246         uint8_t pc_loc;
 247         __xdata struct ao_task *task;
 248
 249         for (i = 0; i < ao_num_tasks; i++) {
 250                 task = ao_tasks[i];
 251                 pc_loc = task->stack_count - 17;
 252                 printf("%12s: wchan %04x pc %04x\n",
 253                        task->name,
 254                        (int16_t) task->wchan,
 255                        (task->stack[pc_loc]) | (task->stack[pc_loc+1] << 8));
 256         }
 257 }
 258
 259 void
 260 ao_start_scheduler(void)
 261 {
 262         ao_cur_task_index = AO_NO_TASK_INDEX;
 263         ao_cur_task = NULL;
 264         ao_yield();
 265 }