git.gag.com Git - fw/altos/blob - src-avr/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 #ifdef AVR
  28
  29 #define PUSH8(stack, val)       (*((stack)--) = (val))
  30
  31 static void
  32 ao_init_stack(__xdata struct ao_task *task, void (*start)(void))
  33 {
  34         uint8_t         *sp = task->stack + AO_STACK_SIZE - 1;
  35         uint16_t        a = (uint16_t) start;
  36         int             i;
  37
  38         /* Return address */
  39         PUSH8(sp, a);
  40         PUSH8(sp, (a >> 8));
  41
  42         /* Clear register values */
  43         i = 32;
  44         while (i--)
  45                 PUSH8(sp, 0);
  46
  47         /* SREG with interrupts enabled */
  48         PUSH8(sp, 0x80);
  49         task->sp = sp;
  50 }
  51 #else
  52 static void
  53 ao_init_stack(__xdata struct ao_task *task, void (*start)(void))
  54 {
  55         uint8_t __xdata *stack = task->stack;
  56         /*
  57          * Construct a stack frame so that it will 'return'
  58          * to the start of the task
  59          */
  60
  61         *stack++ = ((uint16_t) start);
  62         *stack++ = ((uint16_t) start) >> 8;
  63
  64         /* and the stuff saved by ao_switch */
  65         *stack++ = 0;           /* acc */
  66         *stack++ = 0x80;        /* IE */
  67         *stack++ = 0;           /* DPL */
  68         *stack++ = 0;           /* DPH */
  69         *stack++ = 0;           /* B */
  70         *stack++ = 0;           /* R2 */
  71         *stack++ = 0;           /* R3 */
  72         *stack++ = 0;           /* R4 */
  73         *stack++ = 0;           /* R5 */
  74         *stack++ = 0;           /* R6 */
  75         *stack++ = 0;           /* R7 */
  76         *stack++ = 0;           /* R0 */
  77         *stack++ = 0;           /* R1 */
  78         *stack++ = 0;           /* PSW */
  79         *stack++ = 0;           /* BP */
  80         task->stack_count = stack - task->stack;
  81 }
  82 #endif
  83
  84 void
  85 ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant
  86 {
  87         uint8_t task_id;
  88         uint8_t t;
  89         if (ao_num_tasks == AO_NUM_TASKS)
  90                 ao_panic(AO_PANIC_NO_TASK);
  91         for (task_id = 1; task_id != 0; task_id++) {
  92                 for (t = 0; t < ao_num_tasks; t++)
  93                         if (ao_tasks[t]->task_id == task_id)
  94                                 break;
  95                 if (t == ao_num_tasks)
  96                         break;
  97         }
  98         ao_tasks[ao_num_tasks++] = task;
  99         task->task_id = task_id;
 100         task->name = name;
 101         task->wchan = NULL;
 102         ao_init_stack(task, start);
 103 }
 104
 105 void
 106 ao_show_task_from(void)
 107 {
 108         if (ao_cur_task)
 109                 printf("switching from %s\n", ao_cur_task->name);
 110 }
 111
 112 void
 113 ao_show_task_to(void)
 114 {
 115         printf("switching to %s\n", ao_cur_task->name);
 116 }
 117
 118 /* Task switching function. This must not use any stack variables */
 119 void
 120 ao_yield(void) __naked
 121 {
 122 #ifdef AVR
 123         asm("push r31" "\n\t" "push r30");
 124         asm("push r29" "\n\t" "push r28" "\n\t" "push r27" "\n\t" "push r26" "\n\t" "push r25");
 125         asm("push r24" "\n\t" "push r23" "\n\t" "push r22" "\n\t" "push r21" "\n\t" "push r20");
 126         asm("push r19" "\n\t" "push r18" "\n\t" "push r17" "\n\t" "push r16" "\n\t" "push r15");
 127         asm("push r14" "\n\t" "push r13" "\n\t" "push r12" "\n\t" "push r11" "\n\t" "push r10");
 128         asm("push r9" "\n\t" "push r8" "\n\t" "push r7" "\n\t" "push r6" "\n\t" "push r5");
 129         asm("push r4" "\n\t" "push r3" "\n\t" "push r2" "\n\t" "push r1" "\n\t" "push r0");
 130         cli();
 131         asm("in r0, __SREG__" "\n\t" "push r0");
 132         sei();
 133 #else
 134         /* Save current context */
 135         _asm
 136                 /* Push ACC first, as when restoring the context it must be restored
 137                  * last (it is used to set the IE register). */
 138                 push    ACC
 139                 /* Store the IE register then enable interrupts. */
 140                 push    _IEN0
 141                 setb    _EA
 142                 push    DPL
 143                 push    DPH
 144                 push    b
 145                 push    ar2
 146                 push    ar3
 147                 push    ar4
 148                 push    ar5
 149                 push    ar6
 150                 push    ar7
 151                 push    ar0
 152                 push    ar1
 153                 push    PSW
 154         _endasm;
 155         PSW = 0;
 156         _asm
 157                 push    _bp
 158         _endasm;
 159 #endif
 160
 161         if (ao_cur_task_index == AO_NO_TASK_INDEX)
 162                 ao_cur_task_index = ao_num_tasks-1;
 163         else
 164         {
 165 #ifdef AVR
 166                 uint8_t sp_l, sp_h;
 167                 asm("in %0,__SP_L__" : "=&r" (sp_l) );
 168                 asm("in %0,__SP_H__" : "=&r" (sp_h) );
 169                 ao_cur_task->sp = (uint8_t *) ((uint16_t) sp_l | ((uint16_t) sp_h << 8));
 170 #else
 171                 uint8_t stack_len;
 172                 __data uint8_t *stack_ptr;
 173                 __xdata uint8_t *save_ptr;
 174                 /* Save the current stack */
 175                 stack_len = SP - (AO_STACK_START - 1);
 176                 ao_cur_task->stack_count = stack_len;
 177                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 178                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 179                 do
 180                         *save_ptr++ = *stack_ptr++;
 181                 while (--stack_len);
 182 #endif
 183         }
 184
 185 #ifndef AVR
 186         /* Empty the stack; might as well let interrupts have the whole thing */
 187         SP = AO_STACK_START - 1;
 188 #endif
 189
 190         ao_show_task_from();
 191         /* Find a task to run. If there isn't any runnable task,
 192          * this loop will run forever, which is just fine
 193          */
 194         {
 195                 __pdata uint8_t ao_next_task_index = ao_cur_task_index;
 196                 for (;;) {
 197                         ++ao_next_task_index;
 198                         if (ao_next_task_index == ao_num_tasks)
 199                                 ao_next_task_index = 0;
 200
 201                         ao_cur_task = ao_tasks[ao_next_task_index];
 202                         if (ao_cur_task->wchan == NULL) {
 203                                 ao_cur_task_index = ao_next_task_index;
 204                                 break;
 205                         }
 206
 207                         /* Check if the alarm is set for a time which has passed */
 208                         if (ao_cur_task->alarm &&
 209                             (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) {
 210                                 ao_cur_task_index = ao_next_task_index;
 211                                 break;
 212                         }
 213
 214 #ifdef AVR
 215 #else
 216                         /* Enter lower power mode when there isn't anything to do */
 217                         if (ao_next_task_index == ao_cur_task_index)
 218                                 PCON = PCON_IDLE;
 219 #endif
 220                 }
 221         }
 222
 223 #ifdef AVR
 224         {
 225                 uint8_t sp_l, sp_h;
 226                 sp_l = (uint16_t) ao_cur_task->sp;
 227                 sp_h = ((uint16_t) ao_cur_task->sp) >> 8;
 228                 ao_show_task_to();
 229                 cli();
 230                 asm("out __SP_H__,%0" : : "r" (sp_h) );
 231                 asm("out __SP_L__,%0" : : "r" (sp_l) );
 232                 asm("pop r0"    "\n\t"
 233                     "out __SREG__, r0");
 234                 asm("pop r0" "\n\t" "pop r1" "\n\t" "pop r2" "\n\t" "pop r3" "\n\t" "pop r4");
 235                 asm("pop r5" "\n\t" "pop r6" "\n\t" "pop r7" "\n\t" "pop r8" "\n\t" "pop r9");
 236                 asm("pop r10" "\n\t" "pop r11" "\n\t" "pop r12" "\n\t" "pop r13" "\n\t" "pop r14");
 237                 asm("pop r15" "\n\t" "pop r16" "\n\t" "pop r17" "\n\t" "pop r18" "\n\t" "pop r19");
 238                 asm("pop r20" "\n\t" "pop r21" "\n\t" "pop r22" "\n\t" "pop r23" "\n\t" "pop r24");
 239                 asm("pop r25" "\n\t" "pop r26" "\n\t" "pop r27" "\n\t" "pop r28" "\n\t" "pop r29");
 240                 asm("pop r30" "\n\t" "pop r31");
 241                 asm("ret");
 242         }
 243 #else
 244         {
 245                 uint8_t stack_len;
 246                 __data uint8_t *stack_ptr;
 247                 __xdata uint8_t *save_ptr;
 248
 249                 /* Restore the old stack */
 250                 stack_len = ao_cur_task->stack_count;
 251                 SP = AO_STACK_START - 1 + stack_len;
 252
 253                 stack_ptr = (uint8_t __data *) AO_STACK_START;
 254                 save_ptr = (uint8_t __xdata *) ao_cur_task->stack;
 255                 do
 256                         *stack_ptr++ = *save_ptr++;
 257                 while (--stack_len);
 258         }
 259
 260         _asm
 261                 pop             _bp
 262                 pop             PSW
 263                 pop             ar1
 264                 pop             ar0
 265                 pop             ar7
 266                 pop             ar6
 267                 pop             ar5
 268                 pop             ar4
 269                 pop             ar3
 270                 pop             ar2
 271                 pop             b
 272                 pop             DPH
 273                 pop             DPL
 274                 /* The next byte of the stack is the IE register.  Only the global
 275                 enable bit forms part of the task context.  Pop off the IE then set
 276                 the global enable bit to match that of the stored IE register. */
 277                 pop             ACC
 278                 JB              ACC.7,0098$
 279                 CLR             _EA
 280                 LJMP    0099$
 281         0098$:
 282                 SETB            _EA
 283         0099$:
 284                 /* Finally pop off the ACC, which was the first register saved. */
 285                 pop             ACC
 286                 ret
 287         _endasm;
 288 #endif
 289 }
 290
 291 uint8_t
 292 ao_sleep(__xdata void *wchan)
 293 {
 294         __critical {
 295                 ao_cur_task->wchan = wchan;
 296         }
 297         ao_yield();
 298         ao_cur_task->alarm = 0;
 299         if (ao_cur_task->wchan) {
 300                 ao_cur_task->wchan = NULL;
 301                 return 1;
 302         }
 303         return 0;
 304 }
 305
 306 void
 307 ao_wakeup(__xdata void *wchan)
 308 {
 309         uint8_t i;
 310
 311         for (i = 0; i < ao_num_tasks; i++)
 312                 if (ao_tasks[i]->wchan == wchan)
 313                         ao_tasks[i]->wchan = NULL;
 314 }
 315
 316 void
 317 ao_alarm(uint16_t delay)
 318 {
 319         /* Make sure we sleep *at least* delay ticks, which means adding
 320          * one to account for the fact that we may be close to the next tick
 321          */
 322         if (!(ao_cur_task->alarm = ao_time() + delay + 1))
 323                 ao_cur_task->alarm = 1;
 324 }
 325
 326 void
 327 ao_exit(void) __critical
 328 {
 329         uint8_t i;
 330         ao_num_tasks--;
 331         for (i = ao_cur_task_index; i < ao_num_tasks; i++)
 332                 ao_tasks[i] = ao_tasks[i+1];
 333         ao_cur_task_index = AO_NO_TASK_INDEX;
 334         ao_yield();
 335         /* we'll never get back here */
 336 }
 337
 338 void
 339 ao_task_info(void)
 340 {
 341         uint8_t i;
 342         __xdata struct ao_task *task;
 343
 344         for (i = 0; i < ao_num_tasks; i++) {
 345                 task = ao_tasks[i];
 346                 printf("%12s: wchan %04x\n",
 347                        task->name,
 348                        (int16_t) task->wchan);
 349         }
 350 }
 351
 352 void
 353 ao_start_scheduler(void)
 354 {
 355         ao_cur_task_index = AO_NO_TASK_INDEX;
 356         ao_cur_task = NULL;
 357         ao_yield();
 358 }