flags.c

/*
 * This file is part of the OpenKinect Project. http://www.openkinect.org
 *
 * Copyright (c) 2010-2011 individual OpenKinect contributors. See the CONTRIB
 * file for details.
 *
 * This code is licensed to you under the terms of the Apache License, version
 * 2.0, or, at your option, the terms of the GNU General Public License,
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 * or the following URLs:
 * http://www.apache.org/licenses/LICENSE-2.0
 * http://www.gnu.org/licenses/gpl-2.0.txt
 *
 * If you redistribute this file in source form, modified or unmodified, you
 * may:
 *   1) Leave this header intact and distribute it under the same terms,
 *      accompanying it with the APACHE20 and GPL20 files, or
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 * In all cases you must keep the copyright notice intact and include a copy
 * of the CONTRIB file.
 *
 * Binary distributions must follow the binary distribution requirements of
 * either License.
 */

#include <string.h> // for memcpy
#include <unistd.h> // for usleep
#include "freenect_internal.h"
#include "flags.h"


FN_INTERNAL int register_for_flag(int flag)
{
    switch(flag)
    {
		case FREENECT_MIRROR_DEPTH:
			return 0x17;
		case FREENECT_MIRROR_VIDEO:
			return 0x47;
		default:
			return -1;
    }
}

int freenect_set_flag(freenect_device *dev, freenect_flag flag, freenect_flag_value value)
{
	freenect_context *ctx = dev->parent;

	if (flag == FREENECT_NEAR_MODE)
	{
		if (dev->usb_cam.PID != PID_K4W_CAMERA)
		{
			FN_WARNING("Near mode is only supported by K4W");
			return -1;
		}

		if (value == FREENECT_ON)
		{
			int ret = write_register(dev, 0x0015, 0x0007);
			if (ret < 0)
				return ret;
			usleep(100000);
			return write_register(dev, 0x02EF, 0x0000);
		}
		else
		{
			int ret = write_register(dev, 0x0015, 0x001E);
			if (ret < 0)
				return ret;
			usleep(100000);
			return write_register(dev, 0x02EF, 0x0190);
		}
	}

    if (flag >= (1 << 16))
    {
        int reg = register_for_flag(flag);
        if (reg < 0)
            return reg;
        return write_register(dev, reg, value);
    }

	uint16_t cmos_value = read_cmos_register(dev, 0x0106);
	if (cmos_value == UINT16_MAX)
	{
		return -1;
	}
	if (value == FREENECT_ON)
		cmos_value |= flag;
	else
		cmos_value &= ~flag;
	return write_cmos_register(dev, 0x0106, cmos_value);
}

int freenect_get_ir_brightness(freenect_device *dev)
{
	freenect_context *ctx = dev->parent;

	const uint16_t brightness = read_register(dev, 0x15);
	if (brightness == UINT16_MAX)
	{
		FN_WARNING("Failed to get IR brightness!");
		return -1;
	}

	return brightness;
}

int freenect_set_ir_brightness(freenect_device *dev, uint16_t brightness)
{
	freenect_context *ctx = dev->parent;

	if (brightness < 1)
	{
		brightness = 1;
	}
	if (brightness > 50)
	{
		brightness = 50;
	}

	const int ret = write_register(dev, 0x15, brightness);
	if (ret < 0)
	{
		FN_WARNING("Failed to set IR brightness");
	}

	return ret;
}

typedef struct {
	uint8_t magic[2];
	uint16_t len;
	uint16_t cmd;
	uint16_t tag;
} cam_hdr;

