/**
 * Serialize record for network transmission.<p>
 * Uses specified MB and ME flags.<p>
 * Does not chunk records.
 */
void NdefRecord::writeToByteBuffer(byte buffer[], uint16_t index, boolean mb, boolean me) {
  boolean sr = mPayload_length < 256;
  boolean il = mId_length > 0;

  byte flags = (byte)((mb ? FLAG_MB : 0) | (me ? FLAG_ME : 0) |
          (sr ? FLAG_SR : 0) | (il ? FLAG_IL : 0) | mTnf);
  buffer[index++] = flags;
  buffer[index++] = (byte)mType_length;
  
  if (sr) {
    buffer[index++] = (byte)mPayload_length;
  } else {
    buffer[index++] = (byte)mPayload_length;
  }
  if (il) {
    buffer[index++] = (byte)mId_length;
  }

  arraycopy(mType, 0, buffer, index, mType_length);
  index += mType_length;
  arraycopy(mId, 0, buffer, index, mId_length);
  index += mId_length;
  arraycopy(mPayload, 0, buffer, index, mPayload_length);
  index += mPayload_length;
}
/**
* Reference specification: NFCForum-TS-RTD_1.0
* @brief  a new NDEF Record containing external (application-specific) data
* @param  domain        domain-name of issuing organization
* @param  type          domain-specific type of data
* @param  data          payload as bytes
* @param  data_length   length of payload
* @return
*/
void NdefRecord::createExternal(String domain, String type, byte data[], uint16_t data_length) {
  if (domain.length() == 0) {
    Serial.println("[ERROR]domain is empty");
    return;
  }
  if (type.length() == 0) {
    Serial.println("[ERROR]type is empty");
    return;
  }
  
  domain.toLowerCase();
  type.toLowerCase();

  byte byteDomain[domain.length()+1];
  domain.getBytes(byteDomain, domain.length()+1);
  byte byteType[type.length()+1];
  type.getBytes(byteType, type.length()+1);
  
  byte b[domain.length() + 1 + type.length()];
  arraycopy(byteDomain, 0, b, 0, domain.length());
  b[domain.length()] = ':';
  arraycopy(byteType, 0, b, domain.length() + 1, type.length());
  
  createNdefRecord(TNF_EXTERNAL_TYPE, b, sizeof(b), EMPTY_BYTE_ARRAY, 0, 
                    data, data_length);
}
/**
 * Perform a merge of two sort partitions.
 *
 * @param sd         The sort descriptor.
 * @param comparator The comparator used to compare elements.
 * @param strings    The input strings.
 * @param working    A working array used for the merge operations.
 * @param left       The left bound for the merge.
 * @param mid        The midpoint for the merge.
 * @param right      The right bound of merge (inclusive).
 */
