$search
00001 /* 00002 Aseba - an event-based framework for distributed robot control 00003 Copyright (C) 2007--2010: 00004 Stephane Magnenat <stephane at magnenat dot net> 00005 (http://stephane.magnenat.net) 00006 and other contributors, see authors.txt for details 00007 00008 This program is free software: you can redistribute it and/or modify 00009 it under the terms of the GNU Lesser General Public License as published 00010 by the Free Software Foundation, version 3 of the License. 00011 00012 This program is distributed in the hope that it will be useful, 00013 but WITHOUT ANY WARRANTY; without even the implied warranty of 00014 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00015 GNU Lesser General Public License for more details. 00016 00017 You should have received a copy of the GNU Lesser General Public License 00018 along with this program. If not, see <http://www.gnu.org/licenses/>. 00019 */ 00020 00021 00022 #include <string.h> 00023 00024 // Molole includes 00025 #include <can/can.h> 00026 #include <dma/dma.h> 00027 #include <gpio/gpio.h> 00028 #include <types/types.h> 00029 #include <error/error.h> 00030 #include <flash/flash.h> 00031 00032 // Aseba includes 00033 #include <transport/can/can-net.h> 00034 #include <vm/natives.h> 00035 #include <vm/vm.h> 00036 #include <common/consts.h> 00037 #include <transport/buffer/vm-buffer.h> 00038 00039 #include "skel.h" 00040 00041 00042 static AsebaNativeFunctionDescription AsebaNativeDescription__system_reboot = 00043 { 00044 "_system.reboot", 00045 "Reboot the microcontroller", 00046 { 00047 { 0, 0 } 00048 } 00049 }; 00050 00051 void AsebaResetIntoBootloader(AsebaVMState *vm) { 00052 asm __volatile__ ("reset"); 00053 } 00054 00055 static AsebaNativeFunctionDescription AsebaNativeDescription__system_settings_read = 00056 { 00057 "_system.settings.read", 00058 "Read a setting", 00059 { 00060 { 1, "address"}, 00061 { 1, "value"}, 00062 { 0, 0 } 00063 } 00064 }; 00065 00066 static void AsebaNative__system_settings_read(AsebaVMState *vm) { 00067 uint16 address = vm->variables[AsebaNativePopArg(vm)]; 00068 uint16 destidx = AsebaNativePopArg(vm); 00069 00070 if(address > sizeof(settings)/2 - 1) { 00071 AsebaVMEmitNodeSpecificError(vm, "Address out of settings"); 00072 return; 00073 } 00074 vm->variables[destidx] = ((unsigned int *) &settings)[address]; 00075 } 00076 00077 static AsebaNativeFunctionDescription AsebaNativeDescription__system_settings_write = 00078 { 00079 "_system.settings.write", 00080 "Write a setting", 00081 { 00082 { 1, "address"}, 00083 { 1, "value"}, 00084 { 0, 0 } 00085 } 00086 }; 00087 00088 static void AsebaNative__system_settings_write(AsebaVMState *vm) { 00089 uint16 address = vm->variables[AsebaNativePopArg(vm)]; 00090 uint16 sourceidx = AsebaNativePopArg(vm); 00091 if(address > sizeof(settings)/2 - 1) { 00092 AsebaVMEmitNodeSpecificError(vm, "Address out of settings"); 00093 return; 00094 } 00095 ((unsigned int *) &settings)[address] = vm->variables[sourceidx]; 00096 } 00097 00098 00099 static AsebaNativeFunctionDescription AsebaNativeDescription__system_settings_flash = 00100 { 00101 "_system.settings.flash", 00102 "Write the settings into flash", 00103 { 00104 { 0, 0 } 00105 } 00106 }; 00107 00108 static void AsebaNative__system_settings_flash(AsebaVMState *vm); 00109 00110 00111 00112 #include <skel-user.c> 00113 00114 00115 unsigned int events_flags = 0; 00116 00117 00118 00119 struct private_settings __attribute__((aligned(2))) settings; 00120 // Nice hack to do a compilation assert with 00121 #define COMPILATION_ASSERT(e) do { enum { assert_static__ = 1/(e) };} while(0) 00122 00123 const AsebaLocalEventDescription * AsebaGetLocalEventsDescriptions(AsebaVMState *vm) 00124 { 00125 return localEvents; 00126 } 00127 00128 00129 00130 00131 00132 00133 00134 00135 /* buffers for can-net */ 00136 static __attribute((far)) CanFrame sendQueue[SEND_QUEUE_SIZE]; 00137 00138 static __attribute((far)) CanFrame recvQueue[RECV_QUEUE_SIZE]; 00139 00140 00141 /* VM */ 00142 struct __attribute((far)) _vmVariables vmVariables; 00143 00144 static __attribute((far)) uint16 vmBytecode[VM_BYTECODE_SIZE]; 00145 00146 static __attribute((far)) sint16 vmStack[VM_STACK_SIZE]; 00147 00148 AsebaVMState vmState = { 00149 0, 00150 00151 VM_BYTECODE_SIZE, 00152 vmBytecode, 00153 00154 sizeof(vmVariables) / sizeof(sint16), 00155 (sint16*)&vmVariables, 00156 00157 VM_STACK_SIZE, 00158 vmStack 00159 }; 00160 00161 00162 00163 /* Callbacks */ 00164 00165 void AsebaIdle(void) { 00166 Idle(); 00167 } 00168 00169 void AsebaPutVmToSleep(AsebaVMState *vm) { 00170 } 00171 00172 void AsebaNativeFunction(AsebaVMState *vm, uint16 id) 00173 { 00174 nativeFunctions[id](vm); 00175 } 00176 00177 00178 static void received_packet_dropped(void) 00179 { 00180 /* ---- PUT HERE SOME LED STATE OUTPUT ----- */ 00181 } 00182 00183 static void sent_packet_dropped(void) 00184 { 00185 /* ---- PUT HERE SOME LED STATE OUTPUT ----- */ 00186 } 00187 00188 const AsebaVMDescription * AsebaGetVMDescription(AsebaVMState *vm) { 00189 return &vmDescription; 00190 } 00191 00192 uint16 AsebaShouldDropPacket(uint16 source, const uint8* data) { 00193 return AsebaVMShouldDropPacket(&vmState, source, data); 00194 } 00195 00196 00197 const AsebaNativeFunctionDescription * const * AsebaGetNativeFunctionsDescriptions(AsebaVMState *vm) { 00198 return nativeFunctionsDescription; 00199 } 00200 00201 00202 // START of Bytecode into Flash section ----- 00203 00204 // A chunk is a bytecode image 00205 #define PAGE_PER_CHUNK ((VM_BYTECODE_SIZE*2+3 + INSTRUCTIONS_PER_PAGE*3 - 1) / (INSTRUCTIONS_PER_PAGE*3)) 00206 #if PAGE_PER_CHUNK == 0 00207 #undef PAGE_PER_CHUNK 00208 #define PAGE_PER_CHUNK 1 00209 #endif 00210 00211 /* noload will tell the linker to allocate the space but NOT to initialise it. (left unprogrammed)*/ 00212 /* I cannot delare it as an array since it's too large ( > 65535 ) */ 00213 00214 // Put everything in the same section, so we are 100% sure that the linker will put them contiguously. 00215 // Force the address, since the linker sometimes put it in the middle of the code 00216 #define NUMBER_OF_CHUNK 26 00217 unsigned char aseba_flash[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode"), address(0x15800 - 0x800 /* bootloader */ - 0x400 /* settings */ - NUMBER_OF_CHUNK*0x400L*PAGE_PER_CHUNK)/*, noload*/)); 00218 unsigned char aseba_flash1[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00219 unsigned char aseba_flash2[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00220 unsigned char aseba_flash3[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00221 unsigned char aseba_flash4[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00222 unsigned char aseba_flash5[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00223 unsigned char aseba_flash6[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00224 unsigned char aseba_flash7[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00225 unsigned char aseba_flash8[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00226 unsigned char aseba_flash9[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00227 unsigned char aseba_flash10[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00228 unsigned char aseba_flash11[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00229 unsigned char aseba_flash12[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00230 unsigned char aseba_flash13[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00231 unsigned char aseba_flash14[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00232 unsigned char aseba_flash15[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00233 unsigned char aseba_flash16[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00234 unsigned char aseba_flash17[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00235 unsigned char aseba_flash18[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00236 unsigned char aseba_flash19[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00237 unsigned char aseba_flash20[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00238 unsigned char aseba_flash21[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00239 unsigned char aseba_flash22[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00240 unsigned char aseba_flash23[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00241 unsigned char aseba_flash24[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00242 unsigned char aseba_flash25[PAGE_PER_CHUNK][INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), aligned(INSTRUCTIONS_PER_PAGE * 2), section(".aseba_bytecode")/*, noload*/)); 00243 00244 // Saveguard the bootloader (2 pages) 00245 unsigned char __bootloader[INSTRUCTIONS_PER_PAGE * 2 * 2] __attribute((space(prog), section(".boot"), noload, address(0x15800 - 0x800))); 00246 00247 // Put the settings page at a fixed position so it's independant of linker mood. 00248 unsigned char aseba_settings_flash[INSTRUCTIONS_PER_PAGE * 2] __attribute__ ((space(prog), section(".aseba_settings"), noload, address(0x15800 - 0x800 - 0x400))); 00249 #warning "the settings page is NOT initialised" 00250 00251 00252 unsigned long aseba_flash_ptr; 00253 unsigned long aseba_settings_ptr; 00254 00255 void AsebaWriteBytecode(AsebaVMState *vm) { 00256 // Look for the lowest number of use 00257 unsigned long min = 0xFFFFFFFF; 00258 unsigned long min_addr = 0; 00259 unsigned long count; 00260 unsigned int i; 00261 unsigned long temp_addr = aseba_flash_ptr; 00262 unsigned int instr_count; 00263 unsigned char * bcptr = (unsigned char *) vm->bytecode; 00264 // take the first minimum value 00265 for (i = 0; i < NUMBER_OF_CHUNK; i++, temp_addr += INSTRUCTIONS_PER_PAGE * 2 * PAGE_PER_CHUNK) { 00266 count = flash_read_instr(temp_addr); 00267 if(count < min) { 00268 min = count; 00269 min_addr = temp_addr; 00270 } 00271 } 00272 if(min == 0xFFFFFFFF) { 00273 AsebaVMEmitNodeSpecificError(vm, "Error: min == 0xFFFFFFFF !"); 00274 return; 00275 } 00276 min++; 00277 00278 // Now erase the pages 00279 for(i = 0; i < PAGE_PER_CHUNK; i++) 00280 flash_erase_page(min_addr + i*INSTRUCTIONS_PER_PAGE * 2); 00281 00282 // Then write the usage count and the bytecode 00283 flash_prepare_write(min_addr); 00284 flash_write_instruction(min); 00285 flash_write_buffer((unsigned char *) vm->bytecode, VM_BYTECODE_SIZE*2); 00286 flash_complete_write(); 00287 00288 00289 // Now, check the data 00290 00291 if(min != flash_read_instr(min_addr)) { 00292 AsebaVMEmitNodeSpecificError(vm, "Error: Unable to flash bytecode (1) !"); 00293 return; 00294 } 00295 min_addr += 2; 00296 instr_count = (VM_BYTECODE_SIZE*2) / 3; 00297 00298 for(i = 0; i < instr_count; i++) { 00299 unsigned char data[3]; 00300 flash_read_chunk(min_addr, 3, data); 00301 00302 if(memcmp(data, bcptr, 3)) { 00303 AsebaVMEmitNodeSpecificError(vm, "Error: Unable to flash bytecode (2) !"); 00304 return; 00305 } 00306 bcptr += 3; 00307 min_addr += 2; 00308 } 00309 00310 i = (VM_BYTECODE_SIZE * 2) % 3; 00311 00312 if(i != 0) { 00313 unsigned char data[2]; 00314 flash_read_chunk(min_addr, i, data); 00315 if(memcmp(data, bcptr, i)) { 00316 AsebaVMEmitNodeSpecificError(vm, "Error: Unable to flash bytecode (3) !"); 00317 return; 00318 } 00319 } 00320 00321 AsebaVMEmitNodeSpecificError(vm, "Flashing OK"); 00322 00323 } 00324 00325 const static unsigned int _magic_[8] = {0xDE, 0xAD, 0xCA, 0xFE, 0xBE, 0xEF, 0x04, 0x02}; 00326 static void AsebaNative__system_settings_flash(AsebaVMState *vm) { 00327 // Look for the last "Magic" we know, this is the most up to date conf 00328 // Then write the next row with the correct magic. 00329 // If no magic is found, erase the page, and then write the first one 00330 // If the last magic is found, erase the page and then write the first one 00331 00332 unsigned long setting_addr = aseba_settings_ptr; 00333 int i = 0; 00334 unsigned int mag; 00335 unsigned long temp; 00336 for(i = 0; i < 8; i++) { 00337 mag = flash_read_low(setting_addr + INSTRUCTIONS_PER_ROW * 2 * i); 00338 if(mag != _magic_[i]) 00339 break; 00340 } 00341 00342 if(i == 0 || i == 8) { 00343 flash_erase_page(setting_addr); 00344 i = 0; 00345 } 00346 00347 setting_addr += INSTRUCTIONS_PER_ROW * 2 * i; 00348 00349 flash_prepare_write(setting_addr); 00350 temp = (((unsigned long) *((unsigned char * ) &settings)) << 16) | _magic_[i]; 00351 00352 flash_write_instruction(temp); 00353 flash_write_buffer(((unsigned char *) &settings) + 1, sizeof(settings) - 1); 00354 flash_complete_write(); 00355 } 00356 00357 static void load_code_from_flash(AsebaVMState *vm) { 00358 // Find the last maximum value 00359 unsigned long max = 0; 00360 unsigned long max_addr = 0; 00361 unsigned long temp_addr = aseba_flash_ptr; 00362 unsigned long count; 00363 unsigned int i; 00364 // take the last maximum value 00365 for (i = 0; i < NUMBER_OF_CHUNK; i++, temp_addr += INSTRUCTIONS_PER_PAGE * 2 * PAGE_PER_CHUNK) { 00366 count = flash_read_instr(temp_addr); 00367 if(count >= max) { 00368 max = count; 00369 max_addr = temp_addr; 00370 } 00371 } 00372 if(!max) 00373 // Nothing to load 00374 return; 00375 00376 flash_read_chunk(max_addr + 2, VM_BYTECODE_SIZE*2, (unsigned char *) vm->bytecode); 00377 00378 // Tell the VM to init 00379 AsebaVMSetupEvent(vm, ASEBA_EVENT_INIT); 00380 } 00381 00382 int load_settings_from_flash(void) { 00383 00384 // Max size 95 int, min 1 int 00385 COMPILATION_ASSERT(sizeof(settings) < ((INSTRUCTIONS_PER_ROW*3) - 2)); 00386 COMPILATION_ASSERT(sizeof(settings) > 1); 00387 00388 // The the last "known" magic found 00389 unsigned long temp = aseba_settings_ptr; 00390 int i = 0; 00391 unsigned int mag; 00392 00393 00394 00395 for(i = 0; i < 8; i++) { 00396 mag = flash_read_low(temp + INSTRUCTIONS_PER_ROW * 2 * i); 00397 if(mag != _magic_[i]) 00398 break; 00399 } 00400 if(i == 0) 00401 // No settings found 00402 return -1; 00403 i--; 00404 temp += INSTRUCTIONS_PER_ROW * 2 * i; 00405 *((unsigned char *) &settings) = (unsigned char) (flash_read_high(temp) & 0xFF); 00406 flash_read_chunk(temp + 2, sizeof(settings) - 1, ((unsigned char *) &settings) + 1); 00407 00408 return 0; 00409 } 00410 // END of bytecode into flash section 00411 00412 00413 #ifdef ASEBA_ASSERT 00414 void AsebaAssert(AsebaVMState *vm, AsebaAssertReason reason) { 00415 AsebaVMEmitNodeSpecificError(vm, "VM ASSERT !"); 00416 } 00417 #endif 00418 00419 static unsigned int ui2str(char *str, unsigned int value) { 00420 unsigned div; 00421 unsigned int i = 0; 00422 bool hasSent = false; 00423 for (div = 10000; div > 0; div /= 10) 00424 { 00425 unsigned disp = value / div; 00426 value %= div; 00427 00428 if ((disp != 0) || (hasSent) || (div == 1)) 00429 { 00430 hasSent = true; 00431 str[i++] = '0' + disp; 00432 } 00433 } 00434 str[i] = 0; 00435 return i; 00436 } 00437 00438 static unsigned int ui2strhex(char * str, unsigned int id) { 00439 int shift; 00440 bool hasSent = false; 00441 unsigned int i = 0; 00442 for (shift = 12; shift >= 0; shift -= 4) 00443 { 00444 unsigned disp = ((id & (0xF << shift)) >> shift); 00445 00446 if ((disp != 0) || (hasSent) || (shift == 0)) 00447 { 00448 hasSent = true; 00449 if(disp > 9) 00450 str[i++] = 'A' + disp - 0xA; 00451 else 00452 str[i++] = '0' + disp; 00453 } 00454 } 00455 str[i] = 0; 00456 return i; 00457 } 00458 00459 static void __attribute__((noreturn)) error_handler(const char * file, int line, int id, void* __attribute((unused)) arg) { 00460 char error_string[255]; 00461 char number[10]; 00462 strcpy(error_string, "Molole error 0x"); 00463 ui2strhex(number, (unsigned int) id); 00464 strcat(error_string, number); 00465 strcat(error_string, " in file: "); 00466 strncat(error_string, file, 200); 00467 strcat(error_string, ":"); 00468 ui2str(number, line); 00469 strcat(error_string, number); 00470 00471 00472 AsebaVMEmitNodeSpecificError(&vmState, error_string); 00473 00474 AsebaCanFlushQueue(); 00475 00476 asm __volatile__ ("reset"); 00477 00478 for(;;); // Shutup GCC 00479 } 00480 00481 #define FUID3 0xF80016 00482 00483 void init_aseba_and_can(void) { 00484 unsigned int low, high; 00485 // Read the Device ID at _FUID3 00486 00487 vmState.nodeId = flash_read_low(FUID3); 00488 00489 // Get the section start pointer. Cannot get it in C, so use the assember-style 00490 asm ("mov #tbloffset(.startof.(.aseba_bytecode)), %[l]" : [l] "=r" (low)); 00491 asm ("mov #tblpage(.startof.(.aseba_bytecode)), %[h]" : [h] "=r" (high)); 00492 aseba_flash_ptr = (unsigned long) high << 16 | low; 00493 00494 asm ("mov #tbloffset(.startof.(.aseba_settings)), %[l]" : [l] "=r" (low)); 00495 asm ("mov #tblpage(.startof.(.aseba_settings)), %[h]" : [h] "=r" (high)); 00496 aseba_settings_ptr = (unsigned long) high << 16 | low; 00497 00498 00499 00500 can_init((can_frame_received_callback)AsebaCanFrameReceived, AsebaCanFrameSent, DMA_CAN_RX, DMA_CAN_TX, CAN_SELECT_MODE, 1000, PRIO_CAN); 00501 AsebaCanInit(vmState.nodeId, (AsebaCanSendFrameFP)can_send_frame, can_is_frame_room, received_packet_dropped, sent_packet_dropped, sendQueue, SEND_QUEUE_SIZE, recvQueue, RECV_QUEUE_SIZE); 00502 AsebaVMInit(&vmState); 00503 vmVariables.id = vmState.nodeId; 00504 00505 load_code_from_flash(&vmState); 00506 00507 error_register_callback(error_handler); 00508 00509 } 00510 00511 void __attribute((noreturn)) run_aseba_main_loop(void) { 00512 while(1) 00513 { 00514 update_aseba_variables_read(); 00515 00516 AsebaVMRun(&vmState, 1000); 00517 00518 AsebaProcessIncomingEvents(&vmState); 00519 00520 update_aseba_variables_write(); 00521 00522 // Either we are in step by step, so we go to sleep until further commands, or we are not executing an event, 00523 // and so we have nothing to do. 00524 if (AsebaMaskIsSet(vmState.flags, ASEBA_VM_STEP_BY_STEP_MASK) || AsebaMaskIsClear(vmState.flags, ASEBA_VM_EVENT_ACTIVE_MASK)) 00525 { 00526 unsigned i; 00527 // Find first bit from right (LSB), check if some CAN packets are pending 00528 // Atomically, do an Idle() if nothing is found 00529 // Warning, I'm doing some nasty things with the IPL bits (forcing the value, without safeguard) 00530 // Second warning: It doesn't disable level 7 interrupt. So if you set an event inside a level 7 interrupt 00531 // It may get delayed until next interrupt 00532 asm __volatile__ ("mov #SR, w0 \r\n" 00533 "mov #0xC0, w1\r\n" 00534 "ior.b w1, [w0], [w0]\r\n" // Disable all interrupts 00535 "ff1r [%[word]], %[b]\r\n" 00536 "bra nc, 1f\r\n" 00537 "rcall _AsebaCanRecvBufferEmpty\r\n" 00538 "cp0 w0\r\n" 00539 "bra z, 1f \r\n" 00540 "rcall _clock_idle\r\n" // Powersave WITH interrupt disabled. It works, read section 6.2.5 of dsPIC manual 00541 "1: \r\n" 00542 "mov #SR, w0 \r\n" 00543 "mov #0x1F, w1\r\n" 00544 "and.b w1, [w0],[w0] \r\n" // enable interrupts 00545 : [b] "=x" (i) : [word] "r" (&events_flags) : "cc", "w0", "w1", "w2", "w3", "w4", "w5", "w6", "w7"); 00546 // Why putting "x" as constrain register. Because it's w8-w9, so it's preserved accross function call 00547 // we do a rcall, so we must clobber w0-w7 00548 00549 // If a local event is pending, then execute it. 00550 // Else, we waked up from idle because of an interrupt, so re-execute the whole thing 00551 // FIXME: do we want to kill execution upon local events? that would be consistant so Steph votes yes, but 00552 // we may discuss further 00553 if(i && !(AsebaMaskIsSet(vmState.flags, ASEBA_VM_STEP_BY_STEP_MASK) && AsebaMaskIsSet(vmState.flags, ASEBA_VM_EVENT_ACTIVE_MASK))) { 00554 // that is the same as (thx de Morgan) 00555 //if(i && (AsebaMaskIsClear(vmState.flags, ASEBA_VM_STEP_BY_STEP_MASK) || AsebaMaskIsClear(vmState.flags, ASEBA_VM_EVENT_ACTIVE_MASK))) { 00556 i--; 00557 CLEAR_EVENT(i); 00558 vmVariables.source = vmState.nodeId; 00559 AsebaVMSetupEvent(&vmState, ASEBA_EVENT_LOCAL_EVENTS_START - i); 00560 } 00561 } 00562 } 00563 } 00564 00565