1: /*************************************************************************
2: * (C) 2010 AITNET ltd - Sofia/Bulgaria - <misho@aitbg.com>
3: * by Michael Pounov <misho@openbsd-bg.org>
4: *
5: * $Author: misho $
6: * $Id: aitpelco.c,v 1.4 2013/05/30 09:20:16 misho Exp $
7: *
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, 2012, 2013
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: */
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
59: int
60: pelco_GetErrno()
61: {
62: return pelco_Errno;
63: }
64:
65: // pelco_GetError() Get error text of last operation
66: const char *
67: pelco_GetError()
68: {
69: return pelco_Error;
70: }
71:
72: // pelco_SetErr() Set error to variables for internal use!!!
73: void
74: pelcoSetErr(int eno, char *estr, ...)
75: {
76: va_list lst;
77:
78: pelco_Errno = eno;
79: memset(pelco_Error, 0, sizeof pelco_Error);
80: va_start(lst, estr);
81: vsnprintf(pelco_Error, sizeof pelco_Error, 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: */
93: void *
94: pelcoOpen(u_char pelcoVer, u_char camNo)
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) {
104: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", camNo);
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:
136: pelcoSetErr(ENOEXEC, "unsupported Pelco protocol version!\n");
137: }
138:
139: return p;
140: }
141:
142: /*
143: * pelcoClose() Close packet record and free memory
144: * @p = Packet structure for close
145: */
146: void
147: pelcoClose(void * __restrict p)
148: {
149: if (p)
150: free(p);
151: }
152:
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: 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: }
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: */
211: u_char
212: pelcoAddCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
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:
219: if (!p || !*ptr) {
220: pelcoSetErr(EINVAL, "invalid argument!\n");
221: return 0xFF;
222: }
223:
224: switch (ptr[0]) {
225: case VER_D_SYNC:
226: if (ptr[1] < FIRST_CAM_D) {
227: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", ptr[1]);
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]) {
239: pelcoSetErr(ENOEXEC, "Broken Pelco P packet!\n");
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:
250: pelcoSetErr(ENOEXEC, "Invalid protocol!\n");
251: return 0xFF;
252: }
253:
254: return ret;
255: }
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: 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: 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