firmware/target/coldfire/thread-coldfire.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2004 by Linus Nielsen Feltzing
  11  *
  12  * Coldfire processor threading support
  13  *
  14  * This program is free software; you can redistribute it and/or
  15  * modify it under the terms of the GNU General Public License
  16  * as published by the Free Software Foundation; either version 2
  17  * of the License, or (at your option) any later version.
  18  *
  19  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  20  * KIND, either express or implied.
  21  *
  22  ****************************************************************************/
  23
  24 /*---------------------------------------------------------------------------
  25  * Start the thread running and terminate it if it returns
  26  *---------------------------------------------------------------------------
  27  */
  28 void start_thread(void); /* Provide C access to ASM label */
  29 static void __attribute__((used)) __start_thread(void)
  30 {
  31     /* a0=macsr, a1=context */
  32     asm volatile (
  33     "start_thread:             \n" /* Start here - no naked attribute */
  34         "move.l  %a0, %macsr   \n" /* Set initial mac status reg */
  35         "lea.l   48(%a1), %a1  \n"
  36         "move.l  (%a1)+, %sp   \n" /* Set initial stack */
  37         "move.l  (%a1), %a2    \n" /* Fetch thread function pointer */
  38         "clr.l   (%a1)         \n" /* Mark thread running */
  39         "jsr     (%a2)         \n" /* Call thread function */
  40     );
  41     thread_exit();
  42 }
  43
  44 /* Set EMAC unit to fractional mode with saturation for each new thread,
  45  * since that's what'll be the most useful for most things which the dsp
  46  * will do. Codecs should still initialize their preferred modes
  47  * explicitly. Context pointer is placed in d2 slot and start_thread
  48  * pointer in d3 slot. thread function pointer is placed in context.start.
  49  * See load_context for what happens when thread is initially going to
  50  * run.
  51  */
  52 #define THREAD_STARTUP_INIT(core, thread, function) \
  53     ({ (thread)->context.macsr = EMAC_FRACTIONAL | EMAC_SATURATE, \
  54        (thread)->context.d[0] = (uint32_t)&(thread)->context, \
  55        (thread)->context.d[1] = (uint32_t)start_thread,       \
  56        (thread)->context.start = (uint32_t)(function); })
  57
  58 /*---------------------------------------------------------------------------
  59  * Store non-volatile context.
  60  *---------------------------------------------------------------------------
  61  */
  62 static inline void store_context(void* addr)
  63 {
  64     asm volatile (
  65         "move.l  %%macsr,%%d0                  \n"
  66         "movem.l %%d0/%%d2-%%d7/%%a2-%%a7,(%0) \n"
  67         : : "a" (addr) : "d0" /* only! */
  68     );
  69 }
  70
  71 /*---------------------------------------------------------------------------
  72  * Load non-volatile context.
  73  *---------------------------------------------------------------------------
  74  */
  75 static inline void load_context(const void* addr)
  76 {
  77     asm volatile (
  78         "move.l  52(%0), %%d0                   \n"  /* Get start address */
  79         "beq.b   1f                             \n"  /* NULL -> already running */
  80         "movem.l (%0), %%a0-%%a2                \n"  /* a0=macsr, a1=context, a2=start_thread */
  81         "jmp     (%%a2)                         \n"  /* Start the thread */
  82     "1:                                         \n"
  83         "movem.l (%0), %%d0/%%d2-%%d7/%%a2-%%a7 \n"  /* Load context */
  84         "move.l  %%d0, %%macsr                  \n"
  85         : : "a" (addr) : "d0" /* only! */
  86     );
  87 }
  88
  89 /*---------------------------------------------------------------------------
  90  * Put core in a power-saving state if waking list wasn't repopulated.
  91  *---------------------------------------------------------------------------
  92  */
  93 static inline void core_sleep(void)
  94 {
  95     /* Supervisor mode, interrupts enabled upon wakeup */
  96     asm volatile ("stop #0x2000");
  97 };
  98
  99 /*---------------------------------------------------------------------------
 100  * Call this from asm to make sure the sp is pointing to the
 101  * correct place before the context is saved.
 102  *---------------------------------------------------------------------------
 103  */
 104 static inline void _profile_thread_stopped(int current_thread)
 105 {
 106     asm volatile ("move.l %[id], -(%%sp)\n\t"
 107                   "jsr profile_thread_stopped\n\t"
 108                   "addq.l #4, %%sp\n\t"
 109                   :: [id] "r" (current_thread)
 110                   : "cc", "memory");
 111 }