void udp_event(void)
{
#if USE_UDP0
udp_read(&udp0);
#endif // USE_UDP0
#if USE_UDP1
udp_read(&udp1);
#endif // USE_UDP1
#if USE_UDP2
udp_read(&udp2);
#endif // USE_UDP2
}
/* include test_udp */
void
test_udp(void)
{
volatile int nsent = 0, timeout = 3;
struct udpiphdr *ui;
Signal(SIGALRM, sig_alrm);
if (sigsetjmp(jmpbuf, 1)) {
if (nsent >= 3)
err_quit("no response");
printf("timeout\n");
timeout *= 2; /* exponential backoff: 3, 6, 12 */
}
canjump = 1; /* siglongjmp is now OK */
send_dns_query();
nsent++;
alarm(timeout);
ui = udp_read();
canjump = 0;
alarm(0);
if (ui->ui_sum == 0)
printf("UDP checksums off\n");
else
printf("UDP checksums on\n");
if (verbose)
printf("received UDP checksum = %x\n", ntohs(ui->ui_sum));
}
void test(void)
{
uint8 done=0;
uint32 i,n=5;
uint8 who=0xff;
MsgMouseMove move;
//buffer_test();
for(i=0;i<sizeof(_dispatch)/sizeof(*_dispatch);++i)
{
*(_dispatch+i)=hexdump;
}
// who's there?
udp_broadcast(_options._socket,_options._port,&who,1);
memset(&move,0,sizeof(move));
move._hdr._type=MSG_MOUSE;
move._type=MSG_MOUSE_MOVE;
memcpy(move._hdr._code,_options._code,sizeof(move._hdr._code));
move._term._term=0xff;
for(i=0;i<n;++i)
{
ansleep(25);
move._x=move._y=htons(((uint16)i));
move._width=move._height=htons(((uint16)n));
udp_broadcast(_options._socket,_options._port,&move,sizeof(move));
}
while(!done)
{
done=(uint8)(udp_read(_options._socket)<0);
ansleep(25);
}
}
// This is a very weird function, look at udpCheckReceiveFork() in matlab.c for the "why"
int udpmsg_receive(udpmsg *packet)
{
char recv_buf[MAXMSGLENGTH+10];
char senderIP[16]; // "xxx.xxx.xxx.xxx\0"
int ret = 0;
if(udp_check(0)) {
udp_read(recv_buf, sizeof(recv_buf), senderIP);
// printf("%s\n", senderIP);
// First if clause determines whether message is from Psychtoolbox
if (!strcmp(senderIP, macIP)) {
if(strlen(recv_buf)>10 && recv_buf[9]==' ' && strcmp(recv_buf,last_recv)!=0) {
strcpy(last_recv, recv_buf);
strncpy(packet->id, recv_buf, 9);
packet->id[9] = 0;
strcpy(packet->msg, recv_buf+10);
if (strcmp(packet->msg,"RECEIVED")==0)
packet->received = 1;
else if (strcmp(packet->msg,"EXECUTED")==0)
packet->executed = 1;
else {
/* if it is a new message confirm its receipt */
strcpy(recv_buf+10, "RECEIVED");
udp_send(recv_buf, macIP);
}
ret = 1;
}
}
}
return ret;
}
EXPORTED_SYM ssize_t SocketRead(SOCK *sock, void *data, size_t sz)
{
ssize_t n=0, i;
if ((sock->flags & SOCK_FLAG_ACTIVE) != SOCK_FLAG_ACTIVE)
return 0;
switch (sock->protocol) {
case SOCK_TCP:
do {
i = recv(sock->sockfd, data, sz, 0);
if (i <= 0 && (errno == EAGAIN))
continue;
n = i;
} while (n == 0);
break;
case SOCK_UDP:
do {
n = udp_read(sock, data, sz);
} while (n == 0);
break;
}
return n;
}
static void udp_read_handler6(int flags, void *arg)
{
struct udp_sock *us = arg;
(void)flags;
udp_read(us, us->fd6);
}
size_t url_read(void *buf, size_t size, size_t nobj, URL *url)
{
size_t cobj; // the number of objects succeeded in reading
size_t byte_needed = size * nobj;
if(NULL == url)
{
printf("Bad parameter!\n");
return -1;
}
switch(url->protocol)
{
case PRTCL_UDP:
if(url->ts_cnt == 0)
{
size_t rslt;
rslt = udp_read(url->udp, url->buf);
//printf("rslt of udp_read() is %d\n", rslt);
if(rslt > 0)
{
url->ts_cnt += rslt;
url->pbuf = url->buf;
}
}
if(url->ts_cnt >= byte_needed)
{
memcpy(buf, url->pbuf, byte_needed);
url->pbuf += byte_needed;
url->ts_cnt -= byte_needed;
cobj = nobj;
}
else
{
memcpy(buf, url->pbuf, url->ts_cnt);
url->pbuf += url->ts_cnt;
url->ts_cnt -= url->ts_cnt;
cobj = url->ts_cnt;
}
break;
default: // PRTCL_FILE
cobj = fread(buf, size, nobj, url->fd);
break;
}
return cobj;
}
static int Get_HA7_response( struct addrinfo *now, char * name )
{
struct timeval tv = { 50, 0 };
FILE_DESCRIPTOR_OR_ERROR file_descriptor;
struct HA7_response ha7_response ;
struct sockaddr_in from ;
socklen_t fromlen = sizeof(struct sockaddr_in) ;
int on = 1;
file_descriptor = socket(now->ai_family, now->ai_socktype, now->ai_protocol) ;
if ( FILE_DESCRIPTOR_NOT_VALID(file_descriptor) ) {
ERROR_DEBUG("Cannot get socket file descriptor for broadcast.");
return 1;
}
// fcntl (file_descriptor, F_SETFD, FD_CLOEXEC); // for safe forking
if (setsockopt(file_descriptor, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) == -1) {
ERROR_DEBUG("Cannot set socket option for broadcast.");
return 1;
}
if (sendto(file_descriptor, "HA\000\001", 4, 0, now->ai_addr, now->ai_addrlen)
!= 4) {
ERROR_CONNECT("Trouble sending broadcast message");
return 1;
}
/* now read */
if ( udp_read(file_descriptor, &ha7_response, sizeof(struct HA7_response), &tv, &from, &fromlen) != sizeof(struct HA7_response) ) {
LEVEL_CONNECT("HA7 response bad length");
return 1;
}
if ( Test_HA7_response( &ha7_response ) ) {
return 1 ;
}
UCLIBCLOCK ;
snprintf(name,INET_ADDRSTRLEN+20,"%s:%d",inet_ntoa(from.sin_addr),ntohs(ha7_response.port));
UCLIBCUNLOCK ;
return 0 ;
}
// ------------------------------------------------
// Function: dhcp_discover()
// ------------------------------------------------
// Input: -
// Output: TRUE if succesful
// ------------------------------------------------
// Description: Find out a DHCP server address
// ------------------------------------------------
BOOL dhcp_discover(void)
{
BYTE opt;
PPBUF pbuf;
retry = 0;
while(retry < MAX_RETRIES) {
if(!dhcp_send(DHCPDISCOVER, TRUE)) return FALSE;
os_set_timeout(TIMEOUT_DHCP_DISCOVER);
if(udp_listen(SOCKET_DHCP, UDP_DHCP_CLI)) {
pbuf = udp_read(SOCKET_DHCP);
opt = parse_dhcp(pbuf);
release_buffer(pbuf);
if(opt == DHCPOFFER) break;
}
retry++;
}
if(retry >= MAX_RETRIES) return FALSE;
return TRUE;
}
// ------------------------------------------------
// Function: dhcp_req()
// ------------------------------------------------
// Input: -
// Output: TRUE if succesful
// ------------------------------------------------
// Description: Asks the DHCP server for an IP
// address
// ------------------------------------------------
BOOL dhcp_req(void)
{
BYTE opt;
PPBUF pbuf;
retry = 0;
while(retry < MAX_RETRIES) {
if(!dhcp_envia(DHCPREQUEST, TRUE)) return FALSE;
os_set_timeout(TIMEOUT_DHCP_REQUEST);
if(udp_listen(SOCKET_DHCP, UDP_DHCP_CLI)) {
pbuf = udp_read(SOCKET_DHCP);
opt = parse_dhcp(pbuf);
release_buffer(pbuf);
if(opt == DHCPACK) break;
}
retry++;
}
if(retry >= MAX_RETRIES) return FALSE;
return TRUE;
}
static int udp_socket_receive( comm_channel_t *channel, char *buf, int len )
{
udp_connection_t *uc = udp_get_connection( channel );
int res, count;
if( !uc )
{
return( -1 );
}
if( !uc->connected )
{
res = udp_connection_connect( uc, (uint8_t *) buf, len );
}
else
{
if( uc->idx == uc->len )
{
res = udp_read( uc->fd, uc->buf, COMM_BUF_SIZE );
if( res == -1 )
{
perror( "udp read" );
close_connection( uc );
return( res );
}
uc->len = res;
uc->idx = 0;
}
count = min( len, uc->len - uc->idx );
memcpy( buf, uc->buf + uc->idx, count );
uc->idx += count;
return( count );
}
return( res );
}
void run(void)
{
int n;
_options._xdo=xdo_new(_options._display);
if(_options._xdo)
{
while(!done())
{
os_work();
if((n=udp_read(_options._socket))<0)
{
quit();
}
if(!n)
{
ansleep(_options._sleep);
}
}
xdo_free(_options._xdo);
}
}
//--------------------------------------------------------------------------
// Function Name: snmp_read
// Description : Checks to see if any of the fd's set in the fdset belong to snmp.
// Each socket with it's fd set has a packet read from it and
// snmp_parse is called on the packet received. The resulting pdu is
// passed to the callback routine for that session. If the callback
// routine returns successfully, the pdu and it's request are
// deleted.
// Return Value : NONE
// Calling To : udp_read, snmp_parse, free_pdu and user's callback
// function
// Called By : user
// Status : some part of this module should be mdoified
//-------------------------------------------------------------------------
void snmp_read (snmp_session *session)
{
uint8 inpacket[SNMP_MAX_LEN], outpacket[SNMP_MAX_LEN];
int outlength, fromlength, inlength;
inlength = SNMP_MAX_LEN;
if(udp_read(session, inpacket, &inlength)) {
snmpInPkts++ ;
outlength = SNMP_MAX_LEN;
if(snmp_agent_parse(session, inpacket, inlength, outpacket, &outlength))
{
#if defined(SNMP_DUMP) && defined(PC_OUTPUT)
{
int count;
printf("sent %d bytes to %s:\n", (int) outlength,
inet_ntoa(from.sin_addr));
for (count = 0; count < outlength; count++) {
printf("%02X ", outpacket[count]);
if ((count % 16) == 15)
printf("\n");
}
printf("\n\n");
}
#endif
if(snmp_udp_send(session, outpacket,outlength) == 0) return;
snmp_outpkts++;
#ifdef WEBADMIN
RunChangeIPAddress();
#ifndef _PC
RunWriteDataToEEPROM();
#endif
#endif WEBADMIN
}
}
}
static void wsc_ie_read_callback(int sock, void *eloop_ctx, void *sock_ctx)
{
WSC_IE_DATA * data = eloop_ctx;
WSC_IE_COMMAND_DATA * cmdData;
char readBuf[WSC_WLAN_DATA_MAX_LENGTH];
int recvBytes;
struct sockaddr_in from;
u8 * bufPtr;
wpa_printf(MSG_DEBUG, "WSC_IE: Entered wsc_ie_read_callback. " "sock = %d", sock);
recvBytes = udp_read(data->udpFdCom, readBuf,
WSC_WLAN_DATA_MAX_LENGTH, &from);
if (recvBytes == -1) {
wpa_printf(MSG_ERROR, "WSC_IE: Reading Command message "
"from upper layer failed");
return;
}
cmdData = (WSC_IE_COMMAND_DATA *) readBuf;
if (cmdData->type == WSC_IE_TYPE_SET_BEACON_IE) {
wpa_printf(MSG_DEBUG, "WSC_IE: SET_BEACON_IE from upper layer");
bufPtr = (u8 *) &(cmdData->data[0]);
hostapd_set_wsc_beacon_ie(data->hapd, bufPtr, cmdData->length);
} else if (cmdData->type == WSC_IE_TYPE_SET_PROBE_RESPONSE_IE) {
wpa_printf(MSG_DEBUG, "WSC_IE: SET_PR_RESP_IE from upper layer");
bufPtr = (u8 *) &(cmdData->data[0]);
hostapd_set_wsc_probe_resp_ie(data->hapd, bufPtr, cmdData->length);
} else {
wpa_printf(MSG_ERROR, "WSC_IE: Wrong command type from upper layer");
return;
}
return;
}
int
get_server_info( int sock, u32 addr, u16 port)
{
u32 r_addr;
u16 r_port;
int n, i;
u8 pkt[256], *str;
pkt[0] = pkt[1] = pkt[2] = pkt[3] = 0xff;
sprintf(&pkt[4], "details");
n = udp_send(sock, addr, port, pkt, strlen(pkt));
printf(". connecting to the server... "); fflush(stdout);
if (async_read(sock, 6)<0)
goto server_down;
n = udp_read(sock, &addr, &port, pkt, sizeof(pkt));
if (n<0)
{
server_down:
printf("\bserver down!\r*\n");
exit(0);
}
printf("\bdone\n");
str = &pkt[4];
str+=strlen(str)+1;
printf("\t server_name [%s]\n", str); str+=strlen(str)+1;
printf("\t map_name [%s]\n", str); str+=strlen(str)+1;
str+=strlen(str)+1;
printf("\t game_name [%s]\n", str); str+=strlen(str)+1;
printf("\tusers_online [%d of %d]\n", str[0], str[1]); str+=3;
printf("\t remote_OS [%s]\n", (str[1]=='w' ? "windows" : (str[1]=='l' ? "linux" : "unknown")));
if (str[1]=='w') return 2;
if (str[1]=='l') return 1;
return 0;
}
static void udp_process(vector_t* for_del)
{
struct sockaddr_in srcaddr;
socklen_t addrlen = sizeof(srcaddr);
ssize_t rc;
ssize_t idx;
client_t* client = NULL;
hash_functor_t functor = {
msg_ident_hash,
msg_ident_compare,
hash_dummy_dup,
hash_dummy_dup,
msg_group_free_hash,
msg_group_free_hash_val
};
pair_t pair;
rc = udp_read(this.remotefd, this.recv_buffer, this.recv_buffer_len, &srcaddr, &addrlen);
if (rc <= 0)
{
if (errno == EAGAIN || errno == EWOULDBLOCK) return;
perror("recvfrom");
return;
}
pair.key = (void*)(long)srcaddr.sin_addr.s_addr;
pair.val = (void*)(long)srcaddr.sin_port;
idx = active_vector_exists(&this.clients, compare_clients_by_remote_ip_and_port, &pair, sizeof(pair));
if (idx >= 0)
{
size_t len;
active_vector_get(&this.clients, idx, (void**)&client, &len);
process_msg(client, (msg_t*)this.recv_buffer, for_del, idx);
}
else
{
msg_t* msg = (msg_t*)this.recv_buffer;
if (msg->syscontrol && CHECK_SYS_OP(msg->sys, SYS_LOGIN, 1)) // 除了sys login其他一律丢包
{
client = malloc(sizeof(*client));
if (client == NULL)
{
SYSLOG(LOG_ERR, "Not enough memory");
return;
}
client->remote_ip = srcaddr.sin_addr.s_addr;
client->remote_port = srcaddr.sin_port;
client->addr = srcaddr;
client->keepalive = (unsigned int)time(NULL);
client->status = CLIENT_STATUS_CHECKLOGIN | CLIENT_STATUS_WAITING_BODY;
if (!hash_init(&client->recv_table, functor, 11))
{
SYSLOG(LOG_ERR, "hash_init failed");
free(client);
return;
}
if (!active_vector_append(&this.clients, client, sizeof(*client)))
{
SYSLOG(LOG_ERR, "append to clients error");
free(client);
}
process_msg(client, msg, for_del, active_vector_count(&this.clients));
}
}
if (client) checkout_ttl(&client->recv_table);
}
static int name_server_send (int ns, struct sockaddr_in *nsap)
{
int resplen = 0;
if (badns & (1 << ns)) /* this NameServer already marked bad */
{
resolve_close();
return (NEXT_NS);
}
if (Qhook)
{
int done = 0;
int loops = 0;
do
{
res_sendhookact act = (*Qhook) (&nsap, (const u_char**)&ns_buf,
&ns_buflen, ns_ans, ns_anssiz,
&resplen);
switch (act)
{
case res_goahead:
done = 1;
break;
case res_nextns:
resolve_close();
return (NEXT_NS);
case res_done:
return (resplen);
case res_modified:
/* give the hook another try */
if (++loops < 42)
break;
/* fallthrough */
case res_error:
/* fallthrough */
default:
return (-1);
}
}
while (!done);
}
Dprint (_res.options & RES_DEBUG,
(";; Querying server (# %d) address = %s\n",
ns + 1, inet_ntoa(nsap->sin_addr)));
if (v_circuit) /* i.e. TCP */
{
int truncated;
u_short len;
u_char *cp;
/* Use virtual circuit; at most one attempt per server.
*/
Try = _res.retry;
truncated = 0;
if (!sock || !vc)
{
DWORD his_ip = ntohl (nsap->sin_addr.s_addr);
WORD his_port = ntohs (nsap->sin_port);
if (sock)
resolve_close();
sock = (sock_type*) calloc (sizeof(_tcp_Socket), 1);
if (!sock)
{
Perror ("calloc(vc)", "no memory");
return (-1);
}
if (!tcp_open(&sock->tcp,0,his_ip,his_port,NULL) ||
!tcp_conn(&sock->tcp,&errno,dns_timeout))
{
Aerror ("tcp_open/vc", "failed/timeout", *nsap);
badns |= (1 << ns);
resolve_close();
return (NEXT_NS);
}
vc = 1;
}
/* Send length & message
*/
{
int send_len = INT16SZ + ns_buflen;
BYTE *send_buf = (BYTE*) alloca (send_len);
PUTSHORT (ns_buflen, send_buf);
memcpy (send_buf + INT16SZ, ns_buf, ns_buflen);
if (sock_write(sock,send_buf,send_len) != send_len)
{
Perror ("sock_write() failed", sockerr(sock));
badns |= (1 << ns);
resolve_close();
return (NEXT_NS);
}
}
/* Receive length & response
*/
cp = ns_ans;
len = INT16SZ;
while ((n = tcp_read(&sock->tcp,cp,len,&errno,dns_timeout)) > 0)
{
cp += n;
len -= n;
if ((signed)len <= 0)
break;
}
if (n <= 0)
{
Perror ("tcp_read() failed", sockerr(sock));
resolve_close();
return (NEXT_NS);
}
resplen = _getshort (ns_ans);
if (resplen > ns_anssiz)
{
Dprint (_res.options & RES_DEBUG,(";; response truncated\n"));
truncated = 1;
len = ns_anssiz;
}
else
len = resplen;
cp = ns_ans;
while (len && (n = tcp_read(&sock->tcp,cp,len,&errno,dns_timeout)) > 0)
{
cp += n;
len -= n;
}
if (n <= 0)
{
Perror ("tcp_read(vc)",sockerr(sock));
resolve_close();
return (NEXT_NS);
}
if (truncated)
{
/* Flush rest of answer so connection stays in synch.
*/
anhp->tc = 1;
len = resplen - ns_anssiz;
while (len)
{
u_char junk[PACKETSZ];
n = (len > sizeof(junk) ? sizeof(junk) : len);
n = tcp_read (&sock->tcp,junk,n,&errno,dns_timeout);
if (n > 0)
len -= n;
else break;
}
}
}
else /* !v_circuit, i.e. UDP */
{
DWORD timeout;
if (!sock || vc)
{
if (vc)
resolve_close();
sock = (sock_type*) calloc (sizeof(_udp_Socket), 1);
if (!sock)
{
Perror ("calloc(dg)", "no memory");
return (-1);
}
connected = 0;
}
/* Connect only if we are sure we won't
* receive a response from another server.
*/
if (!connected)
{
DWORD his_ip = ntohl (nsap->sin_addr.s_addr);
WORD his_port = ntohs (nsap->sin_port);
if (!udp_open(&sock->udp,0,his_ip,his_port,NULL))
{
Aerror ("connect/dg", "ARP failed", *nsap);
badns |= (1 << ns);
resolve_close();
return (NEXT_NS);
}
connected = 1;
}
if (sock_write(sock,(const BYTE*)ns_buf,ns_buflen) != ns_buflen)
{
Perror ("sock_write() failed", "");
badns |= (1 << ns);
resolve_close();
return (NEXT_NS);
}
/* Wait for reply
*/
timeout = (unsigned)_res.retrans << Try;
if (Try > 0)
timeout /= _res.nscount;
if ((long)timeout <= 0)
timeout = 1;
wait:
n = udp_read (&sock->udp, ns_ans, ns_anssiz, &errno, timeout);
if (n == 0)
{
Dprint (_res.options & RES_DEBUG, (";; timeout\n"));
gotsomewhere = 1;
resolve_close();
return (NEXT_NS);
}
gotsomewhere = 1;
if (hp->id != anhp->id)
{
/* response from old query, ignore it.
* XXX - potential security hazard could be detected here.
*/
DprintQ ((_res.options & RES_DEBUG) || (_res.pfcode & RES_PRF_REPLY),
(";; old answer:\n"), ns_ans, resplen);
goto wait;
}
if (!(_res.options & RES_INSECURE2) &&
!res_queriesmatch(ns_buf, ns_buf+ns_buflen, ns_ans, ns_ans+ns_anssiz))
{
/* response contains wrong query? ignore it.
* XXX - potential security hazard could be detected here.
*/
DprintQ ((_res.options & RES_DEBUG) || (_res.pfcode & RES_PRF_REPLY),
(";; wrong query name:\n"), ns_ans, resplen);
goto wait;
}
if (anhp->rcode == SERVFAIL ||
anhp->rcode == NOTIMP ||
anhp->rcode == REFUSED)
{
DprintQ (_res.options & RES_DEBUG,("server rejected query:\n"),
ns_ans,resplen);
badns |= (1 << ns);
resolve_close();
/* don't retry if called from dig */
if (!_res.pfcode)
return (NEXT_NS);
}
if (!(_res.options & RES_IGNTC) && anhp->tc)
{
/* get rest of answer; use TCP with same server.
*/
Dprint (_res.options & RES_DEBUG, (";; truncated answer\n"));
v_circuit = 1;
resolve_close();
return (SAME_NS);
}
} /* if vcicuit / dg */
Dprint ((_res.options & RES_DEBUG) ||
((_res.pfcode & RES_PRF_REPLY) && (_res.pfcode & RES_PRF_HEAD1)),
(";; got answer:\n"));
DprintQ ((_res.options & RES_DEBUG) || (_res.pfcode & RES_PRF_REPLY),
(" \b"), ns_ans, resplen);
/*
* If using virtual circuits (TCP), we assume that the first server
* is preferred over the rest (i.e. it is on the local machine) and
* only keep that one open. If we have temporarily opened a virtual
* circuit, or if we haven't been asked to keep a socket open,
* close the socket.
*/
if ((v_circuit && (!(_res.options & RES_USEVC) || ns != 0)) ||
!(_res.options & RES_STAYOPEN))
resolve_close();
if (Rhook)
{
int done = 0, loops = 0;
do
{
res_sendhookact act = (*Rhook) (nsap, ns_buf, ns_buflen,
ns_ans, ns_anssiz, &resplen);
switch (act)
{
case res_goahead:
case res_done:
done = 1;
break;
case res_nextns:
resolve_close();
return (NEXT_NS);
case res_modified:
/* give the hook another try */
if (++loops < 42)
break;
/* fallthrough */
case res_error:
/* fallthrough */
default:
return (-1);
}
}
while (!done);
}
return (resplen);
}
usb_size_t
usb_read (usb_t usb, void *buffer, usb_size_t length)
{
return udp_read (usb->udp, buffer, length);
}
/**
* 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;
}
int main(int argc, char* argv[]) {
int opt;
bool vns_console = false;
char* command_file = NULL; char* pid_file = NULL;
int i, ifcount = 0; struct vnlif iflist[vnlif_maxcount];
int command_fd, command_eth = -1; FILE* pid_fd;
struct pollfd pollfds[2+2*vnlif_maxcount]; int pollres;
char buffer[MSGSIZE]; int len;
char* pktbuffer = buffer + MSGHDRSIZE;
c_base* vnsbasehdr = (c_base*)buffer;
c_hwinfo* vnshwinfo = (c_hwinfo*)buffer;
c_packet_header* vnspkthdr = (c_packet_header*)buffer;
// ---------------- parse command options ----------------
while ((opt = getopt(argc, argv, "si:c:p:")) != -1) {
switch (opt) {
case 's': vns_console = true; break;
case 'i':
if (ifcount < vnlif_maxcount) {
if (!vnlif_parse(iflist + ifcount, optarg)) die("vnlif_parse");
} else {
die("cmdline vnlif_maxcount");
}
++ifcount;
break;
case 'c': command_file = optarg; break;
case 'p': pid_file = optarg; break;
}
}
if (ifcount == 0 || command_file == NULL || pid_file == NULL) {
printf("vnlsvc: Virtual Network Lab Service\nTAP-UDP: vnlsvc -i eth0/tap0/1a:8e:22:b1:da:f1/198.51.100.1#255.255.255.0/192.0.2.1:2001/192.0.2.2:2001/ -c /tmp/command.pipe -p /tmp/vnltun.pid\nVNSconsole-UDP: vnlsvc -s -i eth0//1a:8e:22:b1:da:f1/198.51.100.1#255.255.255.0/192.0.2.1:2001/192.0.2.2:2001/ -c /tmp/command.pipe -p /tmp/vnltun.pid\n");
exit(1);
}
// ---------------- open files and sockets ----------------
command_fd = open(command_file, O_RDONLY | O_NONBLOCK);
if (command_fd < 0) die("command open");
pollfds[0].fd = 0; pollfds[0].events = vns_console ? POLLIN : 0;
pollfds[1].fd = command_fd; pollfds[1].events = POLLIN;
for (i = 0; i < ifcount; ++i) {
pollfds[2+i].fd = iflist[i].udp_fd
= udp_open(&(iflist[i].tip), &(iflist[i].rtip));
pollfds[2+i].events = POLLIN;
if (!vns_console) {
pollfds[2+ifcount+i].fd = iflist[i].tap_fd = tap_open(iflist[i].tapname);
pollfds[2+ifcount+i].events = POLLIN;
}
}
//pollfds: 0=stdin, 1=command pipe, 2..2+ifcount-1=UDP, 2+ifcount..2+2*ifcount-1=TAP
// ---------------- initialize ----------------
pid_fd = fopen(pid_file, "w");
if (pid_fd == NULL) die("pid output");
fprintf(pid_fd, "%d", getpid());
fclose(pid_fd);
srand(time(NULL));
if (vns_console) {
con_init();
vns_wAuthReq(buffer); con_write(buffer);
}
// ---------------- main poll loop ----------------
while (1) {
pollres = poll(pollfds, 2+(vns_console?1:2)*ifcount, -1);
if (pollres == -1) die("poll");
else if (pollres == 0) { perror("poll timeout"); continue; }
/*
for (i = 0; i < 2+(vns_console?1:2)*ifcount; ++i) {
if (pollfds[i].revents & (POLLERR | POLLHUP | POLLNVAL)) {
FILE* dbglog = fopen("/tmp/vnlsvc.debug.log","a");
fprintf(dbglog,"%d %d %d %x\n",time(NULL),getpid(),i,pollfds[i].revents);
fclose(dbglog);
}
}
*/
// ---------------- VNS protocol, console to UDP ----------------
if (vns_console && (pollfds[0].revents & POLLIN)) {
if (con_read(buffer)) {
switch (vns_getType(buffer)) {
case VNS_AUTH_REPLY:
vns_wAuthStatus(buffer); con_write(buffer);
break;
case VNSOPEN:
iflist_hwinfo(vnshwinfo, iflist, ifcount);
con_write(buffer);
break;
case VNSPACKET:
i = iflist_find(iflist, ifcount, vnspkthdr->mInterfaceName);
if (i < 0) break;
len = ntohl(vnspkthdr->mLen) - sizeof(c_packet_header);
if (lossy_filter(iflist[i].lossy)) {
udp_write(iflist[i].udp_fd, pktbuffer, len, &(iflist[i].rtip));
}
break;
}
}
}
// ---------------- console error ----------------
if (vns_console && (pollfds[0].revents & (POLLERR | POLLHUP))) {
die("console error");
}
// ---------------- setlossy command ----------------
if (pollfds[1].revents & POLLIN) {
if (command_eth < 0 && 1 == read(command_fd, buffer, 1)) {
command_eth = buffer[0];
}
if (1 == read(command_fd, buffer, 1) && command_eth < ifcount) {
iflist[command_eth].lossy = buffer[0];
}
}
if (pollfds[1].revents & POLLHUP) {
command_eth = -1;
close(command_fd);
pollfds[1].fd = command_fd = open(command_file, O_RDONLY | O_NONBLOCK);
if (command_fd < 0) die("command reopen");
}
for (i = 0; i < ifcount; ++i) {
// ---------------- UDP to console/TAP ----------------
if (pollfds[2+i].revents & POLLIN) {
len = udp_read(iflist[i].udp_fd, pktbuffer, &(iflist[i].rtip));
if (len > 0) {
if (vns_console) {
vns_wPacketHdr(buffer, len, iflist[i].ifname);
con_write(buffer);
} else {
tap_write(iflist[i].tap_fd, pktbuffer, len);
}
}
}
// ---------------- UDP error ----------------
if (pollfds[2+i].revents & POLLERR) {
int sockerror; socklen_t socklen = sizeof(int);
getsockopt(iflist[i].udp_fd, SOL_SOCKET, SO_ERROR, &sockerror, &socklen);
}
// ---------------- TAP to UDP ----------------
if (!vns_console && (pollfds[2+ifcount+i].revents & POLLIN)) {
len = tap_read(iflist[i].tap_fd, pktbuffer);
if (len > 0 && lossy_filter(iflist[i].lossy)) {
udp_write(iflist[i].udp_fd, pktbuffer, len, &(iflist[i].rtip));
}
}
}
}
}
