dc1394_format7.c

较新版本的libdc1394

/*
 * 1394-Based Digital Camera Format_7 functions for the Control Library
 *
 * Written by Damien Douxchamps <[e-mail]>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
#include <unistd.h>
#include <netinet/in.h>
#include <errno.h>
#include <stdlib.h>
 
#include "dc1394_control.h"
#include "dc1394_internal.h"
#include "config.h"

#define REG_CAMERA_V_CSR_INQ_BASE                        0x2E0U

#define REG_CAMERA_FORMAT7_MAX_IMAGE_SIZE_INQ            0x000U
#define REG_CAMERA_FORMAT7_UNIT_SIZE_INQ                 0x004U
#define REG_CAMERA_FORMAT7_IMAGE_POSITION                0x008U
#define REG_CAMERA_FORMAT7_IMAGE_SIZE                    0x00CU
#define REG_CAMERA_FORMAT7_COLOR_CODING_ID               0x010U
#define REG_CAMERA_FORMAT7_COLOR_CODING_INQ              0x014U
#define REG_CAMERA_FORMAT7_PIXEL_NUMBER_INQ              0x034U
#define REG_CAMERA_FORMAT7_TOTAL_BYTES_HI_INQ            0x038U
#define REG_CAMERA_FORMAT7_TOTAL_BYTES_LO_INQ            0x03CU
#define REG_CAMERA_FORMAT7_PACKET_PARA_INQ               0x040U
#define REG_CAMERA_FORMAT7_BYTE_PER_PACKET               0x044U
#define REG_CAMERA_FORMAT7_PACKET_PER_FRAME_INQ          0x048U
#define REG_CAMERA_FORMAT7_UNIT_POSITION_INQ             0x04CU
#define REG_CAMERA_FORMAT7_FRAME_INTERVAL_INQ            0x050U
#define REG_CAMERA_FORMAT7_DATA_DEPTH_INQ                0x054U
#define REG_CAMERA_FORMAT7_COLOR_FILTER_ID               0x058U
#define REG_CAMERA_FORMAT7_VALUE_SETTING                 0x07CU

/**********************/ 
/* Internal functions */
/**********************/

static int
QueryFormat7CSROffset(raw1394handle_t handle, nodeid_t node, int mode,
		      quadlet_t *value)
{
    int retval;

    if ( (mode > MODE_FORMAT7_MAX) || (mode < MODE_FORMAT7_MIN) )
    {
        return DC1394_FAILURE;
    }

    mode-= MODE_FORMAT7_MIN;
    retval= GetCameraControlRegister(handle, node,
                                     REG_CAMERA_V_CSR_INQ_BASE +
                                     (mode * 0x04U), value);
    return (retval ? DC1394_FAILURE : DC1394_SUCCESS);
}

static int
GetCameraFormat7Register(raw1394handle_t handle, nodeid_t node,
                         unsigned int mode, octlet_t offset, quadlet_t *value)
{
    int retval, retry= MAX_RETRIES;
    quadlet_t csr;
    
    dc1394_camerahandle *camera;
    camera = (dc1394_camerahandle*) raw1394_get_userdata( handle );

    if ( (mode > MODE_FORMAT7_MAX) || (mode < MODE_FORMAT7_MIN) )
    {
        return DC1394_FAILURE;
    }

    if (camera->format7_csr[mode-MODE_FORMAT7_MIN]==0) {
      if (QueryFormat7CSROffset(handle, node, mode, &csr) != DC1394_SUCCESS) 
	return DC1394_FAILURE;
      else 
	camera->format7_csr[mode-MODE_FORMAT7_MIN]=csr;
    }
    else {
      csr=camera->format7_csr[mode-MODE_FORMAT7_MIN];
    }

    csr*=0x04UL;

    /* retry a few times if necessary (addition by PDJ) */
    while(retry--)
    {
        retval= raw1394_read(handle, 0xffc0 | node,
                             CONFIG_ROM_BASE + csr + offset, 4, value);

#ifdef LIBRAW1394_OLD
        if (retval >= 0)
        {
            int ack= retval >> 16;
            int rcode= retval & 0xffff;

#ifdef SHOW_ERRORS
            printf("Format 7 reg read ack of %x rcode of %x\n", ack, rcode);
#endif

            if ( ((ack == ACK_PENDING) || (ack == ACK_LOCAL)) &&
                 (rcode == RESP_COMPLETE) )
            { 
                /* conditionally byte swap the value */
                *value= ntohl(*value); 
                return DC1394_SUCCESS;
            }

        }
#else
        if (!retval)
        {
            /* conditionally byte swap the value */
            *value= ntohl(*value);
            return (retval ? DC1394_FAILURE : DC1394_SUCCESS);
        }
        else if (errno != EAGAIN)
        {
            return (retval ? DC1394_FAILURE : DC1394_SUCCESS);
        }
#endif /* LIBRAW1394_VERSION <= 0.8.2 */

        usleep(SLOW_DOWN);
    }
    
    *value = ntohl(*value);
    return (retval ? DC1394_FAILURE : DC1394_SUCCESS);
}