FN_INTERNAL int send_cmd(freenect_device *dev, uint16_t cmd, void *cmdbuf, unsigned int cmd_len, void *replybuf, int reply_len)
{
	freenect_context *ctx = dev->parent;
	int res, actual_len;
	uint8_t obuf[0x400];
	uint8_t ibuf[0x200];
	cam_hdr *chdr = (cam_hdr*)obuf;
	cam_hdr *rhdr = (cam_hdr*)ibuf;

	if (cmd_len & 1 || cmd_len > (0x400 - sizeof(*chdr))) {
		FN_ERROR("send_cmd: Invalid command length (0x%x)\n", cmd_len);
		return -1;
	}

	chdr->magic[0] = 0x47;
	chdr->magic[1] = 0x4d;
	chdr->cmd = fn_le16(cmd);
	chdr->tag = fn_le16(dev->cam_tag);
	chdr->len = fn_le16(cmd_len / 2);

	memcpy(obuf+sizeof(*chdr), cmdbuf, cmd_len);

	res = fnusb_control(&dev->usb_cam, 0x40, 0, 0, 0, obuf, cmd_len + sizeof(*chdr));
	FN_SPEW("send_cmd: cmd=%04x tag=%04x len=%04x: %d\n", cmd, dev->cam_tag, cmd_len, res);
	if (res < 0) {
		FN_ERROR("send_cmd: Output control transfer failed (%d)\n", res);
		return res;
	}

	do {
		actual_len = fnusb_control(&dev->usb_cam, 0xc0, 0, 0, 0, ibuf, 0x200);
		FN_FLOOD("send_cmd: actual length = %d\n", actual_len);
	} while ((actual_len == 0) || (actual_len == 0x200));
	FN_SPEW("Control reply: %d\n", res);
	if (actual_len < (int)sizeof(*rhdr)) {
		FN_ERROR("send_cmd: Input control transfer failed (%d)\n", res);
		return res;
	}
	actual_len -= sizeof(*rhdr);

	if (rhdr->magic[0] != 0x52 || rhdr->magic[1] != 0x42) {
		FN_ERROR("send_cmd: Bad magic %02x %02x\n", rhdr->magic[0], rhdr->magic[1]);
		return -1;
	}
	if (rhdr->cmd != chdr->cmd) {
		FN_ERROR("send_cmd: Bad cmd %02x != %02x\n", rhdr->cmd, chdr->cmd);
		return -1;
	}
	if (rhdr->tag != chdr->tag) {
		FN_ERROR("send_cmd: Bad tag %04x != %04x\n", rhdr->tag, chdr->tag);
		return -1;
	}
	if (fn_le16(rhdr->len) != (actual_len/2)) {
		FN_ERROR("send_cmd: Bad len %04x != %04x\n", fn_le16(rhdr->len), (int)(actual_len/2));
		return -1;
	}

	if (actual_len > reply_len) {
		FN_WARNING("send_cmd: Data buffer is %d bytes long, but got %d bytes\n", reply_len, actual_len);
		memcpy(replybuf, ibuf+sizeof(*rhdr), reply_len);
	} else {
		memcpy(replybuf, ibuf+sizeof(*rhdr), actual_len);
	}

	dev->cam_tag++;

	return actual_len;
}

// returns UINT16_MAX on error
FN_INTERNAL uint16_t read_register(freenect_device *dev, uint16_t reg)
{
	freenect_context *ctx = dev->parent;

	uint16_t reply[2];
	uint16_t cmd = fn_le16(reg);

	int res = send_cmd(dev, 0x02, &cmd, 2, reply, 4);
	if (res < 0)
	{
		FN_ERROR("read_register: send_cmd() failed: %d\n", res);
		return UINT16_MAX;
	}
	if (res != 4)
		FN_WARNING("read_register: send_cmd() returned %d [%04x %04x], 0000 expected\n", res, reply[0], reply[1]);

	FN_DEBUG("read_register: 0x%04x => 0x%04x\n", reg, reply[1]);
	return reply[1];
}

FN_INTERNAL int write_register(freenect_device *dev, uint16_t reg, uint16_t data)
{
	freenect_context *ctx = dev->parent;
	uint16_t reply[2];
	uint16_t cmd[2];

	cmd[0] = fn_le16(reg);
	cmd[1] = fn_le16(data);

	FN_DEBUG("write_register: 0x%04x <= 0x%02x\n", reg, data);
	int res = send_cmd(dev, 0x03, cmd, 4, reply, 4);
	if (res < 0)
	{
		FN_ERROR("write_register: send_cmd() returned %d\n", res);
		return res;
	}
	if (res != 2)
		FN_WARNING("write_register: send_cmd() returned %d [%04x %04x], 0000 expected\n", res, reply[0], reply[1]);

	return 0;
}

// returns UINT16_MAX on error
FN_INTERNAL uint16_t read_cmos_register(freenect_device *dev, uint16_t reg)
{
	freenect_context *ctx = dev->parent;
	uint16_t replybuf[0x200];
	uint16_t cmdbuf[3];

	cmdbuf[0] = 1;
	cmdbuf[1] = reg & 0x7fff;
	cmdbuf[2] = 0;

	int res = send_cmd(dev, 0x95, cmdbuf, 6, replybuf, 6);
	if (res < 0)
	{
		FN_ERROR("read_cmos_register: send_cmd() returned %d\n", res);
		return UINT16_MAX;
	}

	FN_DEBUG("read_cmos_register: 0x%04x => 0x%04x\n", reg, replybuf[2]);
	return replybuf[2];
}

FN_INTERNAL int write_cmos_register(freenect_device *dev, uint16_t reg, uint16_t value)
{
	freenect_context *ctx = dev->parent;
	uint16_t replybuf[0x200];
	uint16_t cmdbuf[3];

	cmdbuf[0] = 1;
	cmdbuf[1] = reg | 0x8000;
	cmdbuf[2] = value;

	FN_DEBUG("write_cmos_register: 0x%04x <= 0x%04x\n", reg, value);
	int res = send_cmd(dev, 0x95, cmdbuf, 6, replybuf, 6);
	if (res < 0)
		FN_ERROR("write_cmos_register: send_cmd() returned %d\n", res);
	return res;
}