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