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
00105
00106
00107 #include <cfg/os.h>
00108 #include <cfg/memory.h>
00109
00110 #include <string.h>
00111
00112 #include <sys/atom.h>
00113 #include <sys/heap.h>
00114 #include <sys/thread.h>
00115
00116
00117 #include <dev/irqstack.h>
00118
00119 #ifdef NUTDEBUG
00120 #include <sys/osdebug.h>
00121 #endif
00122
00127
00128 #ifdef IRQSTACK_SIZE
00129
00136 uint16_t _irqstackdec = 128;
00137 #endif
00138
00145 typedef struct {
00146 uint8_t csf_r29;
00147 uint8_t csf_r28;
00148 uint8_t csf_r17;
00149 uint8_t csf_r16;
00150 uint8_t csf_r15;
00151 uint8_t csf_r14;
00152 uint8_t csf_r13;
00153 uint8_t csf_r12;
00154 uint8_t csf_r11;
00155 uint8_t csf_r10;
00156 uint8_t csf_r9;
00157 uint8_t csf_r8;
00158 uint8_t csf_r7;
00159 uint8_t csf_r6;
00160 uint8_t csf_r5;
00161 uint8_t csf_r4;
00162 uint8_t csf_r3;
00163 uint8_t csf_r2;
00164 #ifdef __AVR_3_BYTE_PC__
00165 uint8_t csf_pcex;
00166 #endif
00167 uint8_t csf_pchi;
00168 uint8_t csf_pclo;
00169 } SWITCHFRAME;
00170
00176 typedef struct {
00177 uint8_t cef_arghi;
00178 uint8_t cef_arglo;
00179 uint8_t cef_rampz;
00180 uint8_t cef_sreg;
00181 uint8_t cef_r1;
00182 #ifdef __AVR_3_BYTE_PC__
00183 uint8_t cef_pcex;
00184 #endif
00185 uint8_t cef_pchi;
00186 uint8_t cef_pclo;
00187 } ENTERFRAME;
00188
00189 #define LONG_PTR_P(lp, mem_p) \
00190 __asm__ __volatile__("ldi %A0, lo8("#mem_p ")" "\n\t" \
00191 "ldi %B0, hi8("#mem_p ")" "\n\t" \
00192 "ldi %C0, hh8("#mem_p ")" "\n\t" \
00193 "clr %D0" \
00194 :"=d" (lp))
00195
00196
00197
00198
00199 static void NutThreadEntry(void) __attribute__ ((naked));
00200 static void NutThreadEntry(void)
00201 {
00202 __asm__ __volatile__("pop r25" "\n\t"
00203 "pop r24" "\n\t"
00204 "pop __tmp_reg__" "\n\t"
00205 "out %0, __tmp_reg__" "\n\t"
00206 "pop __tmp_reg__" "\n\t"
00207 "out %1, __tmp_reg__" "\n\t"
00208 "pop __zero_reg__" "\n\t"
00209 "reti" "\n\t"
00210 ::"I" _SFR_IO_ADDR(RAMPZ), "I" _SFR_IO_ADDR(SREG)
00211 );
00212 }
00213
00214
00226 void NutThreadSwitch(void) __attribute__ ((noinline)) __attribute__ ((naked));
00227 void NutThreadSwitch(void)
00228 {
00229
00230
00231
00232 asm volatile ("push r2" "\n\t"
00233 "push r3" "\n\t"
00234 "push r4" "\n\t"
00235 "push r5" "\n\t"
00236 "push r6" "\n\t"
00237 "push r7" "\n\t"
00238 "push r8" "\n\t"
00239 "push r9" "\n\t"
00240 "push r10" "\n\t"
00241 "push r11" "\n\t"
00242 "push r12" "\n\t"
00243 "push r13" "\n\t"
00244 "push r14" "\n\t"
00245 "push r15" "\n\t"
00246 "push r16" "\n\t"
00247 "push r17" "\n\t"
00248 "push r28" "\n\t"
00249 "push r29" "\n\t"
00250 "in %A0, %1" "\n\t"
00251 "in %B0, %2" "\n\t"
00252 :"=r" (runningThread->td_sp)
00253 :"I" _SFR_IO_ADDR(SPL),
00254 "I" _SFR_IO_ADDR(SPH)
00255 );
00256
00257
00258
00259
00260
00261 asm volatile (".global thread_start\n"
00262 "thread_start:\n\t"::);
00263
00264
00265
00266
00267 runningThread = runQueue;
00268 runningThread->td_state = TDS_RUNNING;
00269 asm volatile ("out %1, %A0" "\n\t"
00270 "out %2, %B0" "\n\t"
00271 "pop r29" "\n\t"
00272 "pop r28" "\n\t"
00273 "pop r17" "\n\t"
00274 "pop r16" "\n\t"
00275 "pop r15" "\n\t"
00276 "pop r14" "\n\t"
00277 "pop r13" "\n\t"
00278 "pop r12" "\n\t"
00279 "pop r11" "\n\t"
00280 "pop r10" "\n\t"
00281 "pop r9" "\n\t"
00282 "pop r8" "\n\t"
00283 "pop r7" "\n\t"
00284 "pop r6" "\n\t"
00285 "pop r5" "\n\t"
00286 "pop r4" "\n\t"
00287 "pop r3" "\n\t"
00288 "pop r2" "\n\t"
00289 "ret" "\n\t"
00290 ::"r" (runningThread->td_sp),
00291 "I" _SFR_IO_ADDR(SPL),
00292 "I" _SFR_IO_ADDR(SPH)
00293 );
00294 }
00295
00314 HANDLE NutThreadCreate(char * name, void (*fn) (void *), void *arg, size_t stackSize)
00315 {
00316 uint8_t *threadMem;
00317 SWITCHFRAME *sf;
00318 ENTERFRAME *ef;
00319 NUTTHREADINFO *td;
00320 #ifdef IRQSTACK_SIZE
00321 if (stackSize > _irqstackdec + 128) stackSize -= _irqstackdec;
00322 #endif
00323
00324
00325
00326
00327 if ((threadMem = NutStackAlloc(stackSize + sizeof(NUTTHREADINFO))) == 0) {
00328 return 0;
00329 }
00330
00331 td = (NUTTHREADINFO *) (threadMem + stackSize);
00332 ef = (ENTERFRAME *) ((uint16_t) td - sizeof(ENTERFRAME));
00333 sf = (SWITCHFRAME *) ((uint16_t) ef - sizeof(SWITCHFRAME));
00334
00335
00336 memcpy(td->td_name, name, sizeof(td->td_name) - 1);
00337 td->td_name[sizeof(td->td_name) - 1] = 0;
00338 td->td_sp = (uint16_t) sf - 1;
00339 td->td_memory = threadMem;
00340 *((uint32_t *) threadMem) = DEADBEEF;
00341 *((uint32_t *) (threadMem + 4)) = DEADBEEF;
00342 *((uint32_t *) (threadMem + 8)) = DEADBEEF;
00343 *((uint32_t *) (threadMem + 12)) = DEADBEEF;
00344 td->td_priority = 64;
00345
00346
00347
00348
00349 #ifdef __AVR_3_BYTE_PC__
00350 ef->cef_pcex = 0;
00351 #endif
00352 ef->cef_pchi = (uint8_t) (((uint16_t) fn) >> 8);
00353 ef->cef_pclo = (uint8_t) (((uint16_t) fn) & 0xff);
00354 ef->cef_sreg = 0x80;
00355 ef->cef_rampz = 0;
00356 ef->cef_r1 = 0;
00357
00358 ef->cef_arglo = (uint8_t) (((uint16_t) arg) & 0xff);
00359 ef->cef_arghi = (uint8_t) (((uint16_t) arg) >> 8);
00360
00361 #ifdef __AVR_3_BYTE_PC__
00362 sf->csf_pcex = 0;
00363 #endif
00364 sf->csf_pchi = (uint8_t) (((uint16_t) NutThreadEntry) >> 8);
00365 sf->csf_pclo = (uint8_t) (((uint16_t) NutThreadEntry) & 0xff);
00366
00367
00368
00369
00370
00371 td->td_next = nutThreadList;
00372 nutThreadList = td;
00373 td->td_state = TDS_READY;
00374 td->td_timer = 0;
00375 td->td_queue = 0;
00376 #ifdef NUTDEBUG
00377 if (__os_trf)
00378 fprintf(__os_trs, "Cre<%04x>", (uptr_t) td);
00379 #endif
00380
00381 NutThreadAddPriQueue(td, (NUTTHREADINFO **) & runQueue);
00382
00383 #ifdef NUTDEBUG
00384 if (__os_trf) {
00385 NutDumpThreadList(__os_trs);
00386
00387 }
00388 #endif
00389
00390
00391
00392
00393 if (runningThread == 0) {
00394 NutEnterCritical();
00395 asm volatile ("rjmp thread_start\n\t"::);
00396
00397 }
00398
00399
00400
00401
00402
00403
00404 if (runningThread != runQueue) {
00405 runningThread->td_state = TDS_READY;
00406 #ifdef NUTDEBUG
00407 if (__os_trf)
00408 fprintf(__os_trs, "New<%04x %04x>", (uptr_t) runningThread, (uptr_t) runQueue);
00409 #endif
00410 NutEnterCritical();
00411 NutThreadSwitch();
00412 NutExitCritical();
00413 }
00414
00415 return td;
00416 }
00417