Beispiel #1
0
static void centre_and_rescale(float *verts, int numvertices)
{
   float cx=0.0f, cy=0.0f, cz=0.0f, scale=0.0f;
   float minx=0.0f, miny=0.0f, minz=0.0f;
   float maxx=0.0f, maxy=0.0f, maxz=0.0f;
   int i;
   float *v = verts;
   minx = maxx = verts[0];
   miny = maxy = verts[1];
   minz = maxz = verts[2];
   for (i=0; i<numvertices; i++) {
      float x = *v++;
      float y = *v++;
      float z = *v++;
      minx = vcos_min(minx, x);
      miny = vcos_min(miny, y);
      minz = vcos_min(minz, z);
      maxx = vcos_max(maxx, x);
      maxy = vcos_max(maxy, y);
      maxz = vcos_max(maxz, z);
      cx += x;
      cy += y;
      cz += z;
   }
   cx /= (float)numvertices;
   cy /= (float)numvertices;
   cz /= (float)numvertices;
   scale = 3.0f / (maxx-minx + maxy-miny + maxz-minz);
   v = verts;
   for (i=0; i<numvertices; i++) {
      *v = (*v-cx) * scale; v++;
      *v = (*v-cy) * scale; v++;
      *v = (*v-cz) * scale; v++;
   }
}
Beispiel #2
0
static void print_mmal_event_log(VC_MEM_ACCESS_HANDLE_T vc, MMAL_DBG_LOG_T *log)
{
   uint32_t i;
   uint32_t n = vcos_min(log->num_entries, log->index);
   uint32_t mask = log->num_entries-1;
   uint32_t start = log->index < log->num_entries ?
      0 : log->index & mask;
   uint32_t last_t = 0;
   (void)vc;

   for (i=0; i<n; i++)
   {
      MMAL_DBG_ENTRY_T *e = &log->entries[(start+i) & mask];
      char buf[256];
      int j;
      uint32_t t = e->time;
      printf("[%08u]: ", t-last_t);
      last_t = t;
      for (j=0; j<n_handlers; j++)
      {
         if (handlers[j].type == e->event_type)
         {
            handlers[j].handler(e, buf, sizeof(buf));
            printf("%s\n", buf);
            break;
         }
      }
      if (j == n_handlers )
         printf("Unknown event type %d\n", e->event_type);
   }
}
Beispiel #3
0
/******************************************************************************
NAME
   vc_gencmd_read_response

SYNOPSIS
   int vc_gencmd_read_response

FUNCTION
   Block until something comes back

RETURNS
   Error code from dequeue message
******************************************************************************/
int vc_gencmd_read_response (char *response, int maxlen) {
   int i = 0;
   int success = -1;
   int ret_code = 0;
   int32_t sem_ok = 0;

   if(lock_obtain() == 0)
   {
      //Note this will ALWAYS reset response buffer and overwrite any partially read responses
      use_gencmd_service();
      do {
         //TODO : we need to deal with messages coming through on more than one connections properly
         //At the moment it will always try to read the first connection if there is something there
         for(i = 0; i < gencmd_client.num_connections; i++) {
            //Check if there is something in the queue, if so return immediately
            //otherwise wait for the event and read again
            success = (int) vchi_msg_dequeue( gencmd_client.open_handle[i], gencmd_client.response_buffer,
                                              sizeof(gencmd_client.response_buffer), &gencmd_client.response_length, VCHI_FLAGS_NONE);

            if(success == 0) {
#ifdef __NetBSD__
	       uint32_t v;
#endif
#ifdef __NetBSD__
	       memcpy(&v, gencmd_client.response_buffer, sizeof(v));
               ret_code = VC_VTOH32(v);
#else
               ret_code = VC_VTOH32( *(int *)gencmd_client.response_buffer );
#endif
               break;
            } else {
               gencmd_client.response_length = 0;
            }
         }
      } while(!gencmd_client.response_length && vcos_event_wait(&gencmd_client.message_available_event) == VCOS_SUCCESS);
      
      if(gencmd_client.response_length && sem_ok == 0) {
         gencmd_client.response_length -= sizeof(int); //first word is error code
         memcpy(response, gencmd_client.response_buffer+sizeof(int), (size_t) vcos_min((int)gencmd_client.response_length, (int)maxlen));
      }

      release_gencmd_service();
      lock_release();
   }

   // If we read anything, return the VideoCore code. Error codes < 0 mean we failed to
   // read anything...
   //How do we let the caller know the response code of gencmd?
   //return ret_code;

   return success;
}
Beispiel #4
0
/** Callback invoked by VCHIQ
  */
