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.1.1.1.2.5 2010/03/23 01:25:43 misho Exp $
7: *
8: *************************************************************************/
9: #include "global.h"
10: #include "aitpelco.h"
11:
12:
13: static int pelco_Errno;
14: static char pelco_Error[STRSIZ];
15:
16:
17: //
18: // Error maintenance functions ...
19: //
20:
21: // pelco_GetErrno() Get error code of last operation
22: inline int pelco_GetErrno()
23: {
24: return pelco_Errno;
25: }
26:
27: // pelco_GetError() Get error text of last operation
28: inline const char *pelco_GetError()
29: {
30: return pelco_Error;
31: }
32:
33: // pelco_SetErr() Set error to variables for internal use!!!
34: inline void pelcoSetErr(int eno, char *estr, ...)
35: {
36: va_list lst;
37:
38: pelco_Errno = eno;
39: memset(pelco_Error, 0, STRSIZ);
40: va_start(lst, estr);
41: vsnprintf(pelco_Error, STRSIZ, estr, lst);
42: va_end(lst);
43: }
44:
45: // ----------------------------------------------------------
46:
47: /*
48: * pelcoOpen() Open packet record for camera number with Pelco version D/P
49: * @pelcoVer = Pelco protocol version Dd | Pp
50: * @camNo = Packet for camera number address
51: * return: NULL error, !=NULL ok, allocated memory for packet
52: */
53: inline void *pelcoOpen(u_char pelcoVer, u_char camNo)
54: {
55: pelco_d_t *pd;
56: pelco_p_t *pp;
57: void *p = NULL;
58:
59: switch (pelcoVer) {
60: case 'D':
61: case 'd':
62: if (camNo < FIRST_CAM_D) {
63: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", camNo);
64: return NULL;
65: }
66:
67: pd = malloc(sizeof(pelco_d_t));
68: if (!pd) {
69: SETERR;
70: return NULL;
71: } else
72: memset(pd, 0, sizeof(pelco_d_t));
73:
74: pd->d_sync = VER_D_SYNC;
75: pd->d_cam = camNo;
76:
77: p = pd;
78: break;
79: case 'P':
80: case 'p':
81: pp = malloc(sizeof(pelco_p_t));
82: if (!pp) {
83: SETERR;
84: return NULL;
85: } else
86: memset(pp, 0, sizeof(pelco_p_t));
87:
88: pp->p_stx = VER_P_STX;
89: pp->p_cam = !camNo ? camNo : camNo - 1;
90: pp->p_etx = VER_P_ETX;
91:
92: p = pp;
93: break;
94: default:
95: pelcoSetErr(ENOEXEC, "unsupported Pelco protocol version!\n");
96: }
97:
98: return p;
99: }
100:
101: /*
102: * pelcoClose() Close packet record and free memory
103: * @p = Packet structure for close
104: */
105: inline void pelcoClose(void * __restrict p)
106: {
107: if (p)
108: free(p);
109: }
110:
111: /*
112: * pelcoLoad() Load packet from input buffer
113: * @buffer = Pelco packet from input buffer
114: * return: NULL error, !=NULL ok, allocated memory for packet
115: */
116: inline void *pelcoLoad(u_char *buffer)
117: {
118: pelco_d_t *pd;
119: pelco_p_t *pp;
120: void *p = NULL;
121:
122: if (!buffer || !*buffer) {
123: pelcoSetErr(EINVAL, "invalid argument!\n");
124: return NULL;
125: }
126:
127: switch (pelco_GetVersion(buffer)) {
128: case 'd':
129: if (pelco_GetCamNo(buffer) < FIRST_CAM_D) {
130: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n",
131: pelco_GetCamNo(buffer));
132: return NULL;
133: }
134:
135: pd = malloc(sizeof(pelco_d_t));
136: if (!pd) {
137: SETERR;
138: return NULL;
139: } else
140: memcpy(pd, buffer, sizeof(pelco_d_t));
141:
142: p = pd;
143: break;
144: case 'p':
145: pp = malloc(sizeof(pelco_p_t));
146: if (!pp) {
147: SETERR;
148: return NULL;
149: } else
150: memcpy(pp, buffer, sizeof(pelco_p_t));
151:
152: p = pp;
153: break;
154: default:
155: pelcoSetErr(ENOEXEC, "unsupported Pelco protocol version!\n");
156: }
157:
158: return p;
159: }
160:
161: /*
162: * pelcoAddCmdData() Add commands and datas for already opened packet
163: * @p = Input Packet structure
164: * @cmd[2] = Input Commands 1 & 2
165: * @data[2] = Input Data for commands 1 & 2
166: * return: 0xFF - error, 0 - ok
167: */
168: inline u_char pelcoAddCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
169: {
170: u_char ret = 0;
171: pelco_d_t *pd = (pelco_d_t *) p;
172: pelco_p_t *pp = (pelco_p_t *) p;
173: u_char *ptr = p;
174:
175: if (!p || !*ptr) {
176: pelcoSetErr(EINVAL, "invalid argument!\n");
177: return 0xFF;
178: }
179:
180: switch (ptr[0]) {
181: case VER_D_SYNC:
182: if (ptr[1] < FIRST_CAM_D) {
183: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", ptr[1]);
184: return 0xFF;
185: }
186:
187: if (cmd)
188: memcpy(&pd->d_cmd1, cmd, 2);
189: if (data)
190: memcpy(&pd->d_data, data, 2);
191: pd->d_crc = crcPelco('d', p);
192: break;
193: case VER_P_STX:
194: if (VER_P_ETX != ptr[6]) {
195: pelcoSetErr(ENOEXEC, "Broken Pelco P packet!\n");
196: return 0xFF;
197: }
198:
199: if (cmd)
200: memcpy(&pp->p_cmd1, cmd, 2);
201: if (data)
202: memcpy(&pp->p_data, data, 2);
203: pp->p_crc = crcPelco('p', p);
204: break;
205: default:
206: pelcoSetErr(ENOEXEC, "Invalid protocol!\n");
207: return 0xFF;
208: }
209:
210: return ret;
211: }
212:
213: /*
214: * pelcoGetCmdData() Get from packet commands and datas with verify packet
215: * @p = Input Packet structure
216: * @cmd[2] = Output Commands 1 & 2
217: * @data[2] = Output Data for commands 1 & 2
218: * return: 'd' - PelcoD, 'p' - PelcoP, 0 - unknown or bad packet
219: */
220: inline u_char pelcoGetCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
221: {
222: return pelco_GetCamCmdData(p, NULL, cmd, data);
223: }
224:
225: /*
226: * pelcoChkSum() Check ot Correct check sum in packet
227: * @p = Input Packet structure
228: * @correct = Calculate new check sum if incorrect !=0, if ==0 only check
229: * return: 0xFF - bad packet, 1 invalid check sum, 0 check sum is correct.
230: */
231: inline u_char pelcoChkSum(void * __restrict p, u_char correct)
232: {
233: u_char sum, *ptr = p;
234: pelco_d_t *pd = (pelco_d_t *) p;
235: pelco_p_t *pp = (pelco_p_t *) p;
236:
237: if (!p || !*ptr) {
238: pelcoSetErr(EINVAL, "invalid argument!\n");
239: return 0xFF;
240: }
241:
242: switch (ptr[0]) {
243: case VER_D_SYNC:
244: if (ptr[1] < FIRST_CAM_D) {
245: pelcoSetErr(ENOEXEC, "unsupported camera number %d!\n", ptr[1]);
246: return 0xFF;
247: }
248:
249: sum = crcPelco('d', p);
250: if (correct)
251: pd->d_crc = sum;
252: break;
253: case VER_P_STX:
254: if (VER_P_ETX != ptr[6]) {
255: pelcoSetErr(ENOEXEC, "Broken Pelco P packet!\n");
256: return 0xFF;
257: }
258:
259: sum = crcPelco('p', p);
260: if (correct)
261: pp->p_crc = sum;
262: break;
263: default:
264: pelcoSetErr(ENOEXEC, "Invalid protocol!\n");
265: return 0xFF;
266: }
267:
268: return sum;
269: }
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