/*************************************************************************
* (C) 2010 AITNET ltd - Sofia/Bulgaria - <misho@aitbg.com>
*  by Michael Pounov <misho@openbsd-bg.org>
*
* $Author: misho $
* $Id: get.c,v 1.1 2010/01/22 00:57:32 misho Exp $
*
*************************************************************************/
#include "global.h"


/*
 * pelco_GetVersion() Return Pelco protocol version
 * @p = Packet structure
 * return: 'd' - PelcoD, 'p' - PelcoP, 0 - unknown or bad packet
 */
inline u_char pelco_GetVersion(void * __restrict p)
{
	u_char *ptr = (u_char *) p;

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

	switch (*ptr) {
		case VER_D_SYNC:
			return 'd';
		case VER_P_STX:
			if (VER_P_ETX == ptr[6])
				return 'p';
			pelcoSetErr(ENOEXEC, "Error:: broken Pelco P packet ...\n");
			break;
		default:
			pelcoSetErr(ENOEXEC, "Error:: unsupported Pelco protocol version!\n");
	}

	return 0;
}

/*
 * pelco_GetCamNo() Get Camera number and check for valid packet
 * @p = Packet structure
 * return: 0xFF and pelco_GetErrno() == ENOEXEC - error, 
 	any number is camera address
 */
inline u_char pelco_GetCamNo(void * __restrict p)
{
	u_char ver;
	u_char *ptr = (u_char *) p;

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

	ver = pelco_GetVersion(p);
	switch (ver) {
		case 'd':
			if (crcPelco(ver, p) == ptr[6])
				return ptr[1];

			pelcoSetErr(ENOEXEC, "Error:: broken Pelco D packet!!!\n");
			break;
		case 'p':
			if (crcPelco(ver, p) == ptr[7])
				return ptr[1] + 1;

			pelcoSetErr(ENOEXEC, "Error:: broken Pelco P packet!!!\n");
			break;
	}

	return 0xFF;
}

/*
 * pelco_GetCamCmdData() Get from Camera commands and datas with verify packet
 * @p = Input Packet structure
 * @cam = Output camera number
 * @cmd[2] = Output Commands 1 & 2
 * @data[2] = Output Data for commands 1 & 2
 * return: 'd' - PelcoD, 'p' - PelcoP, 0 - unknown or bad packet
 */
inline u_char pelco_GetCamCmdData(void * __restrict p, u_char * __restrict cam, 
		u_char * __restrict cmd, u_char * __restrict data)
{
	u_char ret;
	u_char *ptr = (u_char *) p;

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

	ret = pelco_GetVersion(p);
	switch (ret) {
		case 'd':
			if (crcPelco(ret, p) != ptr[6]) {
				pelcoSetErr(ENOEXEC, "Error:: broken Pelco D packet!!!\n");
				return 0;
			}

			if (cam)
				*cam = ptr[1];
			break;
		case 'p':
			if (crcPelco(ret, p) != ptr[7]) {
				pelcoSetErr(ENOEXEC, "Error:: broken Pelco P packet!!!\n");
				return 0;
			}

			if (cam)
				*cam = ptr[1] + 1;
			break;
	}

	if (cmd)
		memcpy(cmd, ptr + 2, 2);
	if (data)
		memcpy(data, ptr + 4, 2);
	return ret;
}

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

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

	if (cmd1) {
		cmd1 = (struct bitD_Cmd1*) &cmd[0];
		ret++;
	}
	if (cmd2) {
		cmd2 = (struct bitD_Cmd2*) &cmd[1];
		ret++;
	}

	return ret;
}

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

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

	if (cmd1) {
		cmd1 = (struct bitP_Cmd1*) &cmd[0];
		ret++;
	}
	if (cmd2) {
		cmd2 = (struct bitP_Cmd2*) &cmd[1];
		ret++;
	}

	return ret;
}