Orocos Real-Time Toolkit  2.5.0
fosi_internal.cpp
00001 /***************************************************************************
00002   tag:
00003 
00004                         fosi_internal.hpp -  description
00005                            -------------------
00006     begin                : Jan 21 2006
00007     copyright            : (C) 2006 Klaas Gadeyne
00008     email                : firstname lastname at fmtc be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 
00028 
00029 #include <pkgconf/kernel.h>
00030 #include <pkgconf/os_ecos.h>
00031 #include "ThreadInterface.hpp"
00032 #include "fosi.h"
00033 #include "../fosi_internal_interface.hpp"
00034 #include <cyg/kernel/kapi.h>
00035 #include <iostream>
00036 #include <string>
00037 
00038 #define INTERNAL_QUAL
00039 
00040 namespace RTT
00041 {
00042   namespace os {
00043     INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main_task)
00044     {
00045         const char* name = "main";
00046         main_task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
00047         return 0;
00048     }
00049 
00050     INTERNAL_QUAL int rtos_task_delete_main(RTOS_TASK* main_task)
00051     {
00052         free(main_task->name);
00053         return 0;
00054     }
00055 
00056 
00057     INTERNAL_QUAL int rtos_task_create(RTOS_TASK * task,
00058                        int priority,
00059                        unsigned cpu_affinity,
00060                        const char * name,
00061                        int sched_type,
00062                        size_t stack_size,
00063                        void * (*start_routine)(void *),
00064                        ThreadInterface* obj) {
00065       /* sched_type is unused in eCos */
00066       // Allocate room for threads name
00067       if ( strlen(name) == 0 )
00068           name = "Thread";
00069       task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
00070 
00071       // Allocate necessary stack...
00072       task->stack = (char *)malloc(stack_size?stack_size:OROSEM_OS_ECOS_STACK_SIZE);
00073 
00074       // Create the thread
00075       cyg_thread_create((cyg_addrword_t) priority, // priority
00076             (cyg_thread_entry_t *) start_routine, // Entry point
00077             (cyg_addrword_t) obj, // Entry point data
00078             task->name, // Name
00079             task->stack, // the stack
00080             OROSEM_OS_ECOS_STACK_SIZE, // stacksize
00081             &(task->handle),
00082             &(task->thread));
00083       // Start in fake Soft Real-Time mode
00084       task->hrt = false;
00085       // initialize the rest to zero
00086       task->periodMark = 0;
00087       task->period = 0;
00088       task->counter_hdl = 0;
00089       task->alarm_hdl = 0;
00090       // Resume (start) thread
00091       cyg_thread_resume(task->handle);
00092       return 0;
00093     }
00094 
00095     INTERNAL_QUAL void rtos_task_yield(RTOS_TASK*) {
00096       cyg_thread_yield();
00097     }
00098 
00099     INTERNAL_QUAL void wakeup_handler(cyg_handle_t alarm_handle,cyg_addrword_t data)
00100     {
00101       cyg_sem_t * wakeup_sem = (cyg_sem_t *) data;
00102       cyg_semaphore_post(wakeup_sem);
00103     }
00104 
00105     INTERNAL_QUAL void rtos_task_make_periodic(RTOS_TASK* mytask, NANO_TIME nanosecs )
00106     {
00107       if (nanosecs != 0)
00108     { // Init stuff (fixme, better check if only called once?)
00109       // diag_printf("fosi_internal.hpp rtos_task_make_periodic() ticks = %d...\n",nano2ticks(nanosecs));
00110       // Get handle to system clock
00111       mytask->sys_clk_hdl = cyg_real_time_clock();
00112       // Convert clock to counter handle
00113       cyg_clock_to_counter(mytask->sys_clk_hdl,&(mytask->counter_hdl));
00114       // Initialise semaphore
00115       cyg_semaphore_init(&(mytask->wakeup_sem),0);
00116       cyg_alarm_create(mytask->counter_hdl,
00117                wakeup_handler,
00118                (cyg_addrword_t) (&(mytask->wakeup_sem)),
00119                &(mytask->alarm_hdl),
00120                &(mytask->alarm_obj));
00121       // set next wake-up time.
00122       cyg_alarm_initialize(mytask->alarm_hdl,
00123                    rtos_get_time_ticks() + nano2ticks(nanosecs),
00124                    nano2ticks(nanosecs));
00125     } else /* period is zero */ {
00126         // Check if alarm already created...
00127         if (mytask->alarm_hdl != 0) {
00128             // diag_printf("fosi_internal.hpp rtos_task_set_period() WARNING: Disabling alarm...\n");
00129             cyg_alarm_disable(mytask->alarm_hdl);
00130         }
00131 
00132       // set period
00133       mytask->period = nanosecs;
00134     }
00135 
00136     INTERNAL_QUAL void rtos_task_set_period( RTOS_TASK* mytask, NANO_TIME nanosecs )
00137     {
00138       mytask->period = nanosecs;
00139       // FIXME:: Can one reinitialize an alarm??
00140       // diag_printf("fosi_internal.hpp WARNING: Can one reinitialize an alarm??\n");
00141       if (nanosecs != 0){
00142     // Check if alarm already created...
00143         if (mytask->alarm_hdl == 0){
00144       // diag_printf("fosi_internal.hpp rtos_task_set_period() WARNING: Alarm not yet initialized, doing this now...\n");
00145       rtos_task_make_periodic(mytask,nanosecs);
00146     }
00147     else {
00148       cyg_alarm_initialize(mytask->alarm_hdl,
00149                    rtos_get_time_ticks() + nano2ticks(nanosecs),
00150                    nano2ticks(nanosecs));
00151       // diag_printf("fosi_internal.hpp rtos_task_set_period() Setting period to %d ticks...\n",nano2ticks(nanosecs));
00152     // FIXME need call to cyg_alarm_enable here, or is this
00153     // "included" in initialize??
00154     }
00155       }
00156       else /* period is zero */ {
00157     // Check if alarm already created...
00158         if (mytask->alarm_hdl != 0) {
00159       // diag_printf("fosi_internal.hpp rtos_task_set_period() WARNING: Disabling alarm...\n");
00160       cyg_alarm_disable(mytask->alarm_hdl);
00161     }
00162       }
00163     }
00164 
00165     INTERNAL_QUAL void rtos_task_set_wait_period_policy( RTOS_TASK* task, int policy )
00166     {
00167       // Do nothing
00168     }
00169 
00170     INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task )
00171     {
00172       cyg_semaphore_wait(&(task->wakeup_sem));
00173       // should be used to detect overruns
00174       return 0;
00175     }
00176 
00177     INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask) {
00178       // Free name
00179       free(mytask->name);
00180       // KG: Peter does not check return values, it appears...
00181       bool succeed = cyg_thread_delete(mytask->handle);
00182       if (succeed == false)
00183           diag_printf("cyg_thread_delete: Error deleting task\n");
00184       // Free stack space
00185       free(mytask->stack);
00186     }
00187 
00188     // for both SingleTread and PeriodicThread
00189     INTERNAL_QUAL const char * rtos_task_get_name(const RTOS_TASK* t)
00190     {
00191       cyg_thread_info info;
00192       bool succeed = cyg_thread_get_info(t->handle,cyg_thread_get_id(t->handle),&info);
00193       if (succeed == false)
00194           diag_printf("fosi_internal.hpp rtos_task_get_name() WARNING: cyg_thread_get_info returned false...\n");
00195       return info.name;
00196     }
00197 
00198     INTERNAL_QUAL int rtos_task_set_priority(RTOS_TASK *t, int priority)
00199     {
00200       cyg_thread_set_priority(t->handle,(cyg_priority_t) priority);
00201       return int(cyg_thread_get_priority(t->handle)) == priority ? 0 : 1;
00202     }
00203 
00204     INTERNAL_QUAL int rtos_task_get_priority(const RTOS_TASK *t)
00205     {
00206       return (int) cyg_thread_get_priority(t->handle);
00207     }
00208 
00209     INTERNAL_QUAL int rtos_task_set_cpu_affinity(RTOS_TASK * task, unsigned cpu_affinity)
00210     {
00211     return -1;
00212     }
00213 
00214     INTERNAL_QUAL unsigned rtos_task_get_cpu_affinity(const RTOS_TASK *task)
00215     {
00216     return ~0;
00217     }
00218 
00219       INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* t, int sched_type) {
00220           if (sched_type == SCHED_ECOS_FIFO )
00221               return 0;
00222           return -1;
00223       }
00224 
00225       INTERNAL_QUAL int rtos_task_get_scheduler(const RTOS_TASK* mytask) {
00226           return SCHED_ECOS_FIFO;
00227       }
00228 
00229     INTERNAL_QUAL int rtos_task_check_scheduler(int* scheduler)
00230     {
00231         if (*scheduler != SCHED_ECOS_FIFO )
00232             log(Error) << "Unknown scheduler type." <<endlog();
00233             *scheduler = SCHED_ECOS_FIFO;
00234             return -1;
00235         }
00236         return 0;
00237     }
00238 
00239       INTERNAL_QUAL int rtos_task_check_priority(int* scheduler, int* priority)
00240       {
00241           int ret = 0;
00242           ret = rtos_task_check_scheduler(&scheduler);
00243 
00244           // FIXME: what are the valid priority ranges ???
00245 
00246           return ret;
00247       }
00248 
00249 
00250 
00251 
00252 
00253 
00254   }
00255 }
00256 #undef INTERNAL_QUAL
00257