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 #include <cfg/os.h>
00094 #include <cfg/memory.h>
00095
00096 #include <string.h>
00097
00098 #include <sys/atom.h>
00099 #include <sys/heap.h>
00100 #include <sys/thread.h>
00101
00106
00113 typedef struct {
00114 u_char csf_r29;
00115 u_char csf_r28;
00116 u_char csf_r23;
00117 u_char csf_r22;
00118 u_char csf_r21;
00119 u_char csf_r20;
00120 u_char csf_r15;
00121 u_char csf_r14;
00122 u_char csf_r13;
00123 u_char csf_r12;
00124 u_char csf_r11;
00125 u_char csf_r10;
00126 #ifdef __AVR_ATmega2561__
00127 u_char csf_pcex;
00128 #endif
00129 u_char csf_pchi;
00130 u_char csf_pclo;
00131 } SWITCHFRAME;
00132
00138 typedef struct {
00139 u_char cef_arghi;
00140 u_char cef_arglo;
00144 u_char cef_yhi;
00145 u_char cef_ylo;
00146 u_char cef_rampz;
00147 u_char cef_sreg;
00148 u_char cef_r1;
00149 #ifdef __AVR_ATmega2561__
00150 u_char cef_pcex;
00151 #endif
00152 u_char cef_pchi;
00153 u_char cef_pclo;
00154 } ENTERFRAME;
00155
00156
00157
00158
00159
00160 static void NutThreadEntry(void)
00161 {
00162 asm("pop r17");
00163 asm("pop r16");
00164 asm("pop r29");
00165 asm("pop r28");
00166 asm("pop r0");
00167 asm("out 0x3B, r0");
00168 asm("pop r0");
00169 asm("pop r1");
00170 asm("out 0x3F, r0");
00171 asm("reti");
00172 }
00173
00185 void NutThreadSwitch(void)
00186 {
00187
00188
00189
00190 register u_char i = 0;
00191 register u_char j = 0;
00192
00193 asm("push r10");
00194 asm("push r11");
00195 asm("push r12");
00196 asm("push r13");
00197 asm("push r14");
00198 asm("push r15");
00199 asm("push r20");
00200 asm("push r21");
00201 asm("push r22");
00202 asm("push r23");
00203 asm("push r28");
00204 asm("push r29");
00205 asm("in %i, $3D");
00206 asm("in %j, $3E");
00207
00208 runningThread->td_sp = (((u_short) j) << 8) & 0xFF00 | (i & 0xFF);
00209
00210
00211
00212
00213
00214 asm(".globl thread_start");
00215 asm("thread_start:");
00216
00217
00218
00219
00220
00221 runningThread = runQueue;
00222 runningThread->td_state = TDS_RUNNING;
00223
00224 i = (u_char) (runningThread->td_sp & 0xFF);
00225 j = (u_char) ((runningThread->td_sp >> 8) & 0xFF);
00226
00227 asm("out $3D, %i");
00228 asm("out $3E, %j");
00229 asm("pop r29");
00230 asm("pop r28");
00231 asm("pop r23");
00232 asm("pop r22");
00233 asm("pop r21");
00234 asm("pop r20");
00235 asm("pop r15");
00236 asm("pop r14");
00237 asm("pop r13");
00238 asm("pop r12");
00239 asm("pop r11");
00240 asm("pop r10");
00241 }
00242
00261 HANDLE NutThreadCreate(u_char * name, void (*fn) (void *), void *arg, size_t stackSize)
00262 {
00263 u_char *threadMem;
00264 SWITCHFRAME *sf;
00265 ENTERFRAME *ef;
00266 NUTTHREADINFO *td;
00267 u_short yreg;
00268 const u_char *paddr;
00269
00270
00271
00272
00273 if ((threadMem = NutStackAlloc(stackSize + sizeof(NUTTHREADINFO))) == 0) {
00274 return 0;
00275 }
00276
00277 td = (NUTTHREADINFO *) (threadMem + stackSize);
00278 ef = (ENTERFRAME *) ((u_short) td - sizeof(ENTERFRAME));
00279 sf = (SWITCHFRAME *) ((u_short) ef - sizeof(SWITCHFRAME));
00280
00281
00282 memcpy(td->td_name, name, sizeof(td->td_name) - 1);
00283 td->td_name[sizeof(td->td_name) - 1] = 0;
00284 td->td_sp = (u_short) sf - 1;
00285 td->td_memory = threadMem;
00286 *((u_long *) threadMem) = DEADBEEF;
00287 *((u_long *) (threadMem + 4)) = DEADBEEF;
00288 *((u_long *) (threadMem + 8)) = DEADBEEF;
00289 *((u_long *) (threadMem + 12)) = DEADBEEF;
00290 td->td_priority = 64;
00291
00292
00293
00294
00295 paddr = (const u_char *) fn;
00296 ef->cef_pclo = *paddr;
00297 ef->cef_pchi = *(paddr + 1);
00298 #ifdef __AVR_ATmega2561__
00299 ef->cef_pcex = *(paddr + 2);
00300 #endif
00301 ef->cef_sreg = 0x80;
00302 ef->cef_rampz = 0;
00303 ef->cef_r1 = 0;
00304
00305 ef->cef_arglo = (u_char) (((u_short) arg) & 0xff);
00306 ef->cef_arghi = (u_char) (((u_short) arg) >> 8);
00307
00308 yreg = td->td_sp - 40;
00309 ef->cef_yhi = (u_char) (yreg >> 8);
00310 ef->cef_ylo = (u_char) (yreg & 0xFF);
00311
00312 paddr = (const u_char *) NutThreadEntry;
00313 sf->csf_pclo = *paddr;
00314 sf->csf_pchi = *(paddr + 1);
00315 #ifdef __AVR_ATmega2561__
00316 sf->csf_pcex = *(paddr + 2);
00317 #endif
00318
00319
00320
00321
00322
00323 td->td_next = nutThreadList;
00324 nutThreadList = td;
00325 td->td_state = TDS_READY;
00326 td->td_timer = 0;
00327 td->td_queue = 0;
00328 #ifdef NUTDEBUG
00329 if (__os_trf)
00330 fprintf(__os_trs, "Cre<%04x>", (uptr_t) td);
00331 #endif
00332
00333 NutThreadAddPriQueue(td, (NUTTHREADINFO **) & runQueue);
00334
00335 #ifdef NUTDEBUG
00336 if (__os_trf) {
00337 NutDumpThreadList(__os_trs);
00338
00339 }
00340 #endif
00341
00342
00343
00344
00345 if (runningThread == 0) {
00346 NutEnterCritical();
00347 asm("rjmp thread_start");
00348
00349 }
00350
00351
00352
00353
00354
00355
00356 if (runningThread != runQueue) {
00357 runningThread->td_state = TDS_READY;
00358 #ifdef NUTDEBUG
00359 if (__os_trf)
00360 fprintf(__os_trs, "New<%04x %04x>", (uptr_t) runningThread, (uptr_t) runQueue);
00361 #endif
00362 NutEnterCritical();
00363 NutThreadSwitch();
00364 NutExitCritical();
00365 }
00366
00367 return td;
00368 }
00369