00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
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
00066
00067 if ( strlen(name) == 0 )
00068 name = "Thread";
00069 task->name = strcpy( (char*)malloc( (strlen(name) + 1) * sizeof(char)), name);
00070
00071
00072 task->stack = (char *)malloc(stack_size?stack_size:OROSEM_OS_ECOS_STACK_SIZE);
00073
00074
00075 cyg_thread_create((cyg_addrword_t) priority,
00076 (cyg_thread_entry_t *) start_routine,
00077 (cyg_addrword_t) obj,
00078 task->name,
00079 task->stack,
00080 OROSEM_OS_ECOS_STACK_SIZE,
00081 &(task->handle),
00082 &(task->thread));
00083
00084 task->hrt = false;
00085
00086 task->periodMark = 0;
00087 task->period = 0;
00088 task->counter_hdl = 0;
00089 task->alarm_hdl = 0;
00090
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 {
00109
00110
00111 mytask->sys_clk_hdl = cyg_real_time_clock();
00112
00113 cyg_clock_to_counter(mytask->sys_clk_hdl,&(mytask->counter_hdl));
00114
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
00122 cyg_alarm_initialize(mytask->alarm_hdl,
00123 rtos_get_time_ticks() + nano2ticks(nanosecs),
00124 nano2ticks(nanosecs));
00125 } else {
00126
00127 if (mytask->alarm_hdl != 0) {
00128
00129 cyg_alarm_disable(mytask->alarm_hdl);
00130 }
00131
00132
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
00140
00141 if (nanosecs != 0){
00142
00143 if (mytask->alarm_hdl == 0){
00144
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
00152
00153
00154 }
00155 }
00156 else {
00157
00158 if (mytask->alarm_hdl != 0) {
00159
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
00168 }
00169
00170 INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* task )
00171 {
00172 cyg_semaphore_wait(&(task->wakeup_sem));
00173
00174 return 0;
00175 }
00176
00177 INTERNAL_QUAL void rtos_task_delete(RTOS_TASK* mytask) {
00178
00179 free(mytask->name);
00180
00181 bool succeed = cyg_thread_delete(mytask->handle);
00182 if (succeed == false)
00183 diag_printf("cyg_thread_delete: Error deleting task\n");
00184
00185 free(mytask->stack);
00186 }
00187
00188
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
00245
00246 return ret;
00247 }
00248
00249
00250
00251
00252
00253
00254 }
00255 }
00256 #undef INTERNAL_QUAL
00257