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) __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         {
  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         {
 118                 __pdata uint8_t ao_next_task_index = ao_cur_task_index;
 119                 for (;;) {
 120                         ++ao_next_task_index;
 121                         if (ao_next_task_index == ao_num_tasks)
 122                                 ao_next_task_index = 0;
 123
 124                         ao_cur_task = ao_tasks[ao_next_task_index];
 125                         if (ao_cur_task->wchan == NULL) {
 126                                 ao_cur_task_index = ao_next_task_index;
 127                                 break;
 128                         }
 129
 130                         /* Enter lower power mode when there isn't anything to do */
 131                         if (ao_next_task_index == ao_cur_task_index)
 132                                 PCON = PCON_IDLE;
 133                 }
 134         }
 135
 136         {
 137                 uint8_t stack_len;
 138                 __data uint8_t *stack_ptr;
 139                 __xdata uint8_t *save_ptr;
 140
 141                 /* Restore the old stack */
 142                 stack_len = ao_cur_task->stack_count;
 143                 SP = AO_STACK_START - 1 + stack_len;
 144
 145                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 146                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 147                 do
 148                         *stack_ptr++ = *save_ptr++;
 149                 while (--stack_len);
 150         }
 151
 152         _asm
 153                 pop             _bp
 154                 pop             PSW
 155                 pop             ar1
 156                 pop             ar0
 157                 pop             ar7
 158                 pop             ar6
 159                 pop             ar5
 160                 pop             ar4
 161                 pop             ar3
 162                 pop             ar2
 163                 pop             b
 164                 pop             DPH
 165                 pop             DPL
 166                 /* The next byte of the stack is the IE register.  Only the global
 167                 enable bit forms part of the task context.  Pop off the IE then set
 168                 the global enable bit to match that of the stored IE register. */
 169                 pop             ACC
 170                 JB              ACC.7,0098$
 171                 CLR             _EA
 172                 LJMP    0099$
 173         0098$:
 174                 SETB            _EA
 175         0099$:
 176                 /* Finally pop off the ACC, which was the first register saved. */
 177                 pop             ACC
 178                 ret
 179         _endasm;
 180 }
 181
 182 void
 183 ao_sleep(__xdata void *wchan)
 184 {
 185         __critical {
 186                 ao_cur_task->wchan = wchan;
 187         }
 188         ao_yield();
 189 }
 190
 191 void
 192 ao_wakeup(__xdata void *wchan)
 193 {
 194         uint8_t i;
 195
 196         for (i = 0; i < ao_num_tasks; i++)
 197                 if (ao_tasks[i]->wchan == wchan)
 198                         ao_tasks[i]->wchan = NULL;
 199 }
 200
 201 void
 202 ao_task_info(void)
 203 {
 204         uint8_t i;
 205         uint8_t pc_loc;
 206         __xdata struct ao_task *task;
 207
 208         for (i = 0; i < ao_num_tasks; i++) {
 209                 task = ao_tasks[i];
 210                 pc_loc = task->stack_count - 17;
 211                 printf("%12s: wchan %04x pc %04x\n",
 212                        task->name,
 213                        (int16_t) task->wchan,
 214                        (task->stack[pc_loc]) | (task->stack[pc_loc+1] << 8));
 215         }
 216 }
 217
 218 void
 219 ao_start_scheduler(void)
 220 {
 221         ao_cur_task_index = AO_NO_TASK_INDEX;
 222         ao_cur_task = NULL;
 223         ao_yield();
 224 }