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
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104 #include <cfg/os.h>
00105 #include <cfg/memory.h>
00106
00107 #include <string.h>
00108
00109 #include <sys/atom.h>
00110 #include <sys/heap.h>
00111 #include <sys/thread.h>
00112
00113
00114 #include <dev/irqstack.h>
00115
00116 #ifdef NUTDEBUG
00117 #include <sys/osdebug.h>
00118 #endif
00119
00124
00125 #ifdef IRQSTACK_SIZE
00126
00133 u_short _irqstackdec = 128;
00134 #endif
00135
00142 typedef struct {
00143 u_char csf_r29;
00144 u_char csf_r28;
00145 u_char csf_r17;
00146 u_char csf_r16;
00147 u_char csf_r15;
00148 u_char csf_r14;
00149 u_char csf_r13;
00150 u_char csf_r12;
00151 u_char csf_r11;
00152 u_char csf_r10;
00153 u_char csf_r9;
00154 u_char csf_r8;
00155 u_char csf_r7;
00156 u_char csf_r6;
00157 u_char csf_r5;
00158 u_char csf_r4;
00159 u_char csf_r3;
00160 u_char csf_r2;
00161 #ifdef __AVR_3_BYTE_PC__
00162 u_char csf_pcex;
00163 #endif
00164 u_char csf_pchi;
00165 u_char csf_pclo;
00166 } SWITCHFRAME;
00167
00173 typedef struct {
00174 u_char cef_arghi;
00175 u_char cef_arglo;
00176 u_char cef_rampz;
00177 u_char cef_sreg;
00178 u_char cef_r1;
00179 #ifdef __AVR_3_BYTE_PC__
00180 u_char cef_pcex;
00181 #endif
00182 u_char cef_pchi;
00183 u_char cef_pclo;
00184 } ENTERFRAME;
00185
00186 #define LONG_PTR_P(lp, mem_p) \
00187 __asm__ __volatile__("ldi %A0, lo8("#mem_p ")" "\n\t" \
00188 "ldi %B0, hi8("#mem_p ")" "\n\t" \
00189 "ldi %C0, hh8("#mem_p ")" "\n\t" \
00190 "clr %D0" \
00191 :"=d" (lp))
00192
00193
00194
00195
00196 static void NutThreadEntry(void) __attribute__ ((naked));
00197 static void NutThreadEntry(void)
00198 {
00199 __asm__ __volatile__("pop r25" "\n\t"
00200 "pop r24" "\n\t"
00201 "pop __tmp_reg__" "\n\t"
00202 "out %0, __tmp_reg__" "\n\t"
00203 "pop __tmp_reg__" "\n\t"
00204 "out %1, __tmp_reg__" "\n\t"
00205 "pop __zero_reg__" "\n\t"
00206 "reti" "\n\t"
00207 ::"I" _SFR_IO_ADDR(RAMPZ), "I" _SFR_IO_ADDR(SREG)
00208 );
00209 }
00210
00211
00223 void NutThreadSwitch(void) __attribute__ ((noinline)) __attribute__ ((naked));
00224 void NutThreadSwitch(void)
00225 {
00226
00227
00228
00229 asm volatile ("push r2" "\n\t"
00230 "push r3" "\n\t"
00231 "push r4" "\n\t"
00232 "push r5" "\n\t"
00233 "push r6" "\n\t"
00234 "push r7" "\n\t"
00235 "push r8" "\n\t"
00236 "push r9" "\n\t"
00237 "push r10" "\n\t"
00238 "push r11" "\n\t"
00239 "push r12" "\n\t"
00240 "push r13" "\n\t"
00241 "push r14" "\n\t"
00242 "push r15" "\n\t"
00243 "push r16" "\n\t"
00244 "push r17" "\n\t"
00245 "push r28" "\n\t"
00246 "push r29" "\n\t"
00247 "in %A0, %1" "\n\t"
00248 "in %B0, %2" "\n\t"
00249 :"=r" (runningThread->td_sp)
00250 :"I" _SFR_IO_ADDR(SPL),
00251 "I" _SFR_IO_ADDR(SPH)
00252 );
00253
00254
00255
00256
00257
00258 asm volatile (".global thread_start\n"
00259 "thread_start:\n\t"::);
00260
00261
00262
00263
00264 runningThread = runQueue;
00265 runningThread->td_state = TDS_RUNNING;
00266 asm volatile ("out %1, %A0" "\n\t"
00267 "out %2, %B0" "\n\t"
00268 "pop r29" "\n\t"
00269 "pop r28" "\n\t"
00270 "pop r17" "\n\t"
00271 "pop r16" "\n\t"
00272 "pop r15" "\n\t"
00273 "pop r14" "\n\t"
00274 "pop r13" "\n\t"
00275 "pop r12" "\n\t"
00276 "pop r11" "\n\t"
00277 "pop r10" "\n\t"
00278 "pop r9" "\n\t"
00279 "pop r8" "\n\t"
00280 "pop r7" "\n\t"
00281 "pop r6" "\n\t"
00282 "pop r5" "\n\t"
00283 "pop r4" "\n\t"
00284 "pop r3" "\n\t"
00285 "pop r2" "\n\t"
00286 "ret" "\n\t"
00287 ::"r" (runningThread->td_sp),
00288 "I" _SFR_IO_ADDR(SPL),
00289 "I" _SFR_IO_ADDR(SPH)
00290 );
00291 }
00292
00311 HANDLE NutThreadCreate(char * name, void (*fn) (void *), void *arg, size_t stackSize)
00312 {
00313 u_char *threadMem;
00314 SWITCHFRAME *sf;
00315 ENTERFRAME *ef;
00316 NUTTHREADINFO *td;
00317 #ifdef IRQSTACK_SIZE
00318 if (stackSize > _irqstackdec + 128) stackSize -= _irqstackdec;
00319 #endif
00320
00321
00322
00323
00324 if ((threadMem = NutStackAlloc(stackSize + sizeof(NUTTHREADINFO))) == 0) {
00325 return 0;
00326 }
00327
00328 td = (NUTTHREADINFO *) (threadMem + stackSize);
00329 ef = (ENTERFRAME *) ((u_short) td - sizeof(ENTERFRAME));
00330 sf = (SWITCHFRAME *) ((u_short) ef - sizeof(SWITCHFRAME));
00331
00332
00333 memcpy(td->td_name, name, sizeof(td->td_name) - 1);
00334 td->td_name[sizeof(td->td_name) - 1] = 0;
00335 td->td_sp = (u_short) sf - 1;
00336 td->td_memory = threadMem;
00337 *((u_long *) threadMem) = DEADBEEF;
00338 *((u_long *) (threadMem + 4)) = DEADBEEF;
00339 *((u_long *) (threadMem + 8)) = DEADBEEF;
00340 *((u_long *) (threadMem + 12)) = DEADBEEF;
00341 td->td_priority = 64;
00342
00343
00344
00345
00346 #ifdef __AVR_3_BYTE_PC__
00347 ef->cef_pcex = 0;
00348 #endif
00349 ef->cef_pchi = (u_char) (((u_short) fn) >> 8);
00350 ef->cef_pclo = (u_char) (((u_short) fn) & 0xff);
00351 ef->cef_sreg = 0x80;
00352 ef->cef_rampz = 0;
00353 ef->cef_r1 = 0;
00354
00355 ef->cef_arglo = (u_char) (((u_short) arg) & 0xff);
00356 ef->cef_arghi = (u_char) (((u_short) arg) >> 8);
00357
00358 #ifdef __AVR_3_BYTE_PC__
00359 sf->csf_pcex = 0;
00360 #endif
00361 sf->csf_pchi = (u_char) (((u_short) NutThreadEntry) >> 8);
00362 sf->csf_pclo = (u_char) (((u_short) NutThreadEntry) & 0xff);
00363
00364
00365
00366
00367
00368 td->td_next = nutThreadList;
00369 nutThreadList = td;
00370 td->td_state = TDS_READY;
00371 td->td_timer = 0;
00372 td->td_queue = 0;
00373 #ifdef NUTDEBUG
00374 if (__os_trf)
00375 fprintf(__os_trs, "Cre<%04x>", (uptr_t) td);
00376 #endif
00377
00378 NutThreadAddPriQueue(td, (NUTTHREADINFO **) & runQueue);
00379
00380 #ifdef NUTDEBUG
00381 if (__os_trf) {
00382 NutDumpThreadList(__os_trs);
00383
00384 }
00385 #endif
00386
00387
00388
00389
00390 if (runningThread == 0) {
00391 NutEnterCritical();
00392 asm volatile ("rjmp thread_start\n\t"::);
00393
00394 }
00395
00396
00397
00398
00399
00400
00401 if (runningThread != runQueue) {
00402 runningThread->td_state = TDS_READY;
00403 #ifdef NUTDEBUG
00404 if (__os_trf)
00405 fprintf(__os_trs, "New<%04x %04x>", (uptr_t) runningThread, (uptr_t) runQueue);
00406 #endif
00407 NutEnterCritical();
00408 NutThreadSwitch();
00409 NutExitCritical();
00410 }
00411
00412 return td;
00413 }
00414