static VCHIQ_STATUS_T mmal_vc_vchiq_callback(VCHIQ_REASON_T reason,
                                             VCHIQ_HEADER_T *vchiq_header,
                                             VCHIQ_SERVICE_HANDLE_T service,
                                             void *context)
{
   LOG_TRACE("reason %d", reason);

   switch (reason)
   {
   case VCHIQ_MESSAGE_AVAILABLE:
      {
         mmal_worker_msg_header *msg = (mmal_worker_msg_header*)vchiq_header->data;
         vcos_assert(msg->magic == MMAL_MAGIC);

         if (msg->msgid == MMAL_WORKER_BUFFER_TO_HOST)
         {
            LOG_TRACE("buffer to host");
            mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)vchiq_header->data;
            LOG_TRACE("len %d context %p", msg->buffer_header.length, msg->drvbuf.client_context);
            vcos_assert(msg->drvbuf.client_context);
            vcos_assert(msg->drvbuf.client_context->magic == MMAL_MAGIC);

            /* If the buffer is referencing another, need to replicate it here
             * in order to use the reference buffer's payload and ensure the
             * reference is not released prematurely */
            if (msg->has_reference)
               mmal_buffer_header_replicate(msg->drvbuf.client_context->buffer,
                                            msg->drvbuf_ref.client_context->buffer);

            /* Sanity check the size of the transfer so we don't overrun our buffer */
            if (!vcos_verify(msg->buffer_header.offset + msg->buffer_header.length <=
                             msg->drvbuf.client_context->buffer->alloc_size))
            {
               LOG_TRACE("buffer too small (%i, %i)",
                         msg->buffer_header.offset + msg->buffer_header.length,
                         msg->drvbuf.client_context->buffer->alloc_size);
               msg->buffer_header.length = 0; /* FIXME: set a buffer flag to signal error */
               msg->drvbuf.client_context->callback(msg);
               vchiq_release_message(service, vchiq_header);
               break;
            }
            /*To handle VC to HOST filled buffer callback of EOS buffer to receive in sync with data buffers*/
            if (!msg->is_zero_copy &&
                  (msg->buffer_header.length != 0 ||
                     (msg->buffer_header.flags & MMAL_BUFFER_HEADER_FLAG_EOS)))
            {
               /* a buffer full of data for us to process */
               VCHIQ_STATUS_T vst = VCHIQ_SUCCESS;
               LOG_TRACE("queue bulk rx: %p, %d", msg->drvbuf.client_context->buffer->data +
                         msg->buffer_header.offset, msg->buffer_header.length);
               int len = msg->buffer_header.length;
               len = (len+3) & (~3);

               if (!len && (msg->buffer_header.flags & MMAL_BUFFER_HEADER_FLAG_EOS))
               {
                  len = 8;
               }
               if (!msg->payload_in_message)
               {
                  /* buffer transferred using vchiq bulk xfer */
                  vst = vchiq_queue_bulk_receive(service,
                     msg->drvbuf.client_context->buffer->data + msg->buffer_header.offset,
                     len, vchiq_header);

                  if (vst != VCHIQ_SUCCESS)
                  {
                     LOG_TRACE("queue bulk rx len %d failed to start", msg->buffer_header.length);
                     msg->buffer_header.length = 0; /* FIXME: set a buffer flag to signal error */
                     msg->drvbuf.client_context->callback(msg);
                     vchiq_release_message(service, vchiq_header);
                  }
               }
               else if (msg->payload_in_message <= MMAL_VC_SHORT_DATA)
               {
                  /* we have already received the buffer data in the message! */
                  MMAL_BUFFER_HEADER_T *dst = msg->drvbuf.client_context->buffer;
                  LOG_TRACE("short data: dst = %p, dst->data = %p, len %d short len %d", dst, dst? dst->data : 0, msg->buffer_header.length, msg->payload_in_message);
                  memcpy(dst->data, msg->short_data, msg->payload_in_message);
                  dst->offset = 0;
                  dst->length = msg->payload_in_message;
                  vchiq_release_message(service, vchiq_header);
                  msg->drvbuf.client_context->callback(msg);
               }
               else
               {
                  /* impossible short data length */
                  LOG_ERROR("Message with invalid short payload length %d",
                            msg->payload_in_message);
                  vcos_assert(0);
               }
            }
            else
            {

               /* Message received from videocore; the client_context should have
                * been passed all the way through by videocore back to us, and will
                * be picked up in the callback to complete the sequence.
                */
               LOG_TRACE("doing cb (%p) context %p",
                         msg->drvbuf.client_context, msg->drvbuf.client_context ?
                         msg->drvbuf.client_context->callback : 0);
               msg->drvbuf.client_context->callback(msg);
               LOG_TRACE("done callback back to client");
               vchiq_release_message(service, vchiq_header);
            }
         }
         else if (msg->msgid == MMAL_WORKER_EVENT_TO_HOST)
         {
            mmal_vc_handle_event_msg(vchiq_header, service, context);
         }
         else
         {
            MMAL_WAITER_T *waiter = msg->u.waiter;
            LOG_TRACE("waking up waiter at %p", waiter);
            vcos_assert(waiter->inuse);
            int len = vcos_min(waiter->destlen, vchiq_header->size);
            waiter->destlen = len;
            LOG_TRACE("copying payload @%p to %p len %d", waiter->dest, msg, len);
            memcpy(waiter->dest, msg, len);
            vchiq_release_message(service, vchiq_header);
            vcos_semaphore_post(&waiter->sem);
         }
      }
      break;
   case VCHIQ_BULK_TRANSMIT_DONE:
      {
         /* nothing to do here, need to wait for the copro to tell us it
          * has emptied the buffer before we can recycle it, otherwise we
          * end up feeding the copro with buffers it cannot handle.
          */
#ifdef VCOS_LOGGING_ENABLED
         mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)context;
#endif
         LOG_TRACE("bulk tx done: %p, %d", msg->buffer_header.data, msg->buffer_header.length);
      }
      break;
   case VCHIQ_BULK_RECEIVE_DONE:
      {
         VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)context;
         mmal_worker_msg_header *msg_hdr = (mmal_worker_msg_header*)header->data;
         if (msg_hdr->msgid == MMAL_WORKER_BUFFER_TO_HOST)
         {
            mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)msg_hdr;
            vcos_assert(msg->drvbuf.client_context->magic == MMAL_MAGIC);
            msg->drvbuf.client_context->callback(msg);
            LOG_TRACE("bulk rx done: %p, %d", msg->buffer_header.data, msg->buffer_header.length);
         }
         else
         {
            mmal_worker_event_to_host *msg = (mmal_worker_event_to_host *)msg_hdr;
            MMAL_PORT_T *port = mmal_vc_port_by_number(msg->client_component, msg->port_type, msg->port_num);

            vcos_assert(port);
            mmal_buffer_header_driver_data(msg->delayed_buffer)->
               client_context->callback_event(port, msg->delayed_buffer);
            LOG_DEBUG("event bulk rx done, length %d", msg->length);
         }
         vchiq_release_message(service, header);
      }
      break;
   case VCHIQ_BULK_RECEIVE_ABORTED:
      {
         VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)context;
         mmal_worker_msg_header *msg_hdr = (mmal_worker_msg_header*)header->data;
         if (msg_hdr->msgid == MMAL_WORKER_BUFFER_TO_HOST)
         {
            mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)msg_hdr;
            LOG_TRACE("bulk rx aborted: %p, %d", msg->buffer_header.data, msg->buffer_header.length);
            vcos_assert(msg->drvbuf.client_context->magic == MMAL_MAGIC);
            msg->buffer_header.flags |= MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED;
            msg->drvbuf.client_context->callback(msg);
         }
         else
         {
            mmal_worker_event_to_host *msg = (mmal_worker_event_to_host *)msg_hdr;
            MMAL_PORT_T *port = mmal_vc_port_by_number(msg->client_component, msg->port_type, msg->port_num);

            vcos_assert(port);
            LOG_DEBUG("event bulk rx aborted");
            msg->delayed_buffer->flags |= MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED;
            mmal_buffer_header_driver_data(msg->delayed_buffer)->
               client_context->callback_event(port, msg->delayed_buffer);
         }
         vchiq_release_message(service, header);
      }
      break;
   case VCHIQ_BULK_TRANSMIT_ABORTED:
      {
         mmal_worker_buffer_from_host *msg = (mmal_worker_buffer_from_host *)context;
         LOG_INFO("bulk tx aborted: %p, %d", msg->buffer_header.data, msg->buffer_header.length);
         vcos_assert(msg->drvbuf.client_context->magic == MMAL_MAGIC);
         /* Nothing to do as the VC side will release the buffer and notify us of the error */
      }
      break;
   default:
      break;
   }

   return VCHIQ_SUCCESS;
}