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