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


static int pelco_Errno;
static char pelco_Error[STRSIZ];


//
// Error maintenance functions ...
//

// pelco_GetErrno() Get error code of last operation
inline int pelco_GetErrno()
{
	return pelco_Errno;
}

// pelco_GetError() Get error text of last operation
inline const char *pelco_GetError()
{
	return pelco_Error;
}

// pelco_SetErr() Set error to variables for internal use!!!
inline void pelcoSetErr(int eno, char *estr, ...)
{
	va_list lst;

	pelco_Errno = eno;
	memset(pelco_Error, 0, STRSIZ);
	va_start(lst, estr);
	vsnprintf(pelco_Error, STRSIZ, estr, lst);
	va_end(lst);
}

// ----------------------------------------------------------

/*
 * pelcoOpen() Open packet record for camera number with Pelco version D/P
 * @pelcoVer = Pelco protocol version Dd | Pp
 * @camNo = Packet for camera number address
 * return: NULL error, !=NULL ok, allocated memory for packet
 */
inline void *pelcoOpen(u_char pelcoVer, u_char camNo)
{
	pelco_d_t *pd;
	pelco_p_t *pp;
	void *p = NULL;

	switch (pelcoVer) {
		case 'D':
		case 'd':
			if (camNo < FIRST_CAM_D) {
				pelcoSetErr(ENOEXEC, "Error:: unsupported camera number %d!\n", camNo);
				return NULL;
			}

			pd = malloc(sizeof(pelco_d_t));
			if (!pd) {
				SETERR;
				return NULL;
			} else
				memset(pd, 0, sizeof(pelco_d_t));

			pd->d_sync = VER_D_SYNC;
			pd->d_cam = camNo;

			p = pd;
			break;
		case 'P':
		case 'p':
			pp = malloc(sizeof(pelco_p_t));
			if (!pp) {
				SETERR;
				return NULL;
			} else
				memset(pp, 0, sizeof(pelco_p_t));

			pp->p_stx = VER_P_STX;
			pp->p_cam = !camNo ? camNo : camNo - 1;
			pp->p_etx = VER_P_ETX;

			p = pp;
			break;
		default:
			pelcoSetErr(ENOEXEC, "Error:: unsupported Pelco protocol version!\n");
	}

	return p;
}

/*
 * pelcoClose() Close packet record and free memory
 * @p = Packet structure for close
 */
inline void pelcoClose(void * __restrict p)
{
	if (p)
		free(p);
}


/*
 * pelcoAddCmdData() Add commands and datas for already opened packet
 * @p = Input Packet structure
 * @cmd[2] = Input Commands 1 & 2
 * @data[2] = Input Data for commands 1 & 2
 * return: 0xFF - error, 0 - ok
 */
inline u_char pelcoAddCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
{
	u_char ret = 0;
	pelco_d_t *pd = (pelco_d_t *) p;
	pelco_p_t *pp = (pelco_p_t *) p;
	u_char *ptr = p;

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

	switch (ptr[0]) {
		case VER_D_SYNC:
			if (ptr[1] < FIRST_CAM_D) {
				pelcoSetErr(ENOEXEC, "Error:: unsupported camera number %d!\n", ptr[1]);
				return 0xFF;
			}

			if (cmd)
				memcpy(&pd->d_cmd1, cmd, 2);
			if (data)
				memcpy(&pd->d_data, data, 2);
			pd->d_crc = crcPelco('d', p);
			break;
		case VER_P_STX:
			if (VER_P_ETX != ptr[6]) {
				pelcoSetErr(ENOEXEC, "Error:: Broken Pelco P packet!\n");
				return 0xFF;
			}

			if (cmd)
				memcpy(&pp->p_cmd1, cmd, 2);
			if (data)
				memcpy(&pp->p_data, data, 2);
			pp->p_crc = crcPelco('p', p);
			break;
		default:
			pelcoSetErr(ENOEXEC, "Error:: Invalid protocol!\n");
			return 0xFF;
	}

	return ret;
}