--- libaitpelco/src/aitpelco.c 2010/01/22 00:57:32 1.1 +++ libaitpelco/src/aitpelco.c 2011/05/10 20:26:08 1.2 @@ -3,9 +3,46 @@ * by Michael Pounov * * $Author: misho $ -* $Id: aitpelco.c,v 1.1 2010/01/22 00:57:32 misho Exp $ +* $Id: aitpelco.c,v 1.2 2011/05/10 20:26:08 misho Exp $ * -*************************************************************************/ +************************************************************************** +The ELWIX and AITNET software is distributed under the following +terms: + +All of the documentation and software included in the ELWIX and AITNET +Releases is copyrighted by ELWIX - Sofia/Bulgaria + +Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011 + by Michael Pounov . All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. +3. All advertising materials mentioning features or use of this software + must display the following acknowledgement: +This product includes software developed by Michael Pounov +ELWIX - Embedded LightWeight unIX and its contributors. +4. Neither the name of AITNET nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS +OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF +SUCH DAMAGE. +*/ #include "global.h" #include "aitpelco.h" @@ -19,19 +56,22 @@ static char pelco_Error[STRSIZ]; // // pelco_GetErrno() Get error code of last operation -inline int pelco_GetErrno() +inline int +pelco_GetErrno() { return pelco_Errno; } // pelco_GetError() Get error text of last operation -inline const char *pelco_GetError() +inline const char * +pelco_GetError() { return pelco_Error; } // pelco_SetErr() Set error to variables for internal use!!! -inline void pelcoSetErr(int eno, char *estr, ...) +inline void +pelcoSetErr(int eno, char *estr, ...) { va_list lst; @@ -50,7 +90,8 @@ inline void pelcoSetErr(int eno, char *estr, ...) * @camNo = Packet for camera number address * return: NULL error, !=NULL ok, allocated memory for packet */ -inline void *pelcoOpen(u_char pelcoVer, u_char camNo) +inline void * +pelcoOpen(u_char pelcoVer, u_char camNo) { pelco_d_t *pd; pelco_p_t *pp; @@ -60,7 +101,7 @@ inline void *pelcoOpen(u_char pelcoVer, u_char camNo) case 'D': case 'd': if (camNo < FIRST_CAM_D) { - pelcoSetErr(ENOEXEC, "Error:: unsupported camera number %d!\n", camNo); + pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", camNo); return NULL; } @@ -92,7 +133,7 @@ inline void *pelcoOpen(u_char pelcoVer, u_char camNo) p = pp; break; default: - pelcoSetErr(ENOEXEC, "Error:: unsupported Pelco protocol version!\n"); + pelcoSetErr(ENOEXEC, "unsupported Pelco protocol version!\n"); } return p; @@ -102,13 +143,64 @@ inline void *pelcoOpen(u_char pelcoVer, u_char camNo) * pelcoClose() Close packet record and free memory * @p = Packet structure for close */ -inline void pelcoClose(void * __restrict p) +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 @@ -116,22 +208,23 @@ inline void pelcoClose(void * __restrict p) * @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) +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"); + 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, "Error:: unsupported camera number %d!\n", ptr[1]); + pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", ptr[1]); return 0xFF; } @@ -143,7 +236,7 @@ inline u_char pelcoAddCmdData(void * __restrict p, u_c break; case VER_P_STX: if (VER_P_ETX != ptr[6]) { - pelcoSetErr(ENOEXEC, "Error:: Broken Pelco P packet!\n"); + pelcoSetErr(ENOEXEC, "Broken Pelco P packet!\n"); return 0xFF; } @@ -154,9 +247,69 @@ inline u_char pelcoAddCmdData(void * __restrict p, u_c pp->p_crc = crcPelco('p', p); break; default: - pelcoSetErr(ENOEXEC, "Error:: Invalid protocol!\n"); + 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; }