void pal_nvm_multi_write32(uint8_t var_id, uint32_t *value)
{
    bool clear = false;

    // Check if current flash address is the last one of the flash section used for this variable.
    if (var_addr[var_id] == (NVM_MULTI_WRITE_START +
        ((var_id +1) * NVM_MULTI_WRITE_NUM_PG_PER_VAR * SPM_PAGESIZE) - sizeof(uint32_t)))
    {
        // Entire flash area for this variable is in use; clear flash pages used for this variable
        clear = true;
    }
    else
    {
        // Assign next storage location
        var_addr[var_id] += sizeof(uint32_t);
    }

    if (clear)
    {
        nvm_var_clear(var_id);
        var_addr[var_id] = NVM_MULTI_WRITE_START + (var_id * NVM_MULTI_WRITE_NUM_PG_PER_VAR * SPM_PAGESIZE);
    }

    // Write new value to page buffer
    flash_fill_page_buffer((var_addr[var_id] % SPM_PAGESIZE), sizeof(uint32_t), (uint8_t*)value);

    // Write page buffer to actual flash page
    flash_write_page(var_addr[var_id]);
}
int main (void) {
    struct eeprom_boot_data eedata;
    DDRB = 0;
    DDRC = 0;
    DDRD = 0;
    PORTB = 0xFF;
    PORTC = 0xFF;
    PORTD = 0xFF;
    
    DDRB |= _BV(2) | _BV(4) | _BV(5);

    seed_pseudorandom();
    load_eeprom_data(&eedata);
    init_eeprom_data(&eedata);
    save_eeprom_data(&eedata);

    if (memcmp_PP(bootloader_data, (void *)__bootloader_start, bootloader_data_len)) {
        DDRB |= _BV(PB5);

        for (int pageaddr = 0; pageaddr < bootloader_data_len; pageaddr += SPM_PAGESIZE) {
            PORTB ^= _BV(PB5);

            memcpy_P(flashdata, bootloader_data + pageaddr, SPM_PAGESIZE);

            flash_write_page((void *)__bootloader_start + pageaddr, flashdata);
        }
    }

    while (1) {
        PORTB ^= _BV(PB5);
        _delay_ms(500);
    }
}
Exemple #3
0
void flash_write(uint8_t* buf, uint32_t addr, size_t size){
	int i = 0;

	for(; i < size / PAGE_SIZE; i++){
		flash_write_page(buf + (i * PAGE_SIZE), i + (addr / PAGE_SIZE));
	}
}
/**
 * Calculate ADCS boot counter and save in flash. The boot counter is available
 * in global variable adcs_boot_counter.
 * @return The status of Flash memory and if the boot counter is valid
 */
