int wsc_ie_init(struct hostapd_data *hapd)
{
struct sockaddr_in to;
char sendBuf[5];
wpa_printf(MSG_DEBUG, "WSC_IE: In wsc_ie_init");
g_wsc_ie_data = malloc(sizeof(WSC_IE_DATA));
if (g_wsc_ie_data == NULL) {
return -1;
}
memset(g_wsc_ie_data, 0, sizeof(WSC_IE_DATA));
g_wsc_ie_data->hapd = hapd;
g_wsc_ie_data->udpFdCom = udp_open();
eloop_register_read_sock(g_wsc_ie_data->udpFdCom, wsc_ie_read_callback,
g_wsc_ie_data, NULL);
/* Send a start packet */
strcpy(sendBuf, "PORT");
to.sin_addr.s_addr = inet_addr(WSC_WLAN_UDP_ADDR);
to.sin_family = AF_INET;
to.sin_port = htons(WSC_WLAN_UDP_PORT);
if (udp_write(g_wsc_ie_data->udpFdCom, sendBuf, 5, &to) < 5) {
wpa_printf(MSG_ERROR, "WSC_IE: Sending Port message to "
"upper Layer failed");
return -1;
} else {
wpa_printf(MSG_DEBUG, "WSC_IE: send port = %d", WSC_WLAN_UDP_PORT );
}
return 0;
}
static int write_rtcp_pkt( hnd_t handle )
{
obe_rtp_ctx *p_rtp = handle;
uint64_t ntp_time = obe_ntp_time();
uint8_t pkt[100];
bs_t s;
bs_init( &s, pkt, RTCP_PACKET_SIZE );
bs_write( &s, 2, RTP_VERSION ); // version
bs_write1( &s, 0 ); // padding
bs_write( &s, 5, 0 ); // reception report count
bs_write( &s, 8, RTCP_SR_PACKET_TYPE ); // packet type
bs_write( &s, 8, 6 ); // length (length in words - 1)
bs_write32( &s, p_rtp->ssrc ); // ssrc
bs_write32( &s, ntp_time / 1000000 ); // NTP timestamp, most significant word
bs_write32( &s, ((ntp_time % 1000000) << 32) / 1000000 ); // NTP timestamp, least significant word
bs_write32( &s, 0 ); // RTP timestamp FIXME
bs_write32( &s, p_rtp->pkt_cnt ); // sender's packet count
bs_write32( &s, p_rtp->octet_cnt ); // sender's octet count
bs_flush( &s );
if( udp_write( p_rtp->udp_handle, pkt, RTCP_PACKET_SIZE ) < 0 )
return -1;
return 0;
}
static int write_rtp_pkt( hnd_t handle, uint8_t *data, int len, int64_t timestamp )
{
obe_rtp_ctx *p_rtp = handle;
uint8_t pkt[RTP_HEADER_SIZE+TS_PACKETS_SIZE];
bs_t s;
bs_init( &s, pkt, RTP_HEADER_SIZE+TS_PACKETS_SIZE );
bs_write( &s, 2, RTP_VERSION ); // version
bs_write1( &s, 0 ); // padding
bs_write1( &s, 0 ); // extension
bs_write( &s, 4, 0 ); // CSRC count
bs_write1( &s, 0 ); // marker
bs_write( &s, 7, MPEG_TS_PAYLOAD_TYPE ); // payload type
bs_write( &s, 16, p_rtp->seq++ ); // sequence number
bs_write32( &s, timestamp / 300 ); // timestamp
bs_write32( &s, p_rtp->ssrc ); // ssrc
bs_flush( &s );
memcpy( &pkt[RTP_HEADER_SIZE], data, len );
if( udp_write( p_rtp->udp_handle, pkt, RTP_HEADER_SIZE+TS_PACKETS_SIZE ) < 0 )
return -1;
p_rtp->pkt_cnt++;
p_rtp->octet_cnt += len;
return 0;
}
/*
Just send the buffer to the host in the rtcp_info.
*/
static void
rtcp_send(iptv_rtcp_info_t *info, sbuf_t *buffer)
{
tvhdebug(LS_IPTV, "RTCP: Sending receiver report");
// We don't care of the result right now
udp_write(info->connection, buffer->sb_data, buffer->sb_ptr, & info->connection->peer);
}
static int udp_socket_transmit( comm_channel_t *channel, const char *buf, int len )
{
udp_connection_t *uc = udp_get_connection( channel );
int res;
if( !uc )
{
return( -1 );
}
if( !uc->connected )
{
errno = EAGAIN;
return( -1 );
}
if( uc->connected )
{
res = udp_write( uc->fd, (uint8_t *) buf, len );
if( res == -1 )
{
close_connection( uc );
}
}
return( res );
}
int socket_write(Socket_T S, void *b, int size) {
int n= 0;
void *p= b;
ASSERT(S);
/* Clear any extra data read from the server */
socket_reset(S);
while(size > 0) {
if(S->ssl) {
n= send_ssl_socket(S->ssl, p, size, S->timeout);
} else {
if(S->type==SOCK_DGRAM)
n= udp_write(S->socket, p, size, S->timeout);
else
n= sock_write(S->socket, p, size, S->timeout);
}
if(n <= 0) break;
p+= n;
size-= n;
}
if(n < 0) {
/* No write or a partial write is an error */
return -1;
}
return (int)(p - b);
}
/* include send_dns_query */
void send_dns_query(void)
{
size_t nbytes;
char *buf, *ptr;
buf = Malloc(sizeof(struct udpiphdr) + 100);
ptr = buf + sizeof(struct udpiphdr); /* leave room for IP/UDP headers */
*((uint16_t *)ptr) = htons(1234); /* identification */
ptr += 2;
*((uint16_t *)ptr) = htons(0x0100); /* flags: recursion desired */
ptr += 2;
*((uint16_t *)ptr) = htons(1); /* # questions */
ptr += 2;
*((uint16_t *)ptr) = 0; /* # answer RRs */
ptr += 2;
*((uint16_t *)ptr) = 0; /* # authority RRs */
ptr += 2;
*((uint16_t *)ptr) = 0; /* # additional RRs */
ptr += 2;
memcpy(ptr, "\001a\014root-servers\003net\000", 20);
ptr += 20;
*((uint16_t *)ptr) = htons(1); /* query type = A */
ptr += 2;
*((uint16_t *)ptr) = htons(1); /* query class = 1 (IP addr) */
ptr += 2;
nbytes = (ptr - buf) - sizeof(struct udpiphdr);
udp_write(buf, nbytes);
if (verbose) printf("sent: %d bytes of data\n", nbytes);
}
int main(int argc, char *argv[])
{
char buf[MAXLINE];
char *ptr;
struct rip_data *rip;
int fd;
int userlen;
int version;
struct in_addr src;
struct sockaddr_in to;
const int on = 1;
if (argc < 4)
err_quit("Usage: %s <src> <dst> <version> [mask]", basename(argv[0]));
version = atoi(argv[3]);
if (version != 1 && version != 2)
err_quit("Unknown RIP version: %d\n", version);
bzero(buf, sizeof(buf));
ptr = buf + sizeof(struct ip) + sizeof(struct udphdr);
*((uint8_t *)ptr) = (uint8_t)1; /* request */
*((uint8_t *)(ptr + 1)) = (uint8_t)version; /* version */
rip = (struct rip_data *)(ptr + 4);
rip->rip_metric = htonl((uint32_t)16);
if (argc == 5)
if (inet_pton(AF_INET, argv[4], &rip->rip_mask) != 1)
err_sys("inet_pton error");
userlen = 4 + sizeof(struct rip_data);
if (inet_pton(AF_INET, argv[1], &src) != 1)
err_sys("inet_pton error");
bzero(&to, sizeof(struct sockaddr_in));
to.sin_family = AF_INET;
to.sin_port = htons(RIPSERV);
if (inet_pton(AF_INET, argv[2], &to.sin_addr) != 1)
err_sys("inet_pton error");
if ((fd = socket(AF_INET, SOCK_RAW, IPPROTO_UDP)) < 0)
err_sys("socket error");
if (setsockopt(fd, IPPROTO_IP, IP_HDRINCL, &on, sizeof(on)) < 0)
err_sys("setsockopt error");
udp_write(fd, buf, userlen,
src, to.sin_addr,
htons(RIPSERV), htons(RIPSERV),
(struct sockaddr *)&to, sizeof(to));
return 0;
}
int main(int argc, char *argv[])
{
char buf[MAXLINE];
char *ptr;
struct rip_data *rip;
int fd;
int userlen;
int metric;
struct in_addr src;
struct sockaddr_in to;
const int on = 1;
if (argc != 5)
err_quit("Usage: %s <src> <dst> <addr> <metric>", basename(argv[0]));
metric = atoi(argv[4]);
if (metric >= 16 || metric <= 0)
err_quit("RIP metric out of range: %d\n", metric);
bzero(buf, sizeof(buf));
ptr = buf + sizeof(struct ip) + sizeof(struct udphdr);
*((uint8_t *)ptr) = (uint8_t)2; /* reply */
*((uint8_t *)(ptr + 1)) = (uint8_t)1; /* RIPv1 */
rip = (struct rip_data *)(ptr + 4);
rip->rip_af = htons((uint16_t)2); /* address family */
rip->rip_metric = htonl(metric);
if (inet_pton(AF_INET, argv[3], &rip->rip_addr) != 1)
err_sys("inet_pton error");
userlen = 4 + sizeof(struct rip_data);
if (inet_pton(AF_INET, argv[1], &src) != 1)
err_sys("inet_pton error");
bzero(&to, sizeof(struct sockaddr_in));
to.sin_family = AF_INET;
to.sin_port = htons(RIPSERV);
if (inet_pton(AF_INET, argv[2], &to.sin_addr) != 1)
err_sys("inet_pton error");
if ((fd = socket(AF_INET, SOCK_RAW, IPPROTO_UDP)) < 0)
err_sys("socket error");
if (setsockopt(fd, IPPROTO_IP, IP_HDRINCL, &on, sizeof(on)) < 0)
err_sys("setsockopt error");
udp_write(fd, buf, userlen,
src, to.sin_addr,
htons(RIPSERV), htons(RIPSERV),
(struct sockaddr *)&to, sizeof(to));
return 0;
}
int wsc_event_notify(char * pDataSend)
{
static int initialized=0;
static int event_sock=0;
struct sockaddr_in to;
struct sockaddr_in from;
char tmpBuffer[WSC_EVENT_RX_BUF_SIZE];
int recvBytes;
if(initialized==0){
event_sock = udp_open();
initialized ++;
}
memset(&to,0,sizeof(to));
to.sin_addr.s_addr = inet_addr(WSC_EVENT_ADDR);
to.sin_family = AF_INET;
to.sin_port = htons(WSC_EVENT_PORT);
if (udp_write(event_sock, pDataSend, strlen(pDataSend), &to) < strlen(pDataSend))
{
wpa_printf(MSG_ERROR, "Hostapd: Sending WSC event to "
"upper Layer failed");
return 1;
}
recvBytes = udp_read_timed(event_sock, (char *) tmpBuffer,
WSC_EVENT_RX_BUF_SIZE, &from, 15);
if (recvBytes == -1)
{
wpa_printf(MSG_INFO, "EAP-WSC: Event messageACK timeout failure\n");
return 1;
}
if(strncmp(tmpBuffer,WSC_EVENT_ACK_STRING, strlen(WSC_EVENT_ACK_STRING))!=0)
{
wpa_printf(MSG_INFO, "EAP-WSC: wrong WSC_EVENT ACK message \n");
return 1;
}
return 0;
}
int
udp_write_queue( udp_connection_t *uc, htsbuf_queue_t *q,
struct sockaddr_storage *storage )
{
htsbuf_data_t *hd;
int l, r = 0;
void *p;
while ((hd = TAILQ_FIRST(&q->hq_q)) != NULL) {
if (!r) {
l = hd->hd_data_len - hd->hd_data_off;
p = hd->hd_data + hd->hd_data_off;
r = udp_write(uc, p, l, storage);
}
htsbuf_data_free(q, hd);
}
q->hq_size = 0;
return r;
}
int main(int argc, char *argv[])
{
int nbyte, port, sport;
struct in_addr src;
struct sockaddr_in to;
char buf[MAXSIZE];
int fd;
const int on = 1;
if (argc != 5)
err_quit("Usage: %s <src> <dst> <port> <bytes>", basename(argv[0]));
if ((nbyte = atoi(argv[4])) >= sizeof(buf) - 28) /* 28 = IP header + UDP header */
err_quit("Packet too large");
port = atoi(argv[3]);
sport = (getpid() & 0xffff) | 0x8000;
if (inet_pton(AF_INET, argv[1], &src) != 1) {
errno = EINVAL;
err_sys("inet_pton error");
}
bzero(&to, sizeof(struct sockaddr_in));
to.sin_family = AF_INET;
to.sin_port = htons(port);
if (inet_pton(AF_INET, argv[2], &to.sin_addr) != 1) {
errno = EINVAL;
err_sys("inet_pton error");
}
if ((fd = socket(AF_INET, SOCK_RAW, IPPROTO_UDP)) < 0)
err_sys("socket SOCK_RAW error");
if (setsockopt(fd, IPPROTO_IP, IP_HDRINCL, &on, sizeof(on)) < 0)
err_sys("setsockopt IP_HDRINCL error");
udp_write(fd, buf, nbyte, src, to.sin_addr,
htons(sport), htons(port),
(struct sockaddr *)&to, sizeof(to));
return 0;
}
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));
}
}
}
}
}
static u8 * eap_wsc_com_buildReq(struct eap_sm *sm, struct eap_wsc_data *data,
int id, size_t *reqDataLen)
{
u8 *req = NULL;
int recvBytes;
struct sockaddr_in from;
struct sockaddr_in to;
WSC_NOTIFY_DATA notifyData;
WSC_NOTIFY_DATA * recvNotify;
notifyData.type = WSC_NOTIFY_TYPE_BUILDREQ;
notifyData.u.bldReq.id = id;
notifyData.u.bldReq.state = data->state;
notifyData.length = 0;
to.sin_addr.s_addr = inet_addr(WSC_EAP_UDP_ADDR);
to.sin_family = AF_INET;
to.sin_port = htons(WSC_EAP_UDP_PORT);
memcpy(notifyData.sta_mac_addr, sm->sta->addr, ETH_ALEN);
if (udp_write(data->udpFdEap, (char *) ¬ifyData,
sizeof(WSC_NOTIFY_DATA), &to) < sizeof(WSC_NOTIFY_DATA))
{
wpa_printf(MSG_INFO, "EAP-WSC: Sending Eap message to "
"upper Layer failed\n");
data->state = FAILURE;
return NULL;
}
recvBytes = udp_read_timed(data->udpFdEap, (char *) data->recvBuf,
WSC_RECVBUF_SIZE, &from, 15); /* Jerry timeout value */
if (recvBytes == -1)
{
wpa_printf(MSG_INFO, "EAP-WSC: req Reading EAP message "
"from upper layer failed\n");
data->state = FAILURE;
return NULL;
}
recvNotify = (WSC_NOTIFY_DATA *) data->recvBuf;
if ( (recvNotify->type != WSC_NOTIFY_TYPE_BUILDREQ_RESULT) ||
(recvNotify->length == 0) ||
(recvNotify->u.bldReqResult.result != WSC_NOTIFY_RESULT_SUCCESS) )
{
wpa_printf(MSG_INFO, "EAP-WSC: Build Request failed "
"soemwhere\n");
data->state = FAILURE;
return NULL;
}
req = (u8 *) malloc(recvNotify->length);
if ( ! req)
{
wpa_printf(MSG_INFO, "EAP-WSC: Memory allocation "
"for the request failed\n");
data->state = FAILURE;
return NULL;
}
memcpy(req, recvNotify + 1, recvNotify->length);
*reqDataLen = recvNotify->length;
data->state = CONTINUE;
sm->eapol_cb->set_eap_respTimeout(sm->eapol_ctx,15);
return req;
}
void send_rtp_packet(
struct UdpSocket *sock,
uint8_t * Jpeg, int JpegLen,
uint32_t m_SequenceNumber, uint32_t m_Timestamp,
uint32_t m_offset, uint8_t marker_bit,
int w, int h,
uint8_t format_code, uint8_t quality_code,
uint8_t has_dri_header)
{
#define KRtpHeaderSize 12 // size of the RTP header
#define KJpegHeaderSize 8 // size of the special JPEG payload header
uint8_t RtpBuf[2048];
int RtpPacketSize = JpegLen + KRtpHeaderSize + KJpegHeaderSize;
memset(RtpBuf,0x00,sizeof(RtpBuf));
/*
The RTP header has the following format:
0 1 2 3
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|V=2|P|X| CC |M| PT | sequence number |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| timestamp |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| synchronization source (SSRC) identifier |
+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+
| contributing source (CSRC) identifiers |
| .... |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* */
// Prepare the 12 byte RTP header
RtpBuf[0] = 0x80; // RTP version
RtpBuf[1] = 0x1a + (marker_bit<<7); // JPEG payload (26) and marker bit
RtpBuf[2] = m_SequenceNumber >> 8;
RtpBuf[3] = m_SequenceNumber & 0x0FF; // each packet is counted with a sequence counter
RtpBuf[4] = (m_Timestamp & 0xFF000000) >> 24; // each image gets a timestamp
RtpBuf[5] = (m_Timestamp & 0x00FF0000) >> 16;
RtpBuf[6] = (m_Timestamp & 0x0000FF00) >> 8;
RtpBuf[7] = (m_Timestamp & 0x000000FF);
RtpBuf[8] = 0x13; // 4 byte SSRC (sychronization source identifier)
RtpBuf[9] = 0xf9; // we just an arbitrary number here to keep it simple
RtpBuf[10] = 0x7e;
RtpBuf[11] = 0x67;
/* JPEG header", are as follows:
*
* http://tools.ietf.org/html/rfc2435
0 1 2 3
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| Type-specific | Fragment Offset |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| Type | Q | Width | Height |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
*/
// Prepare the 8 byte payload JPEG header
RtpBuf[12] = 0x00; // type specific
RtpBuf[13] = (m_offset & 0x00FF0000) >> 16; // 3 byte fragmentation offset for fragmented images
RtpBuf[14] = (m_offset & 0x0000FF00) >> 8;
RtpBuf[15] = (m_offset & 0x000000FF);
RtpBuf[16] = 0x00; // type: 0 422 or 1 421
RtpBuf[17] = 60; // quality scale factor
RtpBuf[16] = format_code; // type: 0 422 or 1 421
if (has_dri_header)
RtpBuf[16] |= 0x40; // DRI flag
RtpBuf[17] = quality_code; // quality scale factor
RtpBuf[18] = w/8; // width / 8 -> 48 pixel
RtpBuf[19] = h/8; // height / 8 -> 32 pixel
// append the JPEG scan data to the RTP buffer
memcpy(&RtpBuf[20],Jpeg,JpegLen);
udp_write(sock,RtpBuf,RtpPacketSize);
};
static u8 * eap_wps_process(struct eap_sm *sm, void *priv,
struct eap_method_ret *ret,
const u8 *reqData, size_t reqDataLen,
size_t *respDataLen)
{
struct eap_wps_data *data = priv;
struct eap_hdr *req;
int recvBytes;
u8 * resp;
u8 * sendBuf;
u32 sendBufLen;
struct sockaddr_in from;
struct sockaddr_in to;
WPS_NOTIFY_DATA notifyData;
WPS_NOTIFY_DATA * recvNotify;
wpa_printf(MSG_DEBUG,"@#*@#*@#*EAP-WPS: Entered eap_wps_process *#@*#@*#@");
req = (struct eap_hdr *) reqData;
wpa_printf(MSG_DEBUG, "EAP-WPS : Received packet(len=%lu) ",
(unsigned long) reqDataLen);
if(ntohs(req->length) != reqDataLen)
{
wpa_printf(MSG_INFO, "EAP-WPS: Pkt length in pkt(%d) differs from"
" supplied (%d)\n", ntohs(req->length), reqDataLen);
ret->ignore = TRUE;
return NULL;
}
notifyData.type = WPS_NOTIFY_TYPE_PROCESS_REQ;
notifyData.length = reqDataLen;
notifyData.u.process.state = data->state;
sendBuf = (u8 *) os_malloc(sizeof(WPS_NOTIFY_DATA) + reqDataLen);
if ( ! sendBuf)
{
wpa_printf(MSG_INFO, "EAP-WPS: Memory allocation "
"for the sendBuf failed\n");
ret->ignore = TRUE;
return NULL;
}
os_memcpy(sendBuf, ¬ifyData, sizeof(WPS_NOTIFY_DATA));
os_memcpy(sendBuf + sizeof(WPS_NOTIFY_DATA), reqData, reqDataLen);
sendBufLen = sizeof(WPS_NOTIFY_DATA) + reqDataLen;
to.sin_addr.s_addr = inet_addr(WPS_EAP_UDP_ADDR);
to.sin_family = AF_INET;
to.sin_port = host_to_be16(WPS_EAP_UDP_PORT);
if (udp_write(data->udpFdEap, (char *) sendBuf, sendBufLen, &to) <
sendBufLen)
{
wpa_printf(MSG_INFO, "EAP-WPS: Sending Eap message to "
"upper Layer failed\n");
ret->ignore = TRUE;
os_free(sendBuf);
return NULL;
}
os_free(sendBuf);
recvBytes = udp_read_timed(data->udpFdEap, (char *) data->recvBuf,
WPS_RECVBUF_SIZE, &from, 5);
if (recvBytes == -1)
{
wpa_printf(MSG_INFO, "EAP-WPS: Reading EAP message "
"from upper layer failed\n");
ret->ignore = TRUE;
return NULL;
}
recvNotify = (WPS_NOTIFY_DATA *) data->recvBuf;
if ( (recvNotify->type != WPS_NOTIFY_TYPE_PROCESS_RESULT) ||
// (recvNotify->length == 0) ||
(recvNotify->u.processResult.result != WPS_NOTIFY_RESULT_SUCCESS) )
{
wpa_printf(MSG_INFO, "EAP-WPS: Process Message failed "
"somewhere\n");
ret->ignore = TRUE;
return NULL;
}
resp = (u8 *) os_malloc(recvNotify->length);
if ( ! resp)
{
wpa_printf(MSG_INFO, "EAP-WPS: Memory allocation "
"for the resp failed\n");
ret->ignore = TRUE;
return NULL;
}
os_memcpy(resp, recvNotify + 1, recvNotify->length);
*respDataLen = recvNotify->length;
ret->ignore = FALSE;
ret->decision = DECISION_COND_SUCC;
ret->allowNotifications = FALSE;
/*check if we're done*/
if (recvNotify->u.processResult.done)
{
ret->methodState = METHOD_DONE;
}
else
{
wpa_printf(MSG_INFO, "Always setting it to METHOD_CONT\n");
ret->methodState = METHOD_CONT;
}
return resp;
}
static int eap_wsc_com_process(struct eap_sm *sm, struct eap_wsc_data *data,
u8 * respData, unsigned int respDataLen)
{
int recvBytes;
struct sockaddr_in from;
struct sockaddr_in to;
u8 * sendBuf;
u32 sendBufLen;
WSC_NOTIFY_DATA notifyData;
WSC_NOTIFY_DATA * recvNotify;
notifyData.type = WSC_NOTIFY_TYPE_PROCESS_RESP;
notifyData.length = respDataLen;
notifyData.u.process.state = data->state;
memcpy(notifyData.sta_mac_addr, sm->sta->addr, ETH_ALEN);
sendBuf = (u8 *) malloc(sizeof(WSC_NOTIFY_DATA) + respDataLen);
if ( ! sendBuf)
{
wpa_printf(MSG_INFO, "EAP-WSC: Memory allocation "
"for the sendBuf failed\n");
data->state = FAILURE;
return -1;
}
memcpy(sendBuf, ¬ifyData, sizeof(WSC_NOTIFY_DATA));
memcpy(sendBuf + sizeof(WSC_NOTIFY_DATA), respData, respDataLen);
sendBufLen = sizeof(WSC_NOTIFY_DATA) + respDataLen;
to.sin_addr.s_addr = inet_addr(WSC_EAP_UDP_ADDR);
to.sin_family = AF_INET;
to.sin_port = htons(WSC_EAP_UDP_PORT);
if (udp_write(data->udpFdEap, (char *) sendBuf,
sendBufLen, &to) < sendBufLen)
{
wpa_printf(MSG_INFO, "EAP-WSC: com Sending Eap message to "
"upper Layer failed\n");
data->state = FAILURE;
free(sendBuf);
return -1;
}
free(sendBuf);
recvBytes = udp_read_timed(data->udpFdEap, (char *) data->recvBuf,
WSC_RECVBUF_SIZE, &from, 15); /* Jerry timeout value change*/
if (recvBytes == -1)
{
wpa_printf(MSG_INFO, "EAP-WSC: com Reading EAP message "
"from upper layer failed\n");
data->state = FAILURE;
return -1;
}
recvNotify = (WSC_NOTIFY_DATA *) data->recvBuf;
/* printf("type = %d, length = %d, result = %d\n",
recvNotify->type,
recvNotify->length,
recvNotify->u.processResult.result);*/
if ( (recvNotify->type != WSC_NOTIFY_TYPE_PROCESS_RESULT) ||
(recvNotify->u.processResult.result != WSC_NOTIFY_RESULT_SUCCESS) )
{
wpa_printf(MSG_DEBUG, "EAP-WSC: Process Message failed "
"somewhere\n");
data->state = FAILURE;
return -1;
}
data->state = CONTINUE;
return 0;
}
static void *open_output( void *ptr )
{
obe_output_params_t *output_params = ptr;
obe_t *h = output_params->h;
struct ip_status status;
hnd_t ip_handle = NULL;
int num_muxed_data = 0;
uint8_t **muxed_data;
obe_udp_opts_t udp_opts;
struct sched_param param = {0};
param.sched_priority = 99;
pthread_setschedparam( pthread_self(), SCHED_FIFO, ¶m );
status.output_params = output_params;
status.ip_handle = &ip_handle;
pthread_cleanup_push( close_output, (void*)&status );
udp_populate_opts( &udp_opts, output_params->output_opts.target );
if( output_params->output_opts.output == OUTPUT_RTP )
{
if( rtp_open( &ip_handle, &udp_opts ) < 0 )
return NULL;
}
else
{
if( udp_open( &ip_handle, &udp_opts ) < 0 )
{
fprintf( stderr, "[udp] Could not create udp output" );
return NULL;
}
}
while( 1 )
{
pthread_mutex_lock( &h->output_queue.mutex );
while( !h->output_queue.size && !h->cancel_output_thread )
{
/* Often this cond_wait is not because of an underflow */
pthread_cond_wait( &h->output_queue.in_cv, &h->output_queue.mutex );
}
if( h->cancel_output_thread )
{
pthread_mutex_unlock( &h->output_queue.mutex );
break;
}
num_muxed_data = h->output_queue.size;
muxed_data = malloc( num_muxed_data * sizeof(*muxed_data) );
if( !muxed_data )
{
pthread_mutex_unlock( &h->output_queue.mutex );
syslog( LOG_ERR, "Malloc failed\n" );
return NULL;
}
memcpy( muxed_data, h->output_queue.queue, num_muxed_data * sizeof(*muxed_data) );
pthread_mutex_unlock( &h->output_queue.mutex );
// printf("\n START %i \n", num_muxed_data );
for( int i = 0; i < num_muxed_data; i++ )
{
if( output_params->output_opts.output == OUTPUT_RTP )
{
if( write_rtp_pkt( ip_handle, &muxed_data[i][7*sizeof(int64_t)], TS_PACKETS_SIZE, AV_RN64( muxed_data[i] ) ) < 0 )
syslog( LOG_ERR, "[rtp] Failed to write RTP packet\n" );
}
else
{
if( udp_write( ip_handle, &muxed_data[i][7*sizeof(int64_t)], TS_PACKETS_SIZE ) < 0 )
syslog( LOG_ERR, "[udp] Failed to write UDP packet\n" );
}
remove_from_queue( &h->output_queue );
free( muxed_data[i] );
}
free( muxed_data );
muxed_data = NULL;
}
pthread_cleanup_pop( 1 );
return NULL;
}
/**
* 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;
}
usb_size_t
usb_write (usb_t usb, const void *buffer, usb_size_t length)
{
return udp_write (usb->udp, buffer, length);
}