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