adcs_error_status increment_boot_counter() {
    adcs_boot_cnt = 0;
    uint8_t adcs_boot_cnt_8[4] = { 0 };
    uint32_t flash_read_address = BOOT_CNT_BASE_ADDRESS;

    for (uint8_t i = 0; i < 4; i++) {
        if (flash_read_byte(&adcs_boot_cnt_8[i], flash_read_address)
                == FLASH_NORMAL) {
            flash_read_address = flash_read_address + BOOT_CNT_OFFSET_ADDRESS;
        } else {
            return ERROR_FLASH;
        }
    }
    cnv8_32(adcs_boot_cnt_8, &adcs_boot_cnt);
    adcs_boot_cnt++;
    if (flash_erase_block4K(BOOT_CNT_BASE_ADDRESS) == FLASH_ERROR) {
        return ERROR_FLASH;
    }
    cnv32_8(adcs_boot_cnt, &adcs_boot_cnt_8);
    if (flash_write_page(adcs_boot_cnt_8, sizeof(adcs_boot_cnt_8),
    BOOT_CNT_BASE_ADDRESS) == FLASH_ERROR) {
        return FLASH_ERROR;
    }
    return error_propagation(ERROR_OK);

}
void pal_nvm_multi_init(uint8_t var_id)
{
    nvm_var_clear(var_id);   // Erase/Clear entire flash page
    uint32_t value = 0x00000000;    // Set the first value to 0x00000000

    // Write first value to page buffer
    var_addr[var_id] = NVM_MULTI_WRITE_START + (var_id * NVM_MULTI_WRITE_NUM_PG_PER_VAR * SPM_PAGESIZE);
    flash_fill_page_buffer(0, sizeof(uint32_t), (uint8_t *)&value);

    // Write page buffer to actual flash page
    flash_write_page(var_addr[var_id]);
}
Exemple #6
0
void RAWHID_PROTOCOL_task()
{
	if (state.status != EXECUTING)
		return;
	uint8_t hdr = state.msg[0];
	switch (hdr) {
	case MESSAGE_DFU:
		run_bootloader();
		break; /* well... */
	case MESSAGE_WRITE_PAGE:
		if (state.len != SPM_PAGESIZE + 2) {
			state.status = MESSAGE_ERROR;
			return;
		}
		const uint8_t pageno = state.msg[1];
		uint32_t addr = LAYOUT_BEGIN + pageno*SPM_PAGESIZE;
		flash_write_page(addr, (uint8_t*)state.msg + 2);
		break;
	case MESSAGE_ACTIVATE_LAYOUT:
		if (state.len != 1) {
			state.status = MESSAGE_ERROR;
			return;
		}
		LAYOUT_set((const struct layout*)LAYOUT_BEGIN);
		break;
	case MESSAGE_DEACTIVATE_LAYOUT:
		if (state.len != 1) {
			state.status = MESSAGE_ERROR;
			return;
		}
		LAYOUT_deactivate();
		break;
	default:
		state.status = WRONG_MESSAGE_ERROR;
		return;
	};
	state.status = IDLE;
}
Exemple #7
0
int main (void){
	cpu_wakeup_init();
	clock_init();
	gpio_init();
	gpio_write(GPIO_PD7, 0);
	gpio_set_output_en(GPIO_PD7, 1);
	i2c_init();
	wd_stop();

	sleep_us(50*1000);
	
    mouse_sensor_no_fifo_init();

	sleep_us(1000*1000);	// for L3G,  must  delay enough time
	
	s16 avg_data[6];
	calib(64, avg_data);

	flash_erase_sector(AIRMOUSE_CALIBRATION_ADDR);
	flash_write_page(AIRMOUSE_CALIBRATION_ADDR, 12, avg_data);
	while (1);
	return 0;

}
Exemple #8
0
////////////////////////////////////////
// Секундный тик
////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RTC_Alarm_IRQHandler(void) { // Тик каждые 4 секунды
		int i;
#ifdef debug
		Wakeup.rtc_wakeup++;
