size_t frame_alloc_mult_contig(uintptr_t* frames, size_t num)
{
/* Iterate over all the frames */
for(uint32_t i = 0; i < 131072; ++i) {
if(frame_get(i * 0x1000) == 0) {
uint8_t enough = TRUE;
for(uint32_t j = 0; j < num; ++j) {
if(frame_get(i + j * 0x1000) == 1) {
enough = FALSE;
break;
}
}
if(enough == TRUE) {
for(uint32_t j = 0; j < num; ++j) {
frame_set(i + j * 0x1000, 1);
frames[j] = P2V(i + j * 0x1000);
}
return num;
}
}
}
return 0;
}
EXP_type Environment_Local_Get(NBR_type Offset_number)
{ EXP_type value_expression;
UNS_type offset;
offset = get_NBR(Offset_number);
value_expression = frame_get(Current_frame,
offset);
return value_expression; }
void send_status(uint8_t ua_type, uint8_t ua_status)
{
uint8_t rua_data[STATUS_SIZE] = { ua_type, ua_status };
uint8_t *pua_buf;
pua_buf = frame_get(rua_data, STATUS_SIZE);
Serial.write(pua_buf, FULL_SIZE + STATUS_SIZE);
free(pua_buf);
}
static void eth_rx(struct gmac_queue *queue)
{
struct eth_sam_dev_data *dev_data =
CONTAINER_OF(queue, struct eth_sam_dev_data, queue_list);
struct net_pkt *rx_frame;
/* More than one frame could have been received by GMAC, get all
* complete frames stored in the GMAC RX descriptor list.
*/
rx_frame = frame_get(queue);
while (rx_frame) {
SYS_LOG_DBG("ETH rx");
if (net_recv_data(dev_data->iface, rx_frame) < 0) {
net_pkt_unref(rx_frame);
}
rx_frame = frame_get(queue);
}
}
static inline int __page_map(paddr_t paddr, size_t pml4, size_t dirtbl, size_t dir, size_t table, int flags)
{
/* Sanity checking */
if (pml4 > 511 || dirtbl > 511 || dir > 511 || table > 511) {
return -EINVAL;
}
__page_t page = {.raw = paddr};
page.present = 1;
page.write = !!(flags & (VM_KW | VM_UW));
page.user = !!(flags & (VM_URWX));
/* Check if PDPT is present */
if (!LVL4[pml4].present) {
paddr_t paddr = frame_get();
pdpt_map(paddr, pml4, flags);
}
pages[GET_PHYS_ADDR(&LVL4[pml4])/PAGE_SIZE].refs++;
/* Check if PD is present */
if (!LVL3(pml4)[dirtbl].present) {
paddr_t paddr = frame_get();
pd_map(paddr, pml4, dirtbl, flags);
}
pages[GET_PHYS_ADDR(&LVL3(pml4)[dirtbl])/PAGE_SIZE].refs++;
/* Check if table is present */
if (!LVL2(pml4, dirtbl)[dir].present) {
paddr_t paddr = frame_get();
table_map(paddr, pml4, dirtbl, dir, flags);
}
pages[GET_PHYS_ADDR(&LVL2(pml4, dirtbl)[dir])/PAGE_SIZE].refs++;
LVL1(pml4, dirtbl, dir)[table] = page;
return 0;
}
int main(void)
{
FrameColor buf[4] = {FRAME_NULL}; // most recent first
bool f_wait = true; // not yet receiving data
char out = 0;
uint idx = 0;
setbuf(stdout, NULL);
while (true) {
buf[3] = buf[2];
buf[2] = buf[1];
buf[1] = buf[0];
buf[0] = frame_get();
// start
if (f_wait &&
buf[3] == WAIT &&
buf[2] == WAIT &&
buf[1] == WAIT &&
buf[0] != WAIT) {
f_wait = false;
}
// stop
if (!f_wait &&
buf[3] == WAIT &&
buf[2] == WAIT &&
buf[1] == WAIT &&
buf[0] == WAIT) {
assert(out == 0 && idx == 0 && "last byte not flushed");
break;
}
if (!f_wait && buf[0] != buf[1] && (buf[0] == ZERO || buf[0] == ONE)) {
if (buf[0] == ONE) {
out |= (0b10000000 >> idx); // flip initial 0 to 1
}
if (idx == 7) {
putchar(out);
out = 0;
idx = 0;
}
else {
idx++;
}
}
}
EXP_type Environment_Global_Get(NBR_type Scope_number,
NBR_type Offset_number)
{ EXP_type value_expression;
FRM_type frame;
UNS_type offset,
scope;
scope = get_NBR(Scope_number);
offset = get_NBR(Offset_number);
frame = get_frame(Current_environment,
scope);
value_expression = frame_get(frame,
offset);
return value_expression; }
void manager_interpret_message( Display *conn, XClientMessageEvent event )
{
frame_t *frame = frame_get( event.window );
if (event.message_type == EA_NET_CLOSE_WINDOW ) {
printf( "\t_NET_CLOSE_WINDOW\n" );
/* Close the application */
Window window = frame->child->id;
Atom *protocols;
int num, i;
XUnmapWindow( conn, window );
XGetWMProtocols( conn, window, &protocols, &num );
for (i = 0; i < num; i++ ) {
if ( protocols[i] == XA_WM_DELETE_WINDOW ) {
XEvent event_send;
event_send.type = ClientMessage;
event_send.xclient.type = ClientMessage;
event_send.xclient.message_type = XA_WM_PROTOCOLS;
event_send.xclient.format = 32;
event_send.xclient.data.l[0] = XA_WM_DELETE_WINDOW;
event_send.xclient.data.l[1] = event.data.l[0];
XSendEvent( conn, window, False, 0, &event_send );
window = None;
}
}
if ( window != None )
XKillClient( conn, window );
} else if ( event.message_type == EA_NET_MOVERESIZE_WINDOW ) {
/* Change the window position on client request */
if ( event.data.l[0] & (MR_FLAG_X | MR_FLAG_Y) )
frame_move( conn, frame, event.data.l[1], event.data.l[2] );
} else if ( event.message_type == EA_NET_ACTIVE_WINDOW ) {
/* Change the window focus on client request */
frame_focus( conn, frame );
}
}
int8_t frame_alloc_aligned(uintptr_t* frame, uint32_t alignment)
{
uint32_t mask = alignment - 1;
/* Iterate over all the frames */
for(uint32_t i = 0; i < 4096; ++i) {
if(frames_bitmap[i] ^ 0xFFFFFFFF) {
/* There's a free frame here */
/* For each bit in this part of the bitmap */
for(uintptr_t f = i * 32 * 0x1000; f < (i+1) * 32 * 0x1000; f += 0x1000) {
/* If the frame is free */
if(0 == frame_get(f) && (f & mask) == 0) {
frame_set(f, 1);
*frame = P2V(f);
return 0;
}
}
}
}
return -ENOMEM;
}
/**
* \brief Master driver main state machine
* Executed only in the context of PendSV_Handler()
* For UML graph, please refer to SDK documentation
*/
static void ser_phy_switch_state(ser_phy_event_source_t evt_src)
{
uint32_t err_code = NRF_SUCCESS;
static bool m_wait_for_ready_flag = false; //local scheduling flag to defer RDY events
switch (m_spi_master_state)
{
case SER_PHY_STATE_IDLE:
if (evt_src == SER_PHY_EVT_GPIO_REQ)
{
m_wait_for_ready_flag = false;
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_ZERO_HEADER;
err_code = header_send(0);
}
else
{
m_spi_master_state = SER_PHY_STATE_RX_WAIT_FOR_RDY;
}
}
else if (evt_src == SER_PHY_EVT_TX_API_CALL)
{
spi_master_raw_assert(mp_tx_buffer != NULL); //api event with tx_buffer == NULL has no sense
m_wait_for_ready_flag = false;
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_HEADER;
err_code = header_send(m_tx_buf_len);
}
else
{
m_spi_master_state = SER_PHY_STATE_TX_WAIT_FOR_RDY;
}
}
break;
case SER_PHY_STATE_TX_WAIT_FOR_RDY:
if (evt_src == SER_PHY_EVT_GPIO_RDY)
{
m_spi_master_state = SER_PHY_STATE_TX_HEADER;
err_code = header_send(m_tx_buf_len);
}
break;
case SER_PHY_STATE_RX_WAIT_FOR_RDY:
if (evt_src == SER_PHY_EVT_GPIO_RDY)
{
m_spi_master_state = SER_PHY_STATE_TX_ZERO_HEADER;
err_code = header_send(0);
}
break;
case SER_PHY_STATE_TX_HEADER:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
m_tx_packet_length = m_tx_buf_len;
m_accumulated_tx_packet_length = 0;
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_PAYLOAD;
err_code = frame_send();
}
else
{
m_wait_for_ready_flag = true;
}
}
else if ((evt_src == SER_PHY_EVT_GPIO_RDY) && m_wait_for_ready_flag)
{
m_wait_for_ready_flag = false;
m_spi_master_state = SER_PHY_STATE_TX_PAYLOAD;
err_code = frame_send();
}
break;
case SER_PHY_STATE_TX_PAYLOAD:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
if (m_accumulated_tx_packet_length < m_tx_packet_length)
{
if (m_slave_ready_flag)
{
err_code = frame_send();
}
else
{
m_wait_for_ready_flag = true;
}
}
else
{
spi_master_raw_assert(m_accumulated_tx_packet_length == m_tx_packet_length);
buffer_release(&mp_tx_buffer, &m_tx_buf_len);
callback_packet_sent();
if ( m_slave_request_flag)
{
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_ZERO_HEADER;
err_code = header_send(0);
}
else
{
m_spi_master_state = SER_PHY_STATE_RX_WAIT_FOR_RDY;
}
}
else
{
m_spi_master_state = SER_PHY_STATE_IDLE; //m_Tx_buffer is NULL - have to wait for API event
}
}
}
else if ((evt_src == SER_PHY_EVT_GPIO_RDY) && m_wait_for_ready_flag )
{
m_wait_for_ready_flag = false;
err_code = frame_send();
}
break;
case SER_PHY_STATE_TX_ZERO_HEADER:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_RX_HEADER;
err_code = header_get();
}
else
{
m_wait_for_ready_flag = true;
}
}
else if ( (evt_src == SER_PHY_EVT_GPIO_RDY) && m_wait_for_ready_flag)
{
m_wait_for_ready_flag = false;
m_spi_master_state = SER_PHY_STATE_RX_HEADER;
err_code = header_get();
}
break;
case SER_PHY_STATE_RX_HEADER:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
m_spi_master_state = SER_PHY_STATE_MEMORY_REQUEST;
m_rx_buf_len = uint16_decode(m_header_buffer);
m_rx_packet_length = m_rx_buf_len;
callback_mem_request();
}
break;
case SER_PHY_STATE_MEMORY_REQUEST:
if (evt_src == SER_PHY_EVT_RX_API_CALL)
{
m_accumulated_rx_packet_length = 0;
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_RX_PAYLOAD;
err_code = frame_get();
}
else
{
m_wait_for_ready_flag = true;
}
}
else if ((evt_src == SER_PHY_EVT_GPIO_RDY) && m_wait_for_ready_flag)
{
m_wait_for_ready_flag = false;
m_spi_master_state = SER_PHY_STATE_RX_PAYLOAD;
err_code = frame_get();
}
break;
case SER_PHY_STATE_RX_PAYLOAD:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
m_accumulated_rx_packet_length += m_current_rx_packet_length;
if (m_accumulated_rx_packet_length < m_rx_packet_length)
{
if (m_slave_ready_flag)
{
err_code = frame_get();
}
else
{
m_wait_for_ready_flag = true;
}
}
else
{
spi_master_raw_assert(m_accumulated_rx_packet_length == m_rx_packet_length);
if (mp_rx_buffer == NULL)
{
callback_packet_dropped();
}
else
{
callback_packet_received();
}
buffer_release(&mp_rx_buffer, &m_rx_buf_len);
if (mp_tx_buffer != NULL) //mp_tx_buffer !=NULL, this means that API_EVT was scheduled
{
if (m_slave_ready_flag )
{
err_code = header_send(m_tx_buf_len);
m_spi_master_state = SER_PHY_STATE_TX_HEADER;
}
else
{
m_spi_master_state = SER_PHY_STATE_TX_WAIT_FOR_RDY;
}
}
else if (m_slave_request_flag)
{
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_ZERO_HEADER;
err_code = header_send(0);
}
else
{
m_spi_master_state = SER_PHY_STATE_RX_WAIT_FOR_RDY;
}
}
else
{
m_spi_master_state = SER_PHY_STATE_IDLE;
}
}
}
else if ( evt_src == SER_PHY_EVT_GPIO_RDY && m_wait_for_ready_flag)
{
m_wait_for_ready_flag = false;
err_code = frame_get();
}
break;
default:
break;
}
if (err_code != NRF_SUCCESS)
{
(void)err_code;
}
}
int main(int argc, char *argv[])
{
int bcfd = -1;
char bcdev_name[] = "/dev/bccatX";
BCIO_package ioctl_var;
bc_buf_params_t buf_param;
bc_buf_ptr_t buf_pa;
unsigned long buf_paddr[MAX_BUFFERS];
char *buf_vaddr[MAX_BUFFERS] = { MAP_FAILED };
char *frame = NULL;
int buf_size = 0;
int c, idx, ret = -1;
char opts[] = "c:pw:t:b:h";
int ii;
int frame_w, frame_h;
int min_w = 0, min_h = 0;;
int cp_offset = 0;
struct timeval tvp, tv, tv0 = {0,0};
unsigned long tdiff = 0;
unsigned long fcount = 0;
for (;;) {
c = getopt_long(argc, argv, opts, (void *)NULL, &idx);
if (-1 == c)
break;
switch (c) {
case 0:
break;
case 'b':
bcdev_id = atoi(optarg) % 10;
break;
case 'c':
cap_dev = optarg;
printf("INFO: capture device is %s\n", cap_dev);
break;
case 'p':
profiling = TRUE;
printf("INFO: profiling enabled\n");
break;
case 'w':
min_w = atoi(optarg);
break;
case 't':
min_h = atoi(optarg);
break;
default:
usage(argv[0]);
return 0;
}
}
signal(SIGINT, signalHandler);
if (frame_init(&buf_param))
return -1;
bcdev_name[strlen(bcdev_name)-1] = '0' + bcdev_id;
if ((bcfd = open(bcdev_name, O_RDWR|O_NDELAY)) == -1) {
printf("ERROR: open %s failed\n", bcdev_name);
goto err_ret;
}
frame_w = buf_param.width;
frame_h = buf_param.height;
if (min_w > 0 && !(min_w % 8))
buf_param.width = min_w;
if (min_h > 0)
buf_param.height = min_h;
if (ioctl(bcfd, BCIOREQ_BUFFERS, &buf_param) != 0) {
printf("ERROR: BCIOREQ_BUFFERS failed\n");
goto err_ret;
}
if (ioctl(bcfd, BCIOGET_BUFFERCOUNT, &ioctl_var) != 0) {
goto err_ret;
}
if (ioctl_var.output == 0) {
printf("ERROR: no texture buffer available\n");
goto err_ret;
}
/* for BC_MEMORY_USERPTR, BCIOSET_BUFFERPHYADDR must be called
* before init IMG_extensions in initTexExt()*/
if (buf_param.type == BC_MEMORY_USERPTR) {
for (idx = 0; idx < buf_param.count; idx++) {
while ((frame = frame_get(&buf_pa)) == NULL) { }
if (frame == (char *)-1)
goto err_ret;
if (ioctl(bcfd, BCIOSET_BUFFERPHYADDR, &buf_pa) != 0) {
frame_restore(frame);
printf("ERROR: BCIOSET_BUFFERADDR[%d]: failed (0x%lx)\n",
buf_pa.index, buf_pa.pa);
goto err_ret;
}
if (frame_restore(frame))
goto err_ret;
}
}
if (initEGL(buf_param.count)) {
printf("ERROR: init EGL failed\n");
goto err_ret;
}
if ((ret = initTexExt(bcdev_id, &buf_info)) < 0) {
printf("ERROR: initTexExt() failed [%d]\n", ret);
goto err_ret;
}
if (buf_info.n > MAX_BUFFERS) {
printf("ERROR: number of texture buffer exceeds the limit\n");
goto err_ret;
}
/*FIXME calc stride instead of 2*/
buf_size = buf_info.w * buf_info.h * 2;
min_w = buf_info.w < frame_w ? buf_info.w : frame_w;
min_h = buf_info.h < frame_h ? buf_info.h : frame_h;
if (buf_info.h > frame_h)
cp_offset = (buf_info.h - frame_h) * buf_info.w;
if (buf_info.w > frame_w)
cp_offset += buf_info.w - frame_w;
if (buf_param.type == BC_MEMORY_MMAP) {
for (idx = 0; idx < buf_info.n; idx++) {
ioctl_var.input = idx;
if (ioctl(bcfd, BCIOGET_BUFFERPHYADDR, &ioctl_var) != 0) {
printf("ERROR: BCIOGET_BUFFERADDR failed\n");
goto err_ret;
}
buf_paddr[idx] = ioctl_var.output;
buf_vaddr[idx] = (char *)mmap(NULL, buf_size,
PROT_READ | PROT_WRITE, MAP_SHARED,
bcfd, buf_paddr[idx]);
if (buf_vaddr[idx] == MAP_FAILED) {
printf("ERROR: mmap failed\n");
goto err_ret;
}
}
}
ret = 0;
idx = 0;
if (profiling == TRUE) {
gettimeofday(&tvp, NULL);
tv0 = tvp;
}
while (!gQuit) {
#ifdef USE_SOLID_PATTERN
usleep(1000 * 1000);
#endif
frame = frame_get(&buf_pa);
if (frame == (char *) -1)
break;
if (frame) {
if (buf_param.type == BC_MEMORY_MMAP) {
for (ii = 0; ii < min_h; ii++)
/*FIXME calc stride instead of 2*/
memcpy(buf_vaddr[idx] + buf_info.w * 2 * ii + cp_offset,
frame + frame_w * 2 * ii, min_w * 2);
}
else /*buf_param.type == BC_MEMORY_USERPTR*/
idx = buf_pa.index;
}
drawCube(idx);
if (frame_restore(frame))
break;
#ifdef X11
if (doX11Events())
gQuit = TRUE;
#endif
idx = (idx + 1) % buf_info.n;
if (profiling == FALSE)
continue;
gettimeofday(&tv, NULL);
fcount++;
if (!(fcount % 60)) {
tdiff = (unsigned long)(tv.tv_sec*1000 + tv.tv_usec/1000 -
tvp.tv_sec*1000 - tvp.tv_usec/1000);
if (tdiff < 1800) /*print fps every 2 sec*/
continue;
fprintf(stderr, "\rAvg FPS: %ld",
fcount / (tv.tv_sec - tv0.tv_sec));
tvp = tv;
}
}
printf("\n");
err_ret:
if (buf_param.type == BC_MEMORY_MMAP) {
for (idx = 0; idx < buf_info.n; idx++) {
if (buf_vaddr[idx] != MAP_FAILED)
munmap(buf_vaddr[idx], buf_size);
}
}
if (bcfd > -1)
close(bcfd);
deInitEGL(buf_info.n);
#ifdef X11
deInitX11();
#endif
frame_cleanup();
printf("done\n");
return ret;
}
EXP_type Environment_Frame_Get(FRM_type Frame,
UNS_type Offset)
{ EXP_type value_expression;
value_expression = frame_get(Frame,
Offset);
return value_expression; }
void loop()
{
// if programming failed, don't try to do anything
if (!b_dmp_ready)
return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && uh_fifo_count < uh_packet_size)
{
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
ua_mpu_interrupt_status = mpu.getIntStatus();
// get current FIFO count
uh_fifo_count = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((ua_mpu_interrupt_status & 0x10) || uh_fifo_count == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
send_status(FIFO_OVERFLOW, ua_mpu_interrupt_status);
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (ua_mpu_interrupt_status & 0x02)
{
uint8_t ua_idx, ua_nb = 0;
uint8_t ua_data_len = 1;
uint8_t ua_types = 0;
uint8_t *pua_buf, *pua_data_buf, *pua_data_start;
// wait for correct available data length, should be a VERY short wait
while (uh_fifo_count < uh_packet_size)
{
uh_fifo_count = mpu.getFIFOCount();
}
// read a packet from FIFO
mpu.getFIFOBytes(ua_fifo_buffer, uh_packet_size);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
uh_fifo_count -= uh_packet_size;
#ifdef OUTPUT_BUFFER
ua_data_len += BUFFER_SIZE;
ua_types |= OUTPUT_BUFFER;
#else
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&s_quaternion, ua_fifo_buffer);
#endif
#ifdef OUTPUT_QUATERNION
ua_data_len += 4 * sizeof(float);
ua_types |= OUTPUT_QUATERNION;
#endif
#ifdef OUTPUT_EULER
mpu.dmpGetEuler(rf_euler, &s_quaternion);
ua_data_len += 3 * sizeof(float);
ua_types |= OUTPUT_EULER;
#endif
#if defined(OUTPUT_YAWPITCHROLL) || defined(OUTPUT_REALACCEL) || defined(OUTPUT_WORLDACCEL)
mpu.dmpGetGravity(&s_gravity, &s_quaternion);
#endif
#ifdef OUTPUT_YAWPITCHROLL
mpu.dmpGetYawPitchRoll(rf_ypr, &s_quaternion, &s_gravity);
ua_data_len += 3 * sizeof(float);
ua_types |= OUTPUT_YAWPITCHROLL;
#endif
#if defined(OUTPUT_REALACCEL) || defined(OUTPUT_WORLDACCEL)
// display real acceleration, adjusted to remove gravity
mpu.dmpGetAccel(&s_acceleration, ua_fifo_buffer);
mpu.dmpGetLinearAccel(&s_acceleration_real, &s_acceleration, &s_gravity);
#endif
#ifdef OUTPUT_REALACCEL
ua_data_len += 3 * sizeof(float);
ua_types |= OUTPUT_REALACCEL;
#endif
#ifdef OUTPUT_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity
mpu.dmpGetLinearAccelInWorld(&s_acceleration_world, &s_acceleration_real, &s_quaternion);
ua_data_len += 3 * sizeof(float);
ua_types |= OUTPUT_WORLDACCEL;
#endif
// allocate the buffe to store the values
pua_data_start = (uint8_t*)malloc(ua_data_len);
// Store the start of the buffer
pua_data_buf = pua_data_start;
// Setup type of data expected
*pua_data_buf++ = ua_types;
#ifdef OUTPUT_BUFFER
*pua_data_buf++ = ua_fifo_buffer[0];
*pua_data_buf++ = ua_fifo_buffer[1];
*pua_data_buf++ = ua_fifo_buffer[4];
*pua_data_buf++ = ua_fifo_buffer[5];
*pua_data_buf++ = ua_fifo_buffer[8];
*pua_data_buf++ = ua_fifo_buffer[9];
*pua_data_buf++ = ua_fifo_buffer[12];
*pua_data_buf++ = ua_fifo_buffer[13];
#endif
#ifdef OUTPUT_QUATERNION
pua_data_buf += store_float32(pua_data_buf, s_quaternion.w);
pua_data_buf += store_float32(pua_data_buf, s_quaternion.x);
pua_data_buf += store_float32(pua_data_buf, s_quaternion.y);
pua_data_buf += store_float32(pua_data_buf, s_quaternion.z);
#endif
#ifdef OUTPUT_EULER
pua_data_buf += store_float32(pua_data_buf, rf_euler[0] * 180/M_PI);
pua_data_buf += store_float32(pua_data_buf, rf_euler[1] * 180/M_PI);
pua_data_buf += store_float32(pua_data_buf, rf_euler[2] * 180/M_PI);
#endif
#ifdef OUTPUT_YAWPITCHROLL
pua_data_buf += store_float32(pua_data_buf, rf_ypr[0] * 180/M_PI);
pua_data_buf += store_float32(pua_data_buf, rf_ypr[1] * 180/M_PI);
pua_data_buf += store_float32(pua_data_buf, rf_ypr[2] * 180/M_PI);
#endif
#ifdef OUTPUT_REALACCEL
pua_data_buf += store_float32(pua_data_buf, s_acceleration_real.x);
pua_data_buf += store_float32(pua_data_buf, s_acceleration_real.y);
pua_data_buf += store_float32(pua_data_buf, s_acceleration_real.z);
#endif
#ifdef OUTPUT_WORLDACCEL
pua_data_buf += store_float32(pua_data_buf, s_acceleration_world.x);
pua_data_buf += store_float32(pua_data_buf, s_acceleration_world.y);
pua_data_buf += store_float32(pua_data_buf, s_acceleration_world.z);
#endif
// send the result
pua_buf = frame_get((uint8_t*)pua_data_start, ua_data_len);
if (pua_buf != NULL)
{
Serial.write(pua_buf, FULL_SIZE + ua_data_len);
free(pua_buf);
}
free(pua_data_start);
// blink LED to indicate activity
b_blink_state = !b_blink_state;
digitalWrite(LED_PIN, b_blink_state);
}
}
EXP_type Environment_Global_Frame_Get(UNS_type Offset)
{ return frame_get(Global_frame,
Offset); }
/**
* \brief Slave driver main state machine
* For UML graph, please refer to SDK documentation
*/
static void spi_slave_event_handle(spi_slave_evt_t event)
{
static uint32_t err_code = NRF_SUCCESS;
static uint16_t packetLength;
switch (m_trans_state)
{
case SPI_RAW_STATE_SETUP_HEADER:
m_trans_state = SPI_RAW_STATE_RX_HEADER;
err_code = header_get();
break;
case SPI_RAW_STATE_RX_HEADER:
if (event.evt_type == SPI_SLAVE_BUFFERS_SET_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_BUFFERS_SET(0);
set_ready_line();
}
if (event.evt_type == SPI_SLAVE_XFER_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_RX_XFER_DONE(event.rx_amount);
spi_slave_raw_assert(event.rx_amount == SER_PHY_HEADER_SIZE);
packetLength = uint16_decode(m_header_rx_buffer);
if (packetLength != 0 )
{
m_trans_state = SPI_RAW_STATE_MEM_REQUESTED;
m_buffer_reqested_flag = true;
m_rx_packet_length = packetLength;
callback_memory_request(packetLength);
}
else
{
if (m_p_tx_buffer)
{
clear_request_line();
m_trans_state = SPI_RAW_STATE_TX_HEADER;
err_code = header_send(m_tx_packet_length);
}
else
{
//there is nothing to send - zero response facilitates pooling - but perhaps, it should be assert
err_code = header_send(0);
}
}
}
break;
case SPI_RAW_STATE_MEM_REQUESTED:
if (event.evt_type == SPI_SLAVE_EVT_TYPE_MAX) //This is API dummy event
{
m_buffer_reqested_flag = false;
m_trans_state = SPI_RAW_STATE_RX_PAYLOAD;
m_accumulated_rx_packet_length = 0;
err_code = frame_get();
}
break;
case SPI_RAW_STATE_RX_PAYLOAD:
if (event.evt_type == SPI_SLAVE_BUFFERS_SET_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_BUFFERS_SET(0);
set_ready_line();
}
if (event.evt_type == SPI_SLAVE_XFER_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_RX_XFER_DONE(event.rx_amount);
spi_slave_raw_assert(event.rx_amount == m_current_rx_frame_length);
m_accumulated_rx_packet_length += m_current_rx_frame_length;
if (m_accumulated_rx_packet_length < m_rx_packet_length )
{
err_code = frame_get();
}
else
{
spi_slave_raw_assert(m_accumulated_rx_packet_length == m_rx_packet_length);
m_trans_state = SPI_RAW_STATE_RX_HEADER;
err_code = header_get();
if (!m_trash_payload_flag)
{
callback_packet_received(m_p_rx_buffer, m_accumulated_rx_packet_length);
}
else
{
callback_packet_dropped();
}
}
}
break;
case SPI_RAW_STATE_TX_HEADER:
if (event.evt_type == SPI_SLAVE_BUFFERS_SET_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_BUFFERS_SET(0);
set_ready_line();
}
if (event.evt_type == SPI_SLAVE_XFER_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_TX_XFER_DONE(event.tx_amount);
spi_slave_raw_assert(event.tx_amount == SER_PHY_HEADER_SIZE + 1);
m_trans_state = SPI_RAW_STATE_TX_PAYLOAD;
m_accumulated_tx_packet_length = 0;
err_code = frame_send();
}
break;
case SPI_RAW_STATE_TX_PAYLOAD:
if (event.evt_type == SPI_SLAVE_BUFFERS_SET_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_BUFFERS_SET(0);
set_ready_line();
}
if (event.evt_type == SPI_SLAVE_XFER_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_TX_XFER_DONE(event.tx_amount);
spi_slave_raw_assert(event.tx_amount == m_current_tx_frame_length + 1);
m_accumulated_tx_packet_length += m_current_tx_frame_length;
if ( m_accumulated_tx_packet_length < m_tx_packet_length )
{
err_code = frame_send();
}
else
{
spi_slave_raw_assert(m_accumulated_tx_packet_length == m_tx_packet_length);
//clear pointer before callback
m_p_tx_buffer = NULL;
callback_packet_transmitted();
//spi slave TX transfer is possible only when RX is ready, so return to waiting for a header
m_trans_state = SPI_RAW_STATE_RX_HEADER;
err_code = header_get();
}
}
break;
default:
err_code = NRF_ERROR_INVALID_STATE;
break;
}
APP_ERROR_CHECK(err_code);
}
int page_map(void *ptr, void *to_ptr, uint16_t tab_prot, uint16_t pg_prot)
{
uintptr_t p, toptr;
//frame_t frame;
uint64_t pml4e, pdpe, pde, pte;
page_t *pg_pml4, *pg_pdp, *pg_pd, *pg_pt;
p = (uintptr_t)ptr; toptr = (uintptr_t)to_ptr;
p &= ~0xFFF;
toptr = toptr & 0xFFF ? (toptr & ~0xFFF) + 0x1000 : toptr;
pg_pml4 = PAGE_PML4_TAB_ADDR(KERNEL_PML4E);
pte = PAGE_ADDR_PTE(p);
pml4e = pdpe = pde = -1;
do
{
if (!PAGE_ADDR_CHECK_PDE(p, pde))
{
if (!PAGE_ADDR_CHECK_PDPE(p, pdpe))
{
if (!PAGE_ADDR_CHECK_PML4E(p, pml4e))
{
pml4e = PAGE_ADDR_PML4E(ptr);
pg_pdp = PAGE_PDP_TAB_ADDR(KERNEL_PML4E, pml4e);
if (!(pg_pml4[pml4e] & 1))
{
//if ((frame = frame_get()) == NOFRAMES) return 0;
pg_pml4[pml4e] = (frame_get() << 0xC) | 1;
memset(pg_pdp, 0, sizeof(page_t) << 9);
}
}
pdpe = PAGE_ADDR_PDPE(p);
pg_pd = PAGE_PD_TAB_ADDR(KERNEL_PML4E, pml4e, pdpe);
if (!(pg_pdp[pdpe] & 1))
{
//if ((frame = frame_get()) == NOFRAMES) return 0;
pg_pdp[pdpe] = (frame_get() << 0xC) | 1;
memset(pg_pd, 0, sizeof(page_t) << 9);
}
}
pde = PAGE_ADDR_PDE(p);
pg_pt = PAGE_PT_TAB_ADDR(KERNEL_PML4E, pml4e, pdpe, pde);
if (!(pg_pd[pde] & 1))
{
//if ((frame = frame_get()) == NOFRAMES) return 0;
pg_pd[pde] = (frame_get() << 0xC) | tab_prot;
memset(pg_pt, 0, sizeof(page_t) << 9);
} else
pg_pd[pde] = pg_pd[pde] | tab_prot;
}
if (!(pg_pt[pte] & 1))
{
//if ((frame = frame_get()) == NOFRAMES) return 0;
pg_pt[pte] = (frame_get() << 0xC) | pg_prot;
if (pg_pt[pte] > 0xFFFFFFFF) panic("Falhou!");
}
pg_pt[pte] |= 3;
p += PAGE_PT_SIZE;
pte = PAGE_ADDR_PTE(p);
} while (p < toptr);
return 1;
}
VALUE
rb_debug_inspector_frame_iseq_get(const rb_debug_inspector_t *dc, long index)
{
VALUE frame = frame_get(dc, index);
return rb_ary_entry(frame, CALLER_BINDING_ISEQ);
}
int main(int argc, char *argv[])
{
int idx, tus, fsize;
char capdev_str[128];
unsigned long ss;
struct timeval ts, te;
if(argc > 1) {
// printf(" Usage : %s [file_path]\n", argv[0]);
strcpy(capdev_str, argv[1]);
} else {
strcpy(capdev_str, dev_cap);
}
signal(SIGQUIT, exit_signal);
signal(SIGINT, exit_signal);
signal(SIGPIPE, SIG_IGN);
if(display_init() < 0) {
printf("display init error\n");
return 0;
}
if(dec_init(CAP_WIDTH, CAP_HEIGHT, VPU_V_MJPG) < 0) {
printf("decodec init error\n");
goto err;
}
if ((fd_cap = open(capdev_str, O_RDWR)) < 0) {
printf("Unable to open [%s]\n", capdev_str);
goto err0;
}
if (setup_cap() < 0) {
printf("Unable to setup capture\n");
goto err1;
}
printf("capture device setup done\n");
if (map_buffers() < 0) {
printf("Unable to map I/O memory\n");
goto err1;
}
printf("buffers mapping done\n");
if (stream_start() < 0) {
printf("Unable to start stream\n");
goto err2;
}
printf("Stream starting... with fps=%d\n", FPS_VIDEO);
gettimeofday(&ts, 0);
do {
idx = frame_get(&fsize);
frame_render(idx, fsize);
frame_put(idx);
gettimeofday(&te, 0);
tus = (te.tv_sec - ts.tv_sec) * 1000000 + (te.tv_usec - ts.tv_usec);
if(tus <= FRAME_DURATION) {
if(!exitflag) usleep(FRAME_DURATION - tus);
} else {
// printf("rander a frame with %.3fms\n", tus / 1000.0);
}
printf("Rander a frame sz = %d, takes %.3fms\n", fsize, tus / 1000.0);
gettimeofday(&ts, 0);
} while(!exitflag);
printf("Stopping stream...\n");
stream_stop();
err2:
umap_buffers();
err1:
close(fd_cap);
err0:
dec_exit();
err:
display_exit();
return 0;
}
int main(int argc, char *argv[])
{
int idx, tus;
unsigned long ss;
struct timeval ts, te;
char cappath[128];
if(argc >= 2) {
strcpy(cappath, argv[1]);
} else {
strcpy(cappath, dev_cap);
}
signal(SIGQUIT, exit_signal);
signal(SIGINT, exit_signal);
signal(SIGPIPE, SIG_IGN);
if(enc_init(CAP_WIDTH, CAP_HEIGHT, FPS_VIDEO, VPU_V_AVC) < 0) {
printf("enc_init error\n");
return -1;
}
if ((fd_cap = open(cappath, O_RDWR)) < 0) {
printf("Unable to open [%s]\n", cappath);
goto err;
}
if (setup_cap() < 0) {
printf("Unable to setup capture\n");
goto err0;
}
printf("capture device setup done\n");
if(display_init(dev_out) < 0) {
printf("display_init err\n");
goto err0;
}
if (map_buffers() < 0) {
printf("Unable to map v4l2 memory\n");
goto err1;
}
printf("buffers mapping done\n");
if (stream_start() < 0) {
printf("Unable to start stream\n");
goto err2;
}
printf("Stream starting... with fps=%d\n", FPS_VIDEO);
static int xxx = 0;
gettimeofday(&ts, 0);
fd_enc = open("myenc.h264", O_RDWR | O_CREAT | O_TRUNC, 0666);
do {
idx = frame_get();
frame_render(idx);
frame_put(idx);
gettimeofday(&te, 0);
tus = (te.tv_sec - ts.tv_sec) * 1000000 + (te.tv_usec - ts.tv_usec);
if(tus <= FRAME_DURATION) {
if(!exitflag) usleep(FRAME_DURATION - tus);
} else {
printf("rander a frame with %.3fms\n", tus / 1000.0);
}
// printf("rander a frame with %.3fms\n", tus / 1000.0);
gettimeofday(&ts, 0);
} while(!exitflag);
close(fd_enc);
printf("Stopping stream...\n");
stream_stop();
err2:
umap_buffers();
err1:
display_exit();
err0:
close(fd_cap);
err:
enc_exit();
return 0;
}
