/*************************************************************************
* (C) 2010 AITNET ltd - Sofia/Bulgaria - <misho@aitbg.com>
* by Michael Pounov <misho@openbsd-bg.org>
*
* $Author: misho $
* $Id: aitpelco.c,v 1.1.1.1.2.5 2010/03/23 01:25:43 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, "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, "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);
}
/*
* pelcoLoad() Load packet from input buffer
* @buffer = Pelco packet from input buffer
* return: NULL error, !=NULL ok, allocated memory for packet
*/
inline void *pelcoLoad(u_char *buffer)
{
pelco_d_t *pd;
pelco_p_t *pp;
void *p = NULL;
if (!buffer || !*buffer) {
pelcoSetErr(EINVAL, "invalid argument!\n");
return NULL;
}
switch (pelco_GetVersion(buffer)) {
case 'd':
if (pelco_GetCamNo(buffer) < FIRST_CAM_D) {
pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n",
pelco_GetCamNo(buffer));
return NULL;
}
pd = malloc(sizeof(pelco_d_t));
if (!pd) {
SETERR;
return NULL;
} else
memcpy(pd, buffer, sizeof(pelco_d_t));
p = pd;
break;
case 'p':
pp = malloc(sizeof(pelco_p_t));
if (!pp) {
SETERR;
return NULL;
} else
memcpy(pp, buffer, sizeof(pelco_p_t));
p = pp;
break;
default:
pelcoSetErr(ENOEXEC, "unsupported Pelco protocol version!\n");
}
return 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 || !*ptr) {
pelcoSetErr(EINVAL, "invalid argument!\n");
return 0xFF;
}
switch (ptr[0]) {
case VER_D_SYNC:
if (ptr[1] < FIRST_CAM_D) {
pelcoSetErr(ENOEXEC, "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, "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, "Invalid protocol!\n");
return 0xFF;
}
return ret;
}
/*
* pelcoGetCmdData() Get from packet commands and datas with verify packet
* @p = Input Packet structure
* @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 pelcoGetCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
{
return pelco_GetCamCmdData(p, NULL, cmd, data);
}
/*
* pelcoChkSum() Check ot Correct check sum in packet
* @p = Input Packet structure
* @correct = Calculate new check sum if incorrect !=0, if ==0 only check
* return: 0xFF - bad packet, 1 invalid check sum, 0 check sum is correct.
*/
inline u_char pelcoChkSum(void * __restrict p, u_char correct)
{
u_char sum, *ptr = p;
pelco_d_t *pd = (pelco_d_t *) p;
pelco_p_t *pp = (pelco_p_t *) p;
if (!p || !*ptr) {
pelcoSetErr(EINVAL, "invalid argument!\n");
return 0xFF;
}
switch (ptr[0]) {
case VER_D_SYNC:
if (ptr[1] < FIRST_CAM_D) {
pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", ptr[1]);
return 0xFF;
}
sum = crcPelco('d', p);
if (correct)
pd->d_crc = sum;
break;
case VER_P_STX:
if (VER_P_ETX != ptr[6]) {
pelcoSetErr(ENOEXEC, "Broken Pelco P packet!\n");
return 0xFF;
}
sum = crcPelco('p', p);
if (correct)
pp->p_crc = sum;
break;
default:
pelcoSetErr(ENOEXEC, "Invalid protocol!\n");
return 0xFF;
}
return sum;
}
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>