static int
SetCameraFormat7Register(raw1394handle_t handle, nodeid_t node,
                         unsigned int mode, octlet_t offset, quadlet_t value)
{
    int retval, retry= MAX_RETRIES;
    quadlet_t csr;
    
    dc1394_camerahandle *camera;
    camera = (dc1394_camerahandle*) raw1394_get_userdata( handle );

    if ( (mode > MODE_FORMAT7_MAX) || (mode < MODE_FORMAT7_MIN) )
    {
        return DC1394_FAILURE;
    }

    if (camera->format7_csr[mode-MODE_FORMAT7_MIN]==0) {
      if (QueryFormat7CSROffset(handle, node, mode, &csr) != DC1394_SUCCESS) 
	return DC1394_FAILURE;
      else 
	camera->format7_csr[mode-MODE_FORMAT7_MIN]=csr;
    }
    else {
      csr=camera->format7_csr[mode-MODE_FORMAT7_MIN];
    }

    csr*=0x04UL;

    /* conditionally byte swap the value (addition by PDJ) */
    value= htonl(value);
 
    /* retry a few times if necessary (addition by PDJ) */
    while(retry--)
    {
        retval= raw1394_write(handle, 0xffc0 | node,
                              CONFIG_ROM_BASE + offset + csr, 4, &value);

#ifdef LIBRAW1394_OLD
        if (retval >= 0)
        {
            int ack= retval >> 16;
            int rcode= retval & 0xffff;

#ifdef SHOW_ERRORS
            printf("Format 7 reg write ack of %x rcode of %x\n", ack, rcode);
#endif

            if ( ((ack == ACK_PENDING) || (ack == ACK_LOCAL) ||
                  (ack == ACK_COMPLETE)) &&
                 ((rcode == RESP_COMPLETE) || (rcode == RESP_SONY_HACK)) ) 
            {
                return DC1394_SUCCESS;
            }
            
            
        }
#else
        if (!retval || (errno != EAGAIN))
        {
            return (retval ? DC1394_FAILURE : DC1394_SUCCESS);
        }
#endif /* LIBRAW1394_VERSION <= 0.8.2 */

 	usleep(SLOW_DOWN);
    }
    return (retval ? DC1394_FAILURE : DC1394_SUCCESS);
    
}

/*==========================================================================
 * This function implements the handshaking available (and sometimes required)
 * on some cameras that comply with the IIDC specs v1.30. Thanks to Yasutoshi
 * Onishi for his feedback and info.
 *==========================================================================*/

int
_dc1394_v130_handshake(raw1394handle_t handle, nodeid_t node, int mode)
{
  int setting_1, err_flag1, err_flag2, v130handshake;
  int exit_loop;

  dc1394_camerahandle *camera;
  camera = (dc1394_camerahandle*) raw1394_get_userdata( handle );

  if (camera->sw_version >= IIDC_VERSION_1_30) {
    // We don't use > because 114 is for ptgrey cameras which are not 1.30 but 1.20
    if (dc1394_query_format7_value_setting(handle, node, mode, &v130handshake,
					   &setting_1, &err_flag1, &err_flag2)
	!= DC1394_SUCCESS) {
      printf("(%s) Unable to read value setting register.\n", __FILE__);
      return DC1394_FAILURE;
    }
  }
  else {
    v130handshake=0;
  }   
  
  if (v130handshake==1) {
    // we should use advanced IIDC v1.30 handshaking.
    //fprintf(stderr,"using handshaking\n");
    // set value setting to 1
    if (dc1394_set_format7_value_setting(handle, node, mode) != DC1394_SUCCESS) {
      printf("(%s) Unable to set value setting register.\n", __FILE__);
      return DC1394_FAILURE;
    }
    // wait for value setting to clear:
    exit_loop=0;
    while (!exit_loop) { // WARNING: there is no timeout in this loop yet.
      if (dc1394_query_format7_value_setting(handle, node, mode, &v130handshake,
					     &setting_1, &err_flag1, &err_flag2)
	  != DC1394_SUCCESS) {
	printf("(%s) Unable to read value setting register.\n", __FILE__);
	return DC1394_FAILURE;
      }
      exit_loop=(setting_1==0);
      usleep(0); 
    }
    if (err_flag1>0) {
      printf("(%s) Invalid image position, size, color coding, ISO speed or bpp\n", __FILE__);
      return DC1394_FAILURE;
    }
    
    // bytes per packet... registers are ready for reading.
  }
  return DC1394_SUCCESS;
}

