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