/*************************************************************************
* (C) 2010 AITNET ltd - Sofia/Bulgaria - <misho@aitbg.com>
*  by Michael Pounov <misho@openbsd-bg.org>
*
* $Author: misho $
* $Id: set.c,v 1.1.1.1.2.1 2010/03/17 13:34:34 misho Exp $
*
*************************************************************************/
#include "global.h"


/*
 * pelco_D_fromCmd() Convert from Pelco D commands
 * @cmd1 = Input typecasted commands 1
 * @cmd2 = Input typecasted commands 2
 * @cmd[2] = Output Commands 1 & 2
 * return: 0xFF - error, 0 - ok
 */
inline u_char pelco_D_fromCmd(struct bitD_Cmd1 cmd1, struct bitD_Cmd2 cmd2, 
		u_char * __restrict cmd)
{
	u_char ret = 0;

	if (!cmd) {
		pelcoSetErr(EINVAL, "invalid argument!\n");
		return 0xFF;
	}

	memcpy(&cmd[0], &cmd1, 1);
	memcpy(&cmd[1], &cmd2, 1);

	return ret;
}

/*
 * pelco_P_fromCmd() Convert from Pelco P commands
 * @cmd1 = Input typecasted commands 1
 * @cmd2 = Input typecasted commands 2
 * @cmd[2] = Output Commands 1 & 2
 * return: 0xFF - error, 0 - ok
 */
inline u_char pelco_P_fromCmd(struct bitP_Cmd1 cmd1, struct bitP_Cmd2 cmd2, 
		u_char * __restrict cmd)
{
	u_char ret = 0;

	if (!cmd) {
		pelcoSetErr(EINVAL, "invalid argument!\n");
		return 0xFF;
	}

	memcpy(&cmd[0], &cmd1, 1);
	memcpy(&cmd[1], &cmd2, 1);

	return ret;
}

/*
 * pelco_SetCamCmdData() Set Camera commands and datas
 * @ver = Input Pelco (d | p) version
 * @cam = Input camera number
 * @cmd[2] = Input Commands 1 & 2
 * @data[2] = Input Data for commands 1 & 2
 * @p = Output Packet structure
 * return: 0xFF - error, 0 - ok
 */
inline u_char pelco_SetCamCmdData(u_char ver, u_char __restrict cam, 
		u_char * __restrict cmd, u_char * __restrict data, void * __restrict p)
{
	u_char ret = 0;
	pelco_d_t *pd = (pelco_d_t*) p;
	pelco_p_t *pp = (pelco_p_t*) p;

	if (!p) {
		pelcoSetErr(EINVAL, "invalid argument!\n");
		return 0xFF;
	}

	switch (ver) {
		case 'D':
		case 'd':
			memset(pd, 0, sizeof(pelco_d_t));
			pd->d_sync = VER_D_SYNC;
			pd->d_cam = cam;
			if (cmd)
				memcpy(&pd->d_cmd1, cmd, 2);
			if (data)
				memcpy(&pd->d_data, data, 2);
			pd->d_crc = crcPelco(ver, p);
			break;
		case 'P':
		case 'p':
			memset(pp, 0, sizeof(pelco_p_t));
			pp->p_stx = VER_P_STX;
			pp->p_cam = !cam ? cam : cam - 1;
			if (cmd)
				memcpy(&pp->p_cmd1, cmd, 2);
			if (data)
				memcpy(&pp->p_data, data, 2);
			pp->p_etx = VER_P_ETX;
			pp->p_crc = crcPelco(ver, p);
			break;
		default:
			pelcoSetErr(ENOEXEC, "Invalid protocol!\n");
			return 0xFF;
	}

	return ret;
}