int
_dc1394_v130_errflag2(raw1394handle_t handle, nodeid_t node, int mode)
{
  int setting_1, err_flag1, err_flag2, v130handshake;

  dc1394_camerahandle *camera;
  camera = (dc1394_camerahandle*) raw1394_get_userdata( handle );

  //fprintf(stderr,"Checking error flags\n");

  if (camera->sw_version >= IIDC_VERSION_1_30) { // if version is 1.30.
    // We don't use > because 0x114 is for ptgrey cameras which are not 1.30 but 1.20
    if (dc1394_query_format7_value_setting(handle, node, mode, &v130handshake,
					   &setting_1, &err_flag1, &err_flag2)
	!= DC1394_SUCCESS) {
      printf("(%s) Unable to read value setting register.\n", __FILE__);
      return DC1394_FAILURE;
    }
  }
  else {
    v130handshake=0;
  }
      
  if (v130handshake==1) {
    if (err_flag2==0)
      return DC1394_SUCCESS;
    else {
      printf("(%s) Error flag 2 is set: proposed bytes per packet is not a valid value.\n", __FILE__);
      return DC1394_FAILURE;
    }
  }

  return DC1394_SUCCESS;
}

/*=================================================================================*
 * The following function returns the bytes per pixel for a defined color coding.  *
 *=================================================================================*/

float
_Format7BytePerPixel(int color_coding)
{
  switch (color_coding)
    {
    case COLOR_FORMAT7_MONO8: return 1.0;
      break;
    case COLOR_FORMAT7_YUV411: return 1.5;
      break;
    case COLOR_FORMAT7_YUV422: return 2.0;
      break;
    case COLOR_FORMAT7_YUV444: return 3.0;
      break;
    case COLOR_FORMAT7_RGB8: return 3.0;
      break;
    case COLOR_FORMAT7_MONO16: return 2.0;
      break;
    case COLOR_FORMAT7_RGB16: return 6.0;
      break;
    case COLOR_FORMAT7_MONO16S: return 2.0;
      break;
    case COLOR_FORMAT7_RGB16S: return 6.0;
      break;
    case COLOR_FORMAT7_RAW8: return 1.0;
      break;
    case COLOR_FORMAT7_RAW16: return 2.0;
      break;
    default: return (-1);
    }
}

 
/*====================================================================== 
 *   see documentation of dc1394_setup_format7_capture() in
 *   dc1394_control.h
 *======================================================================*/
int
_dc1394_basic_format7_setup(raw1394handle_t handle, nodeid_t node,
                            int channel, int mode, int speed,
                            int bytes_per_packet,
                            int left, int top,
                            int width, int height, 
                            dc1394_cameracapture *camera)
{
  dc1394bool_t is_iso_on= DC1394_FALSE;
  unsigned int unit_bytes, max_bytes;
  unsigned packet_bytes=0;
  unsigned int recom_bpp;
  int packets_per_frame;
  int color_coding;
  unsigned int camera_left = 0;
  unsigned int camera_top = 0;
  unsigned int camera_width = 0;
  unsigned int camera_height = 0;
  unsigned int max_width = 0;
  unsigned int max_height = 0;

  dc1394_camerahandle *camerahandle;
  camerahandle = (dc1394_camerahandle*) raw1394_get_userdata( handle );

  if (dc1394_get_iso_status(handle, node, &is_iso_on) != DC1394_SUCCESS) {
    return DC1394_FAILURE;
  }

  if (is_iso_on) {
    if (dc1394_stop_iso_transmission(handle, node) != DC1394_SUCCESS) {
      printf("(%s) Unable to stop iso transmission!\n", __FILE__);
      return DC1394_FAILURE;
    }
  }

  if (dc1394_set_iso_channel_and_speed(handle,node,channel,speed)!=DC1394_SUCCESS) {
    printf("(%s) Unable to set channel %d and speed %d!\n",__FILE__,channel,speed);
    return DC1394_FAILURE;
  }
  
  if (dc1394_set_video_format(handle,node,FORMAT_SCALABLE_IMAGE_SIZE)!=DC1394_SUCCESS) {
    printf("(%s) Unable to set video format %d!\n",__FILE__, FORMAT_SCALABLE_IMAGE_SIZE);
    return DC1394_FAILURE;
  }
  
  if (dc1394_set_video_mode(handle, node,mode) != DC1394_SUCCESS) {
    printf("(%s) Unable to set video mode %d!\n", __FILE__, mode);
    return DC1394_FAILURE;
  }

  // get BPP before setting sizes,...
  if (bytes_per_packet==QUERY_FROM_CAMERA) {
    if (dc1394_query_format7_byte_per_packet(handle, node, mode, &bytes_per_packet) != DC1394_SUCCESS){
      printf("(%s) Unable to get F7 bpp %d!\n", __FILE__, mode);
      return DC1394_FAILURE;
    }
  }

  /*-----------------------------------------------------------------------