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