void StemClass::merge(SortData *sd, int (*comparator)(SortData *, RexxString *, RexxString *), RexxString **strings, RexxString **working, size_t left, size_t mid, size_t right)
{
    size_t leftEnd = mid - 1;
    // merging

    // if arrays are already sorted - no merge
    if (comparator(sd, strings[leftEnd], strings[mid]) <= 0) {
        return;
    }

    size_t leftCursor = left;
    size_t rightCursor = mid;
    size_t workingPosition = left;

    // use merging with exponential search
    do
    {
        RexxString *fromVal = strings[leftCursor];
        RexxString *rightVal = strings[rightCursor];
        if (comparator(sd, fromVal, rightVal) <= 0)
        {
            size_t leftInsertion = find(sd, comparator, strings, rightVal, -1, leftCursor + 1, leftEnd);
            size_t toCopy = leftInsertion - leftCursor + 1;
            arraycopy(strings, leftCursor, working, workingPosition, toCopy);
            workingPosition += toCopy;
            working[workingPosition++] = rightVal;
            // now we've added this
            rightCursor++;
            // step over the section we just copied...which might be
            // all of the remaining section
            leftCursor = leftInsertion + 1;
        }
        else
        {
            size_t rightInsertion = find(sd, comparator, strings, fromVal, 0, rightCursor + 1, right);
            size_t toCopy = rightInsertion - rightCursor + 1;
            arraycopy(strings, rightCursor, working, workingPosition, toCopy);
            workingPosition += toCopy;
            // insert the right-hand value
            working[workingPosition++] = fromVal;
            leftCursor++;
            rightCursor = rightInsertion + 1;
        }
    } while (right >= rightCursor && mid > leftCursor);

    // copy rest of array.  If we've not used up the left hand side,
    // we copy that.  Otherwise, there are items on the right side
    if (leftCursor < mid)
    {
        arraycopy(strings, leftCursor, working, workingPosition, mid - leftCursor);
    }
    else
    {
        arraycopy(strings, rightCursor, working, workingPosition, right - rightCursor + 1);
    }

    arraycopy(working, left, strings, left, right - left + 1);
}
/**
* @breif an NDEF Record containing the Text-typed data
* @param  text_encode       text encoded data as bytes
* @param  language          see NdefRecord.h "unicode locale"
* @param  payload_length    length of payload_encode
* @param  encodeInUtf8      ture:utf-8 encode, false:utf-16 encode
* @return 
*/
void NdefRecord::createText(byte text_encode[],uint16_t payload_length, 
                              byte* language, boolean encodeInUtf8) 
{
  int utfBit = encodeInUtf8 ? 0 : (1 << 7);
  char status = (char) (utfBit + 2);
  byte data[1 + 2 + payload_length];
  data[0] = (byte) status;
  arraycopy(language, 0, data, 1, 2);
  arraycopy(text_encode, 0, data, (1+2), payload_length);
  createNdefRecord(TNF_WELL_KNOWN, RTD_TEXT, 1, EMPTY_BYTE_ARRAY, 0,
                    data, 1 + 2 + payload_length);
}
void LIRGenerator::do_ArrayCopy(Intrinsic* x) {
  assert(x->number_of_arguments() == 5, "wrong type");
  // Note: spill caller save before setting the item
  LIRItem src     (x->argument_at(0), this);
  LIRItem src_pos (x->argument_at(1), this);
  LIRItem dst     (x->argument_at(2), this);
  LIRItem dst_pos (x->argument_at(3), this);
  LIRItem length  (x->argument_at(4), this);
  // load all values in callee_save_registers, as this makes the
  // parameter passing to the fast case simpler
  src.load_item_force     (rlock_callee_saved(T_OBJECT));
  src_pos.load_item_force (rlock_callee_saved(T_INT));
  dst.load_item_force     (rlock_callee_saved(T_OBJECT));
  dst_pos.load_item_force (rlock_callee_saved(T_INT));
  length.load_item_force  (rlock_callee_saved(T_INT));

  int flags;
  ciArrayKlass* expected_type;
  arraycopy_helper(x, &flags, &expected_type);

  CodeEmitInfo* info = state_for(x, x->state());
  __ arraycopy(src.result(), src_pos.result(), dst.result(), dst_pos.result(),
               length.result(), rlock_callee_saved(T_INT),
               expected_type, flags, info);
  set_no_result(x);
}
Esempio n. 6
0
int taskctrl_watch(PTASKWATCHINFO info, int flag)
{
    int ret = WATCH_RET_END;

    if (!info)
        return WATCH_RET_ERROR;

    if (flag == WATCH_FLG_NEW)
    {
        watcher = tsk_getlist();
    }

    if (!watcher)
        return ret;

    while(watcher)
    {
        if (watcher->status == TASK_STAT_END)
        {
            info->type = WATCH_TYPE_END;
            arraycopy(info->taskId, 0, watcher->taskId, 0, PKG_ALL_TASKID_LEN);
            arraycopy(info->time, 0, watcher->time, 0, PKG_ALL_TIME_LEN);
            info->flag = 0;

            ret = WATCH_RET_NOEND;
        }
        else if (watcher->status == TASK_STAT_KILLED)
        {
            info->type = WATCH_TYPE_KILLED;
            arraycopy(info->taskId, 0, watcher->taskId, 0, PKG_ALL_TASKID_LEN);
            arraycopy(info->time, 0, watcher->time, 0, PKG_ALL_TIME_LEN);
            info->flag = 0;

            ret = WATCH_RET_NOEND;
        }

        watcher = watcher->next;

        if (ret == WATCH_RET_NOEND)
        {
            break;
        }
    }

    return ret;
}
Esempio n. 7
0
File: task.c Progetto: mubawa/TDE
PTASKINFO tsk_findbyid(char *id)
{
	char srcId[PKG_ALL_TASKID_LEN + 1];
	arraycopy(srcId, 0, id, 0, PKG_ALL_TASKID_LEN);
	srcId[PKG_ALL_TASKID_LEN] = '\0';

	char cpId[PKG_ALL_TASKID_LEN + 1];
	PTASKINFO temp = header;
	while(temp)
	{
		arraycopy(cpId, 0, temp->taskId, 0, PKG_ALL_TASKID_LEN);
		cpId[PKG_ALL_TASKID_LEN] = '\0';

		if (strcmp(cpId, srcId) == 0)
			return temp;
		temp = temp->next;
	}

	return NULL;
}
Esempio n. 8
0
int main() {
	int res[10], i;

	arraycopy(data, res);
	for (i = 0; i < 10; ++i) {
		printf("%f -> %d\n", data[i], res[i]);
	}

	printf("%d\n", test(45.0));
	return 0;
}
Esempio n. 9
0
File: task.c Progetto: mubawa/TDE
void tsk_add(char* id, int pid, char* command)
{
	PTASKINFO temp = (PTASKINFO)malloc(sizeof(TASKINFO));

	arraycopy(temp->taskId, 0, id, 0, PKG_ALL_TASKID_LEN);
	temp->pid = pid;
	arraycopy(temp->cmd, 0, command, 0, strlen(command));
	temp->status = TASK_STAT_RUNNING;

	temp->before = NULL;
	temp->next   = NULL;

	if (header == NULL)
		header = temp;
	else
	{
		tail->next = temp;
		temp->before = tail;
	}
	tail = temp;
}
Esempio n. 10
0
void LIRGenerator::do_ArrayCopy(Intrinsic* x) {
  assert(x->number_of_arguments() == 5, "wrong type");

  // Make all state_for calls early since they can emit code
  CodeEmitInfo* info = state_for(x, x->state());

  LIRItem src(x->argument_at(0), this);
  LIRItem src_pos(x->argument_at(1), this);
  LIRItem dst(x->argument_at(2), this);
  LIRItem dst_pos(x->argument_at(3), this);
  LIRItem length(x->argument_at(4), this);

  // operands for arraycopy must use fixed registers, otherwise
  // LinearScan will fail allocation (because arraycopy always needs a
  // call)

#ifndef _LP64
  src.load_item_force     (FrameMap::rcx_oop_opr);
  src_pos.load_item_force (FrameMap::rdx_opr);
  dst.load_item_force     (FrameMap::rax_oop_opr);
  dst_pos.load_item_force (FrameMap::rbx_opr);
  length.load_item_force  (FrameMap::rdi_opr);
  LIR_Opr tmp =           (FrameMap::rsi_opr);
#else

  // The java calling convention will give us enough registers
  // so that on the stub side the args will be perfect already.
  // On the other slow/special case side we call C and the arg
  // positions are not similar enough to pick one as the best.
  // Also because the java calling convention is a "shifted" version
  // of the C convention we can process the java args trivially into C
  // args without worry of overwriting during the xfer

  src.load_item_force     (FrameMap::as_oop_opr(j_rarg0));
  src_pos.load_item_force (FrameMap::as_opr(j_rarg1));
  dst.load_item_force     (FrameMap::as_oop_opr(j_rarg2));
  dst_pos.load_item_force (FrameMap::as_opr(j_rarg3));
  length.load_item_force  (FrameMap::as_opr(j_rarg4));

  LIR_Opr tmp =           FrameMap::as_opr(j_rarg5);
#endif // LP64

  set_no_result(x);

  int flags;
  ciArrayKlass* expected_type;
  arraycopy_helper(x, &flags, &expected_type);

  __ arraycopy(src.result(), src_pos.result(), dst.result(), dst_pos.result(), length.result(), tmp, expected_type, flags, info); // does add_safepoint
}
Esempio n. 11
0
int main()
{
     XGpio Gpio;    /* GPIO Device driver instance */

     int Status = XGpio_Initialize(&Gpio, GPIO_DEVICE_ID);
     if (Status != XST_SUCCESS) {
          return XST_FAILURE;
     }
     /*Set the direction for the LEDs to output. */
     XGpio_SetDataDirection(&Gpio, LED_CHANNEL, 0x00);
        
     printf("Allocating arrays\n");
     uint32 *A, *B;
     A = (uint32 *)sds_alloc(NUMBYTES);
     B = (uint32 *)sds_alloc(NUMBYTES);
        
     if (A == NULL)      printf("Could not allocate memory A - exiting\n");
     else if (B == NULL) printf("Could not allocate memory B - exiting\n");
     else {
          int i, j;
          int success = 1;
          printf("LEDs match iteration index...\n");
          for (j = 0; j < 64; j++) {
               XGpio_DiscreteWrite(&Gpio, LED_CHANNEL, (unsigned char) j);
               for (i = N-1; i >= 0; i-- ) {
                    A[i] = rand() % N ;
                    B[i] = 0;
               }
               printf("A: ");
               for (i = 0; i < N; i++)
                    printf("%5d", A[i]);
               printf("\n");
                  
               arraycopy(A, B);
                  
               if (compare_uint32_arrays(A, B)) {
                    success = 0;
                    break;
               }
               printf("Iteration %2d matched.  Update LEDs and spin CPU to make change visible.\n",  j);
               for (i=0; i < 50000000; i++);
          }
          printf("Freeing array\n");
          sds_free(A);
          printf("%s\n", success ? "Test passed!" : "Better luck next time...");
     } 

     return 0;
}
Esempio n. 12
0
void taskctrl_update()
{
    PTASKINFO header = tsk_getlist();
    if (!header)
        return;

    PTASKINFO tmp = header;
    while(tmp)
    {
        if (tmp->status == TASK_STAT_RUNNING
                && !os_isrunning(tmp->pid))
        {
            tmp->status = TASK_STAT_END;
            arraycopy(tmp->time, 0, os_time(), 0, PKG_ALL_TIME_LEN);
        }
        tmp = tmp->next;
    }
}
Esempio n. 13
0
void stripline(char line[]) {
    char c;
    int inTag;
    char save[MAXLEN];
    int i = 0;
    for (int j = 0; j < strlen(line); j++) {
        c = line[j];
        if (c == '<') {
            inTag = IN;
        }
        if (!inTag) {
            save[i] = c;
            i++;
        }
        if (c == '>') {
            inTag = OUT;
        }
    }
    save[i] = '\0';
    arraycopy(save, line);
}
/**
* Reference specification: NFCForum-TS-RTD_URI_1.0
* @brief  an NDEF Record containing the URI
* @param  uriString   string URI to encode.
* @return 
*/
void NdefRecord::createUri(String uriString) {
  if (uriString.length() == 0) {
    Serial.println("[ERROR]uri is empty");
    return;
  }
    
  byte prefix = 0;
#if STORED_IN_RAM
  for (int i = 1; i < sizeof(URI_PREFIX_MAP)/sizeof(URI_PREFIX_MAP[0]); i++) { 
    if (uriString.startsWith(URI_PREFIX_MAP[i])) {
      prefix = (byte) i;
      uriString = uriString.substring(URI_PREFIX_MAP[i].length());
      break;
    }
  }
#else
  uint8_t len = 0x23;
  String URI_PREFIX;
  char buffer[15]; // make sure this is large enough for the largest string it must hold
  for (int i = 1; i < len; i++) { 
    strcpy_P(buffer, (char*)pgm_read_word(&(URI_PREFIX_MAP[i]))); // Necessary casts and dereferencing, just copy. 
    URI_PREFIX = buffer;
    if (uriString.startsWith(URI_PREFIX)) {
      prefix = (byte) i;
      uriString = uriString.substring(URI_PREFIX.length());
      break;
    }
  }
#endif

  byte uriBytes[uriString.length() + 1];
  uriString.getBytes(uriBytes, uriString.length() + 1);
  byte recordBytes[uriString.length() + 1];
  recordBytes[0] = prefix;
  arraycopy(uriBytes, 0, recordBytes, 1, uriString.length());

  createNdefRecord(TNF_WELL_KNOWN, RTD_URI, 1, EMPTY_BYTE_ARRAY, 
                    0, recordBytes, sizeof(recordBytes));
}
Esempio n. 15
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;
}