Annotation of libaitpelco/src/aitpelco.c, revision 1.2
1.1 misho 1: /*************************************************************************
2: * (C) 2010 AITNET ltd - Sofia/Bulgaria - <misho@aitbg.com>
3: * by Michael Pounov <misho@openbsd-bg.org>
4: *
5: * $Author: misho $
1.2 ! misho 6: * $Id: aitpelco.c,v 1.1.1.1.2.6 2011/05/10 20:25:41 misho Exp $
1.1 misho 7: *
1.2 ! misho 8: **************************************************************************
! 9: The ELWIX and AITNET software is distributed under the following
! 10: terms:
! 11:
! 12: All of the documentation and software included in the ELWIX and AITNET
! 13: Releases is copyrighted by ELWIX - Sofia/Bulgaria <info@elwix.org>
! 14:
! 15: Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011
! 16: by Michael Pounov <misho@elwix.org>. All rights reserved.
! 17:
! 18: Redistribution and use in source and binary forms, with or without
! 19: modification, are permitted provided that the following conditions
! 20: are met:
! 21: 1. Redistributions of source code must retain the above copyright
! 22: notice, this list of conditions and the following disclaimer.
! 23: 2. Redistributions in binary form must reproduce the above copyright
! 24: notice, this list of conditions and the following disclaimer in the
! 25: documentation and/or other materials provided with the distribution.
! 26: 3. All advertising materials mentioning features or use of this software
! 27: must display the following acknowledgement:
! 28: This product includes software developed by Michael Pounov <misho@elwix.org>
! 29: ELWIX - Embedded LightWeight unIX and its contributors.
! 30: 4. Neither the name of AITNET nor the names of its contributors
! 31: may be used to endorse or promote products derived from this software
! 32: without specific prior written permission.
! 33:
! 34: THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND
! 35: ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
! 36: IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
! 37: ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
! 38: FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
! 39: DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
! 40: OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
! 41: HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
! 42: LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
! 43: OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
! 44: SUCH DAMAGE.
! 45: */
1.1 misho 46: #include "global.h"
47: #include "aitpelco.h"
48:
49:
50: static int pelco_Errno;
51: static char pelco_Error[STRSIZ];
52:
53:
54: //
55: // Error maintenance functions ...
56: //
57:
58: // pelco_GetErrno() Get error code of last operation
1.2 ! misho 59: inline int
! 60: pelco_GetErrno()
1.1 misho 61: {
62: return pelco_Errno;
63: }
64:
65: // pelco_GetError() Get error text of last operation
1.2 ! misho 66: inline const char *
! 67: pelco_GetError()
1.1 misho 68: {
69: return pelco_Error;
70: }
71:
72: // pelco_SetErr() Set error to variables for internal use!!!
1.2 ! misho 73: inline void
! 74: pelcoSetErr(int eno, char *estr, ...)
1.1 misho 75: {
76: va_list lst;
77:
78: pelco_Errno = eno;
79: memset(pelco_Error, 0, STRSIZ);
80: va_start(lst, estr);
81: vsnprintf(pelco_Error, STRSIZ, estr, lst);
82: va_end(lst);
83: }
84:
85: // ----------------------------------------------------------
86:
87: /*
88: * pelcoOpen() Open packet record for camera number with Pelco version D/P
89: * @pelcoVer = Pelco protocol version Dd | Pp
90: * @camNo = Packet for camera number address
91: * return: NULL error, !=NULL ok, allocated memory for packet
92: */
1.2 ! misho 93: inline void *
! 94: pelcoOpen(u_char pelcoVer, u_char camNo)
1.1 misho 95: {
96: pelco_d_t *pd;
97: pelco_p_t *pp;
98: void *p = NULL;
99:
100: switch (pelcoVer) {
101: case 'D':
102: case 'd':
103: if (camNo < FIRST_CAM_D) {
1.2 ! misho 104: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", camNo);
1.1 misho 105: return NULL;
106: }
107:
108: pd = malloc(sizeof(pelco_d_t));
109: if (!pd) {
110: SETERR;
111: return NULL;
112: } else
113: memset(pd, 0, sizeof(pelco_d_t));
114:
115: pd->d_sync = VER_D_SYNC;
116: pd->d_cam = camNo;
117:
118: p = pd;
119: break;
120: case 'P':
121: case 'p':
122: pp = malloc(sizeof(pelco_p_t));
123: if (!pp) {
124: SETERR;
125: return NULL;
126: } else
127: memset(pp, 0, sizeof(pelco_p_t));
128:
129: pp->p_stx = VER_P_STX;
130: pp->p_cam = !camNo ? camNo : camNo - 1;
131: pp->p_etx = VER_P_ETX;
132:
133: p = pp;
134: break;
135: default:
1.2 ! misho 136: pelcoSetErr(ENOEXEC, "unsupported Pelco protocol version!\n");
1.1 misho 137: }
138:
139: return p;
140: }
141:
142: /*
143: * pelcoClose() Close packet record and free memory
144: * @p = Packet structure for close
145: */
1.2 ! misho 146: inline void
! 147: pelcoClose(void * __restrict p)
1.1 misho 148: {
149: if (p)
150: free(p);
151: }
152:
1.2 ! misho 153: /*
! 154: * pelcoLoad() Load packet from input buffer
! 155: * @buffer = Pelco packet from input buffer
! 156: * return: NULL error, !=NULL ok, allocated memory for packet
! 157: */
! 158: inline void *
! 159: pelcoLoad(u_char *buffer)
! 160: {
! 161: pelco_d_t *pd;
! 162: pelco_p_t *pp;
! 163: void *p = NULL;
! 164:
! 165: if (!buffer || !*buffer) {
! 166: pelcoSetErr(EINVAL, "invalid argument!\n");
! 167: return NULL;
! 168: }
! 169:
! 170: switch (pelco_GetVersion(buffer)) {
! 171: case 'd':
! 172: if (pelco_GetCamNo(buffer) < FIRST_CAM_D) {
! 173: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n",
! 174: pelco_GetCamNo(buffer));
! 175: return NULL;
! 176: }
! 177:
! 178: pd = malloc(sizeof(pelco_d_t));
! 179: if (!pd) {
! 180: SETERR;
! 181: return NULL;
! 182: } else
! 183: memcpy(pd, buffer, sizeof(pelco_d_t));
! 184:
! 185: p = pd;
! 186: break;
! 187: case 'p':
! 188: pp = malloc(sizeof(pelco_p_t));
! 189: if (!pp) {
! 190: SETERR;
! 191: return NULL;
! 192: } else
! 193: memcpy(pp, buffer, sizeof(pelco_p_t));
! 194:
! 195: p = pp;
! 196: break;
! 197: default:
! 198: pelcoSetErr(ENOEXEC, "unsupported Pelco protocol version!\n");
! 199: }
! 200:
! 201: return p;
! 202: }
1.1 misho 203:
204: /*
205: * pelcoAddCmdData() Add commands and datas for already opened packet
206: * @p = Input Packet structure
207: * @cmd[2] = Input Commands 1 & 2
208: * @data[2] = Input Data for commands 1 & 2
209: * return: 0xFF - error, 0 - ok
210: */
1.2 ! misho 211: inline u_char
! 212: pelcoAddCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
1.1 misho 213: {
214: u_char ret = 0;
215: pelco_d_t *pd = (pelco_d_t *) p;
216: pelco_p_t *pp = (pelco_p_t *) p;
217: u_char *ptr = p;
218:
1.2 ! misho 219: if (!p || !*ptr) {
! 220: pelcoSetErr(EINVAL, "invalid argument!\n");
1.1 misho 221: return 0xFF;
222: }
223:
224: switch (ptr[0]) {
225: case VER_D_SYNC:
226: if (ptr[1] < FIRST_CAM_D) {
1.2 ! misho 227: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", ptr[1]);
1.1 misho 228: return 0xFF;
229: }
230:
231: if (cmd)
232: memcpy(&pd->d_cmd1, cmd, 2);
233: if (data)
234: memcpy(&pd->d_data, data, 2);
235: pd->d_crc = crcPelco('d', p);
236: break;
237: case VER_P_STX:
238: if (VER_P_ETX != ptr[6]) {
1.2 ! misho 239: pelcoSetErr(ENOEXEC, "Broken Pelco P packet!\n");
1.1 misho 240: return 0xFF;
241: }
242:
243: if (cmd)
244: memcpy(&pp->p_cmd1, cmd, 2);
245: if (data)
246: memcpy(&pp->p_data, data, 2);
247: pp->p_crc = crcPelco('p', p);
248: break;
249: default:
1.2 ! misho 250: pelcoSetErr(ENOEXEC, "Invalid protocol!\n");
1.1 misho 251: return 0xFF;
252: }
253:
254: return ret;
255: }
1.2 ! misho 256:
! 257: /*
! 258: * pelcoGetCmdData() Get from packet commands and datas with verify packet
! 259: * @p = Input Packet structure
! 260: * @cmd[2] = Output Commands 1 & 2
! 261: * @data[2] = Output Data for commands 1 & 2
! 262: * return: 'd' - PelcoD, 'p' - PelcoP, 0 - unknown or bad packet
! 263: */
! 264: inline u_char
! 265: pelcoGetCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
! 266: {
! 267: return pelco_GetCamCmdData(p, NULL, cmd, data);
! 268: }
! 269:
! 270: /*
! 271: * pelcoChkSum() Check ot Correct check sum in packet
! 272: * @p = Input Packet structure
! 273: * @correct = Calculate new check sum if incorrect !=0, if ==0 only check
! 274: * return: 0xFF - bad packet, 1 invalid check sum, 0 check sum is correct.
! 275: */
! 276: inline u_char
! 277: pelcoChkSum(void * __restrict p, u_char correct)
! 278: {
! 279: u_char sum, *ptr = p;
! 280: pelco_d_t *pd = (pelco_d_t *) p;
! 281: pelco_p_t *pp = (pelco_p_t *) p;
! 282:
! 283: if (!p || !*ptr) {
! 284: pelcoSetErr(EINVAL, "invalid argument!\n");
! 285: return 0xFF;
! 286: }
! 287:
! 288: switch (ptr[0]) {
! 289: case VER_D_SYNC:
! 290: if (ptr[1] < FIRST_CAM_D) {
! 291: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", ptr[1]);
! 292: return 0xFF;
! 293: }
! 294:
! 295: sum = crcPelco('d', p);
! 296: if (correct)
! 297: pd->d_crc = sum;
! 298: break;
! 299: case VER_P_STX:
! 300: if (VER_P_ETX != ptr[6]) {
! 301: pelcoSetErr(ENOEXEC, "Broken Pelco P packet!\n");
! 302: return 0xFF;
! 303: }
! 304:
! 305: sum = crcPelco('p', p);
! 306: if (correct)
! 307: pp->p_crc = sum;
! 308: break;
! 309: default:
! 310: pelcoSetErr(ENOEXEC, "Invalid protocol!\n");
! 311: return 0xFF;
! 312: }
! 313:
! 314: return sum;
! 315: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>
![[BACK]](/icons/cvsweb/back.gif){kind=icon}
Return to aitpelco.c CVS log ![[TXT]](/icons/cvsweb/text.gif){kind=icon}
![[DIR]](/icons/cvsweb/dir.gif){kind=icon}
Up to [ELWIX - Embedded LightWeight unIX -] / libaitpelco / src