skel.c
Go to the documentation of this file.
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         


aseba
Author(s): Stéphane Magnenat
autogenerated on Sun Oct 5 2014 23:46:39