dynamicloaderp.nc

tinyos-2.x.rar

      gVarMemory = malloc(prog_desc.alloc_size);
      memset(gVarMemory, 0, prog_desc.alloc_size);
    } else {
      gVarMemory = NULL;
    }
    
    // Some "real" addresses need offsets added. For example, local relocation table entries need
    // the starting code address
    for (i = 0; i < (prog_desc.patch_table_count * 4); i+=4) {
      if (i < (prog_desc.alloc_count * 4)) {
        tempUInt16 = (uint16_t)gVarMemory;   // Allocation table needs memory's offset
      } else if (i < ((prog_desc.alloc_count + prog_desc.g_reloc_count) * 4)) {
        tempUInt16 = 0;   // Global relocation table doesn't need anything
      } else {
        tempUInt16 = (uint16_t)code;   // Local relocation table needs code's offset
      }
      *((uint16_t *)&(tablesMemory[i])) = *((uint16_t *)&(tablesMemory[i])) + tempUInt16;   // Writes the real address
    }
    
    // Converts function IDs in global relocation table to real addresses
    for (i = (prog_desc.alloc_count * 4); i < ((prog_desc.alloc_count + prog_desc.g_reloc_count) * 4); i+=4) {
      tempUInt16 = *((uint16_t *)&tablesMemory[i]);   // Gets function ID
      tempUInt16 = (uint16_t)fun[tempUInt16].addr;   // Gets the real address of the function ID
      *((uint16_t *)&tablesMemory[i]) = tempUInt16;   // Writes the real address
    }
  }
  
  void loader_addr_1()
  {
    uint16_t i, laddr = 0, raddr = 0;
    
    // Resets before start
    nextTask_chain = 0x0;
    nextAddr = 0;
    
    // Gets the next task by searching for the lowest next patching address
    raddr = 0xFFFF;   // Temp variable to store the minimum patching address so far
    laddr = 0;   // Temp variable to store the current patching address
    for (i = 0; i < (prog_desc.patch_table_count * 4); i+=4) {
      laddr = *((uint16_t *)&tablesMemory[i + 2]);
      
      if (((uint16_t)nextTask_chain == 0x0 && laddr != 0xFFFF) || 
           raddr > laddr) {
        nextTask_chain = &(tablesMemory[i]);
        raddr = laddr;
      }
    }
    
    if (nextTask_chain != 0x0) {
      // Gets the next patching address in the chain from the flash
      raddr = *((uint16_t *)&nextTask_chain[2]);
      call ImageRead.read[readSource](readSourceOffset + prog_desc.code_offset + raddr, 
                                      &nextAddr, 
                                      2);
    } else {
      // Copies the rest of the binary code
      call ImageRead.read[readSource](readSourceOffset + prog_desc.code_offset + codePtr, 
                                      &(code[codePtr]), 
                                      prog_desc.code_count - codePtr);
      prog_desc.loading_stage++;
    }
  }
  
  // Patches the part of binary code that needs "real" addresses
  void loader_addr_2()
  {
    uint16_t laddr;
    
    laddr = *((uint16_t *)&nextTask_chain[2]);   // Gets the current patching address
    
    // Updates the chain with the next patching address
    if (nextAddr == 0x0000) {
      nextAddr = 0xFFFF;   // End of chain, marks it with a big number
    }
    *((uint16_t *)&nextTask_chain[2]) = nextAddr;
    
    // Patches address in the binary code
    *((uint16_t *)&code[laddr]) = *((uint16_t *)&nextTask_chain[0]);
    
    // Copies the binary code between the last patching address and the current patching address
    call ImageRead.read[readSource](readSourceOffset + prog_desc.code_offset + codePtr, 
                                    &(code[codePtr]), 
                                    laddr - codePtr);
    codePtr = laddr + 2;   // Notes up to what location in the binary code we have copied
  }
  
  void loader_datasec()
  {
    uint16_t i, j;
    
    for (i = 0; i < (prog_desc.datasec_count * 6); i+=6) {
      uint16_t destAddr = *((uint16_t *)&(tablesMemory[i])) + (uint16_t)gVarMemory;   // Writes the real address
      uint16_t srcAddr = *((uint16_t *)&(tablesMemory[i + 2])) + (uint16_t)code;   // Writes the real address
      uint16_t size = *((uint16_t *)&(tablesMemory[i + 4]));
      
      for (j = 0; j < size; j++) {
        ((uint8_t *)((void *)(destAddr + j)))[0] = ((uint8_t *)((void *)(srcAddr + j)))[0];
        //*((uint8_t *)&code[destAddr + j]) = *((uint8_t *)&code[srcAddr + j]);
      }
    }
  }
  
  void loadProgram()
  {
    error_t error = SUCCESS;
    switch (prog_desc.loading_stage) {
      case 0:
        // Loads meta data to memory array
        error = call ImageRead.read[readSource](readSourceOffset + 0, 
                                                &prog_desc, 
                                                7 * 2);
        if (error == SUCCESS) {
          prog_desc.loading_stage++;   // Moves to next loading phase
        } else {
          errorHandler();
        }
        break;
      
      case 1:
        loader_metaData();   // Gets meta data
        code = (void *) call PMManager.request(prog_desc.code_count);   // Gets the location of where the code will be copied to
        
        if ((uint16_t)code != 0xFFFF) {
          // Loads patch table to memory array
          error = call ImageRead.read[readSource](readSourceOffset + 7 * 2, 
                                                  tablesMemory, 
                                                  prog_desc.patch_table_count * 4);
          if (error == SUCCESS) {
            prog_desc.loading_stage++;   // Moves to next loading phase
          } else {
            errorHandler();
          }
        } else {
          errorHandler();
        }
        
        break;
      
      case 2:
        loader_patch_table();
        
        eeprom_w();   // Gets write-access to internal flash
        prog_desc.loading_stage++;   // Moves to next loading phase
      
      case 3:
        loader_addr_1();
        break;
      
      case 4:
        eeprom_r();   // Locks the internal flash back
        
        error = call ImageRead.read[readSource](readSourceOffset + 7 * 2 + prog_desc.patch_table_count * 4, 
                                                tablesMemory, 
                                                prog_desc.datasec_count * 6);
        if (error == SUCCESS) {
          prog_desc.loading_stage++;   // Moves to next loading phase
        } else {
          errorHandler();
        }
        
        break;
      case 5:
        loader_datasec();
        prog_desc.loading_stage++;
        
      case 6:
        post start_prog();
        break;
    }
  }
  
  command error_t Init.init()
  {
    int i;
    for (i = 0; i < TOSTHREAD_MAX_NUM_THREADS; i++) {
      threadCodeSizes[i] = 0;
      codeFirstAddrs[i] = 0;
    }
    initProgDesc();
    return SUCCESS;
  }
  
  task void taskLoadProgram()
  {
    loadProgram();
  }
  
  error_t start_load(uint8_t in_readSource, uint16_t in_readSourceOffset)
  {
    if (prog_desc.loading_stage == 0) {
      uint16_t i;
      call Leds.set(7);
      for (i = 0; i < 2000; i++) { }
      call Leds.set(0);
      
      readSource = in_readSource;
      readSourceOffset = in_readSourceOffset;
      post taskLoadProgram();   // Start Loading
      
      return SUCCESS;
    }
    
    return EBUSY;
  }
  
  command error_t DynamicLoader.loadFromFlash(uint8_t volumeId) { return start_load(volumeId, 0); }
  
  command error_t DynamicLoader.loadFromMemory(void *addr) { return start_load(READSOURCE_MEMORY, (uint16_t)addr); }
  
  event void ImageRead.readDone[uint8_t id](storage_addr_t addr, void* buf, storage_len_t len, error_t error)
  {
    if (error == SUCCESS) {
      if (buf == &nextAddr) {
        loader_addr_2();
      } else {
        post taskLoadProgram();
      }
    } else {
      errorHandler();
    }
  }

#ifndef DISABLE_LOADER_USERBUTTON
  event void UserButton.fired()
  {
     call DynamicLoader.loadFromFlash(VOLUME_MICROEXEIMAGE);
  }
#endif
  
  event void ImageRead.computeCrcDone[uint8_t id](storage_addr_t addr, storage_len_t len, uint16_t crc, error_t error) {}
  
  default command error_t ImageRead.read[uint8_t id](storage_addr_t addr, void *buf, storage_len_t len) { return FAIL; }
  default event void DynamicLoader.loadFromFlashDone(uint8_t volumeId, tosthread_t id, error_t error) {}
  default event void DynamicLoader.loadFromMemoryDone(void *addr, tosthread_t id, error_t error) {}
}