#endif	
    if(RTC_GetITStatus(RTC_IT_ALRA) != RESET) 
		{
			RTC_ClearITPendingBit(RTC_IT_ALRA);
      EXTI_ClearITPendingBit(EXTI_Line17);
			if(!poweroff_state)
			{
			Set_next_alarm_wakeup(); // установить таймер просыпания на +4 секунды
				
			DataUpdate.Need_display_update=ENABLE;
			
			if(Power.USB_active)
			{
				USB_not_active++; // Счетчик неактивности USB
				madorc_impulse+=Detector_massive[Detector_massive_pointer]; // Счетчик импульсов для передачи по USB
			}
			
			// Счетчик времени для обновления напряжения АКБ (каждые 4 минуты)
			if(DataUpdate.Batt_update_time_counter>75) 
			{
				DataUpdate.Need_batt_voltage_update=ENABLE;
				DataUpdate.Batt_update_time_counter=0;
			} else DataUpdate.Batt_update_time_counter++;
			
			// Счетчик времени для обновления счетчика импульсов накачки
			if(DataUpdate.pump_counter_update_time>14) 
			{
#ifdef debug
				Wakeup.total_wakeup=0;
				Wakeup.total_cycle=0;
				Wakeup.rtc_wakeup=0;
				Wakeup.tim9_wakeup=0;
				Wakeup.pump_wakeup=0;
				Wakeup.comp_wakeup=0;
				Wakeup.sensor_wakeup=0;
#endif

				if(!Power.USB_active)madorc_impulse=0;
				pump_counter_avg_impulse_by_1sec[1]=pump_counter_avg_impulse_by_1sec[0];
				pump_counter_avg_impulse_by_1sec[0]=0;
				DataUpdate.pump_counter_update_time=0;

				if(pump_counter_avg_impulse_by_1sec[1]==0) //затычка на случай глюка с накачкой
				{
					dac_init();
					Pump_now(DISABLE);
					while(RTC_WakeUpCmd(DISABLE)!=SUCCESS);
					RTC_SetWakeUpCounter(0xF96); 			// Установить таймаут просыпания = 2 секунды
					current_pulse_count=0;
					while(RTC_WakeUpCmd(ENABLE)!=SUCCESS);
				}

			} else DataUpdate.pump_counter_update_time++;
	
			// Счетчик дней
			if(DataUpdate.days_sec_count>=24600) // каждые 24 часа минут
			{
				DataUpdate.days_sec_count=0;
				working_days++;
				
			} else DataUpdate.days_sec_count++;
			// Сдвиг массива дозы
			if(DataUpdate.doze_sec_count>=150) // каждые 10 минут (150)
			{
				DataUpdate.Need_update_mainscreen_counters=ENABLE;

				// -----------------------------------------------------
				DataUpdate.doze_count++;
				if(DataUpdate.doze_count>=doze_length) // Запись страницы во Flash
				//if(DataUpdate.doze_count>1) // Запись страницы во Flash
				{
					DataUpdate.doze_count=0;
					flash_write_page(DataUpdate.current_flash_page);
					DataUpdate.current_flash_page++;
					if(DataUpdate.current_flash_page > ((FLASH_END_ADDR-FLASH_START_ADDR)/FLASH_PAGE_SIZE)) // если за границами диапазона
						DataUpdate.current_flash_page=0;
				}
				// -----------------------------------------------------

				for(i=doze_length;i>0;i--)
				{
					ram_Doze_massive[i]=ram_Doze_massive[i-1];                        // сдвиг массива дозы
					ram_max_fon_massive[i]=ram_max_fon_massive[i-1];                  // сдвиг массива максимального фона
				}
				ram_Doze_massive[0]=0;
				ram_max_fon_massive[0]=0;
				DataUpdate.doze_sec_count=1; //// !!!!! 0

			} else DataUpdate.doze_sec_count++;
			////////////////////////////////////////////////////	

			
			////////////////////////////////////////////////////    
				if(Detector_massive[Detector_massive_pointer]>9)
				{	
					if(Detector_massive[Detector_massive_pointer]>199) // деление на 9 при фоне более 10 000
					{ 
						if(auto_speedup_factor!=10)auto_speedup_factor=9;
					} else
					{
						if(Detector_massive[Detector_massive_pointer]>99) // деление на 5 при фоне более 5 000
						{ 
							if(auto_speedup_factor!=5)auto_speedup_factor=5;
						} else
						{
							if(Detector_massive[Detector_massive_pointer]>19) // деление на 3 при фоне более 1 000
							{ 
								if(auto_speedup_factor!=3)auto_speedup_factor=3;
							} else
							{ // деление на 2 при фоне более 500
								if(auto_speedup_factor!=2)auto_speedup_factor=2;
							}
						}
					}
					
					if(auto_speedup_factor!=1)recalculate_fon(); // пересчет фона, если активированно ускорение
					
				} else
				{ // если ускорение не требуется
					if(auto_speedup_factor!=1){auto_speedup_factor=1;recalculate_fon();}
					else
					{	fon_level+=Detector_massive[Detector_massive_pointer];}
				}

				Detector_massive_pointer++;
				if(Detector_massive_pointer>=(Settings.Second_count>>2))	
				{
					if(auto_speedup_factor==1)fon_level-=Detector_massive[0];
					Detector_massive[0]=0;
					Detector_massive_pointer=0;
				}else
				{
					if(auto_speedup_factor==1)fon_level-=Detector_massive[Detector_massive_pointer];
Exemple #9
0
/**
 * NOTE: The technique is not the same as that used in TinyVM.
 * The return value indicates the impact of the call on the VM
 * system. EXEC_CONTINUE normal return the system should return to the return
 * address provided by the VM. EXEC_RUN The call has modified the value of
 * VM PC and this should be used to restart execution. EXEC_RETRY The call
 * needs to be re-tried (typically for a GC failure), all global state
 * should be left intact, the PC has been set appropriately.
 *
 */
int dispatch_native(TWOBYTES signature, STACKWORD * paramBase)
{
  STACKWORD p0 = paramBase[0];
  switch (signature) {
  case wait_4_5V:
    return monitor_wait((Object *) word2ptr(p0), 0);
  case wait_4J_5V:
    return monitor_wait((Object *) word2ptr(p0), ((int)paramBase[1] > 0 ? 0x7fffffff : paramBase[2]));
  case notify_4_5V:
    return monitor_notify((Object *) word2ptr(p0), false);
  case notifyAll_4_5V:
    return monitor_notify((Object *) word2ptr(p0), true);
  case start_4_5V:
    // Create thread, allow for instruction restart
    return init_thread((Thread *) word2ptr(p0));
  case yield_4_5V:
    schedule_request(REQUEST_SWITCH_THREAD);
    break;
  case sleep_4J_5V:
    sleep_thread(((int)p0 > 0 ? 0x7fffffff : paramBase[1]));
    schedule_request(REQUEST_SWITCH_THREAD);
    break;
  case getPriority_4_5I:
    push_word(get_thread_priority((Thread *) word2ptr(p0)));
    break;
  case setPriority_4I_5V:
    {
      STACKWORD p = (STACKWORD) paramBase[1];

      if (p > MAX_PRIORITY || p < MIN_PRIORITY)
	return throw_new_exception(JAVA_LANG_ILLEGALARGUMENTEXCEPTION);
      else
	set_thread_priority((Thread *) word2ptr(p0), p);
    }
    break;
  case currentThread_4_5Ljava_3lang_3Thread_2:
    push_ref(ptr2ref(currentThread));
    break;
  case interrupt_4_5V:
    interrupt_thread((Thread *) word2ptr(p0));
    break;
  case interrupted_4_5Z:
    {
      JBYTE i = currentThread->interruptState != INTERRUPT_CLEARED;

      currentThread->interruptState = INTERRUPT_CLEARED;
      push_word(i);
    }
    break;
  case isInterrupted_4_5Z:
    push_word(((Thread *) word2ptr(p0))->interruptState
	      != INTERRUPT_CLEARED);
    break;
  case join_4_5V:
    join_thread((Thread *) word2ptr(p0), 0);
    break;
  case join_4J_5V:
    join_thread((Thread *) word2obj(p0), paramBase[2]);
    break;
  case halt_4I_5V:
    schedule_request(REQUEST_EXIT);
    break;
  case shutdown_4_5V:
    shutdown_program(false);
    break;
  case currentTimeMillis_4_5J:
    push_word(0);
    push_word(systick_get_ms());
    break;
  case readSensorValue_4I_5I:
    push_word(sp_read(p0, SP_ANA));
    break;
  case setPowerTypeById_4II_5V:
    sp_set_power(p0, paramBase[1]);
    break;
  case freeMemory_4_5J:
    push_word(0);
    push_word(getHeapFree());
    break;
  case totalMemory_4_5J:
    push_word(0);
    push_word(getHeapSize());
    break;
  case floatToRawIntBits_4F_5I:	// Fall through
  case intBitsToFloat_4I_5F:
    push_word(p0);
    break;
  case doubleToRawLongBits_4D_5J:	// Fall through
  case longBitsToDouble_4J_5D:
    push_word(p0);
    push_word(paramBase[1]);
    break;
  case drawString_4Ljava_3lang_3String_2II_5V:
    {
      String *p = (String *)word2obj(p0);
      Object *charArray;
      if (!p) return throw_new_exception(JAVA_LANG_NULLPOINTEREXCEPTION);
      charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p)));
      if (!charArray) return throw_new_exception(JAVA_LANG_NULLPOINTEREXCEPTION);
      display_goto_xy(paramBase[1], paramBase[2]);
      display_jstring(p);
    }
    break;
  case drawInt_4III_5V:
    display_goto_xy(paramBase[1], paramBase[2]);
    display_int(p0, 0);
    break;
  case drawInt_4IIII_5V:
     display_goto_xy(paramBase[2], paramBase[3]);
     display_int(p0, paramBase[1]);
    break;   
  case asyncRefresh_4_5V:
    display_update();
    break;
  case clear_4_5V:
    display_clear(0);
    break;
  case getDisplay_4_5_1B:
    push_word(display_get_array());
    break;
  case setAutoRefreshPeriod_4I_5I:
    push_word(display_set_auto_update_period(p0));
    break;
  case getRefreshCompleteTime_4_5I:
    push_word(display_get_update_complete_time());
    break;
  case bitBlt_4_1BIIII_1BIIIIIII_5V:
    {
      Object *src = word2ptr(p0);
      Object *dst = word2ptr(paramBase[5]);
      display_bitblt((byte *)(src != NULL ?jbyte_array(src):NULL), paramBase[1], paramBase[2], paramBase[3], paramBase[4], (byte *)(dst!=NULL?jbyte_array(dst):NULL), paramBase[6], paramBase[7], paramBase[8], paramBase[9], paramBase[10], paramBase[11], paramBase[12]);
      break;
    }
  case getSystemFont_4_5_1B:
    push_word(display_get_font());
    break;
  case setContrast_4I_5V:
    nxt_lcd_set_pot(p0);
    break;
  case getBatteryStatus_4_5I:
    push_word(battery_voltage());
    break;
  case getButtons_4_5I:
    push_word(buttons_get());
    break;
  case getTachoCountById_4I_5I:
    push_word(nxt_motor_get_count(p0));
    break;
  case controlMotorById_4III_5V:
    nxt_motor_set_speed(p0, paramBase[1], paramBase[2]); 
    break;
  case resetTachoCountById_4I_5V:
    nxt_motor_set_count(p0, 0);
    break;
  case i2cEnableById_4II_5V:
    if (i2c_enable(p0, paramBase[1]) == 0)
      return EXEC_RETRY;
    else
      break;
  case i2cDisableById_4I_5V:
    i2c_disable(p0);
    break;
  case i2cStatusById_4I_5I:
    push_word(i2c_status(p0));
    break;
  case i2cStartById_4II_1BIII_5I:
    {
    	Object *p = word2obj(paramBase[2]);
    	JBYTE *byteArray = p ? jbyte_array(p) + paramBase[3] : NULL;
    	push_word(i2c_start(p0,
    	                    paramBase[1],
    	                    (U8 *)byteArray,
    	                    paramBase[4],
    	                    paramBase[5]));
    }
    break; 
  case i2cCompleteById_4I_1BII_5I:
    {
    	Object *p = word2ptr(paramBase[1]);
    	JBYTE *byteArray = p ? jbyte_array(p) + paramBase[2] : NULL;
    	push_word(i2c_complete(p0,
    	                       (U8 *)byteArray,
    	                       paramBase[3]));
    }
    break; 
  case playFreq_4III_5V:
    sound_freq(p0,paramBase[1], paramBase[2]);
    break;
  case btGetBC4CmdMode_4_5I:
    push_word(bt_get_mode());
    break;
  case btSetArmCmdMode_4I_5V:
    if (p0 == 0) bt_set_arm7_cmd();
    else bt_clear_arm7_cmd(); 
    break;
  case btSetResetLow_4_5V:
    bt_set_reset_low();
    break;
  case btSetResetHigh_4_5V:
    bt_set_reset_high();
    break;
  case btWrite_4_1BII_5I:
    {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(bt_write(byteArray, paramBase[1], paramBase[2]));                      
    }
    break;
  case btRead_4_1BII_5I:
    {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(bt_read(byteArray, paramBase[1], paramBase[2]));                      
    }
    break;
  case btPending_4_5I:
    {
      push_word(bt_event_check(0xffffffff));
    }
    break;
  case btEnable_4_5V:
    if (bt_enable() == 0)
      return EXEC_RETRY;
    else
      break;
  case btDisable_4_5V:
    bt_disable();
    break;
  case usbRead_4_1BII_5I:
     {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(udp_read(byteArray,paramBase[1], paramBase[2]));
    } 
    break;
  case usbWrite_4_1BII_5I:
     {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(udp_write(byteArray,paramBase[1], paramBase[2]));                      
    }
    break; 
  case usbStatus_4_5I:
    {
      push_word(udp_event_check(0xffffffff));
    }
    break;
  case usbEnable_4I_5V:
    {
      udp_enable(p0);
    }
    break;
  case usbDisable_4_5V:
    {
      udp_disable();
    }
    break;
  case usbReset_4_5V:
    udp_reset();
    break; 
  case usbSetSerialNo_4Ljava_3lang_3String_2_5V: 
    {
      byte *p = word2ptr(p0);
      int len;
      Object *charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p)));

      len = get_array_length(charArray);
      udp_set_serialno((U8 *)jchar_array(charArray), len);
    }
    break;
  case usbSetName_4Ljava_3lang_3String_2_5V:
    {
      byte *p = word2ptr(p0);
      int len;
      Object *charArray = (Object *) word2ptr(get_word_4_ns(fields_start(p)));

      len = get_array_length(charArray);
      udp_set_name((U8 *)jchar_array(charArray), len);
    }
    break;
  case flashWritePage_4_1BI_5I:
    {
      Object *p = word2ptr(p0);
      unsigned long *intArray = (unsigned long *) jint_array(p);
      push_word(flash_write_page(intArray,paramBase[1]));                      
    }
    break;
  case flashReadPage_4_1BI_5I:
    {
      Object *p = word2ptr(p0);
      unsigned long *intArray = (unsigned long *) jint_array(p);
      push_word(flash_read_page(intArray,paramBase[1]));                      
    }
    break;
  case flashExec_4II_5I:
    push_word(run_program((byte *)(&FLASH_BASE[(p0*FLASH_PAGE_SIZE)]), paramBase[1]));
    break;
  case playSample_4IIIII_5V:
    sound_play_sample(((unsigned char *) &FLASH_BASE[(p0*FLASH_PAGE_SIZE)]) + paramBase[1],paramBase[2],paramBase[3],paramBase[4]);
    break;
  case playQueuedSample_4_1BIIII_5I:
    push_word(sound_add_sample((U8 *)jbyte_array(word2obj(p0)) + paramBase[1],paramBase[2],paramBase[3],paramBase[4]));
    break;
  case getTime_4_5I:
    push_word(sound_get_time());
    break;
  case getDataAddress_4Ljava_3lang_3Object_2_5I:
    if (is_array(word2obj(p0)))
      push_word (ptr2word ((byte *) array_start(word2ptr(p0))));
    else
      push_word (ptr2word ((byte *) fields_start(word2ptr(p0))));
    break;
  case getObjectAddress_4Ljava_3lang_3Object_2_5I:
    push_word(p0);
    break;
  case gc_4_5V:
    // Restartable garbage collection
    return garbage_collect();
  case shutDown_4_5V:
    shutdown(); // does not return
  case boot_4_5V:
    display_clear(1);
    while (1) nxt_avr_firmware_update_mode(); // does not return 
  case arraycopy_4Ljava_3lang_3Object_2ILjava_3lang_3Object_2II_5V:
    return arraycopy(word2ptr(p0), paramBase[1], word2ptr(paramBase[2]), paramBase[3], paramBase[4]);
  case executeProgram_4I_5V:
    // Exceute program, allow for instruction re-start
    return execute_program(p0);
  case setDebug_4_5V:
    set_debug(word2ptr(p0));
    break;
  case eventOptions_4II_5I:
    {
      byte old = debugEventOptions[p0];
      debugEventOptions[p0] = (byte)paramBase[1];
      push_word(old);
    }
    break;
  case suspendThread_4Ljava_3lang_3Object_2_5V:
    suspend_thread(ref2ptr(p0));
    break;
  case resumeThread_4Ljava_3lang_3Object_2_5V:
    resume_thread(ref2ptr(p0));
    break;
  case getProgramExecutionsCount_4_5I:
    push_word(gProgramExecutions);
    break;
  case getFirmwareRevision_4_5I:
    push_word((STACKWORD) getRevision());
    break;
  case getFirmwareRawVersion_4_5I:
    push_word((STACKWORD) VERSION_NUMBER); 
    break;
  case hsEnable_4II_5V:
    {
      if (hs_enable((int)p0, (int)paramBase[1]) == 0)
        return EXEC_RETRY;
    }
    break;
  case hsDisable_4_5V:
    {
      hs_disable();
    }
    break;
  case hsWrite_4_1BII_5I:
    {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(hs_write(byteArray, paramBase[1], paramBase[2]));                      
    }
    break;
  case hsRead_4_1BII_5I:
    {
      Object *p = word2ptr(p0);
      byte *byteArray = (byte *) jbyte_array(p);
      push_word(hs_read(byteArray, paramBase[1], paramBase[2]));                      
    }
    break;
  case hsPending_4_5I:
    {
      push_word(hs_pending());
    }
    break;
  case hsSend_4BB_1BII_1C_5I:
    {
      Object *p = word2ptr(paramBase[2]);
      U8 *data = (U8 *)jbyte_array(p);
      p = word2ptr(paramBase[5]);
      U16 *crc = (U16 *)jchar_array(p);
      push_word(hs_send((U8) p0, (U8)paramBase[1], data, paramBase[3], paramBase[4], crc));
    }
    break;
  case hsRecv_4_1BI_1CI_5I:
    {
      Object *p = word2ptr(p0);
      U8 *data = (U8 *)jbyte_array(p);
      p = word2ptr(paramBase[2]);
      U16 *crc = (U16 *)jchar_array(p);
      push_word(hs_recv(data, paramBase[1], crc, paramBase[3]));
    }
    break;
    
  case getUserPages_4_5I:
    push_word(FLASH_MAX_PAGES - flash_start_page);
    break;
  case setVMOptions_4I_5V:
    gVMOptions = p0;
    break;
  case getVMOptions_4_5I:
    push_word(gVMOptions);
    break;
  case isAssignable_4II_5Z:
    push_word(is_assignable(p0, paramBase[1]));
    break;
  case cloneObject_4Ljava_3lang_3Object_2_5Ljava_3lang_3Object_2:
    {
      Object *newObj = clone((Object *)ref2obj(p0));
      if (newObj == NULL) return EXEC_RETRY;
      push_word(obj2ref(newObj));
    }
    break;
  case memPeek_4III_5I:
    push_word(mem_peek(p0, paramBase[1], paramBase[2]));
    break;
  case memCopy_4Ljava_3lang_3Object_2IIII_5V:
    mem_copy(word2ptr(p0), paramBase[1], paramBase[2], paramBase[3], paramBase[4]);
    break;
  case memGetReference_4II_5Ljava_3lang_3Object_2:
    push_word(mem_get_reference(p0, paramBase[1]));
    break;
  case setSensorPin_4III_5V:
    sp_set(p0, paramBase[1], paramBase[2]);
    break;
  case getSensorPin_4II_5I:
    push_word(sp_get(p0, paramBase[1]));
    break;
  case setSensorPinMode_4III_5V:
    sp_set_mode(p0, paramBase[1], paramBase[2]);
    break;
  case readSensorPin_4II_5I:
    push_word(sp_read(p0, paramBase[1]));
    break;
  case nanoTime_4_5J:
    {
      U64 ns = systick_get_ns();
      push_word(ns >> 32);
      push_word(ns);
    }
    break;
  case createStackTrace_4Ljava_3lang_3Thread_2Ljava_3lang_3Object_2_5_1I:
    {
      Object *trace = create_stack_trace((Thread *)ref2obj(p0), ref2obj(paramBase[1]));
      if (trace == NULL) return EXEC_RETRY;
      push_word(obj2ref(trace));
    }
    break;
  case registerEvent_4_5I:
    push_word(register_event((NXTEvent *) ref2obj(p0)));
    break;
  case unregisterEvent_4_5I:
    push_word(unregister_event((NXTEvent *) ref2obj(p0)));
    break;
  case changeEvent_4II_5I:
    push_word(change_event((NXTEvent *) ref2obj(p0), paramBase[1], paramBase[2]));
    break;
  case isInitialized_4I_5Z:
    push_word(is_initialized_idx(p0));
    break;
  case allocate_4II_5Ljava_3lang_3Object_2:
    {
      Object *allocated;
      if(paramBase[1]>0){
        allocated=new_single_array(p0,paramBase[1]);
      }else{
        allocated=new_object_for_class(p0);
      }
      if(allocated == NULL) return EXEC_RETRY;
      push_word(obj2ref(allocated));
    }
    break;
  case memPut_4IIII_5V:
    store_word_ns((byte *)(memory_base[p0] + paramBase[1]), paramBase[2],paramBase[3]);
    break;
  case notifyEvent_4ILjava_3lang_3Thread_2_5Z:
    push_word(debug_event(paramBase[1], NULL, (Thread*) ref2obj(paramBase[2]), 0, 0, 0, 0));
    break;
  case setThreadRequest_4Ljava_3lang_3Thread_2Llejos_3nxt_3debug_3SteppingRequest_2_5V:
    {
      Thread *th = (Thread*) ref2obj(p0);
      th->debugData = (REFERENCE) paramBase[1];
      // currently we only get stepping requests
      if(paramBase[1])
        th->flags |= THREAD_STEPPING;
      else
        th->flags &= ~THREAD_STEPPING;
    }
    break;
  case isStepping_4Ljava_3lang_3Thread_2_5Z:
    {
      Thread *th = (Thread*) ref2obj(p0);
      push_word(is_stepping(th));
    }
    break;
  case setBreakpointList_4_1Llejos_3nxt_3debug_3Breakpoint_2I_5V:
    breakpoint_set_list((Breakpoint**) array_start(p0), paramBase[1]);
    break;
  case enableBreakpoint_4Llejos_3nxt_3debug_3Breakpoint_2Z_5V:
    breakpoint_enable((Breakpoint*) word2ptr(p0), (boolean) paramBase[1]);
    break;
  case firmwareExceptionHandler_4Ljava_3lang_3Throwable_2II_5V:
    firmware_exception_handler((Throwable *)p0, paramBase[1], paramBase[2]);
    break;
  case exitThread_4_5V:
    currentThread->state = DEAD;
    schedule_request(REQUEST_SWITCH_THREAD);
    break;
  case updateThreadFlags_4Ljava_3lang_3Thread_2II_5I:
    ((Thread *)p0)->flags |= paramBase[1];
    ((Thread *)p0)->flags &= ~paramBase[2];
//printf("m %x %d\n", p0, ((Thread *)p0)->flags);
    push_word(((Thread *)p0)->flags);
    break;
    
  default:
    return throw_new_exception(JAVA_LANG_NOSUCHMETHODERROR);
  }
  return EXEC_CONTINUE;
}