Annotation of libaitpelco/src/aitpelco.c, revision 1.1.1.1.2.3
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.1.1.1.2.3! misho 6: * $Id: aitpelco.c,v 1.1.1.1.2.2 2010/03/12 14:57:18 misho Exp $
1.1 misho 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, "Error:: 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, "Error:: 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:
1.1.1.1.2.1 misho 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: switch (pelco_GetVersion(buffer)) {
123: case 'd':
124: if (pelco_GetCamNo(buffer) < FIRST_CAM_D) {
125: pelcoSetErr(ENOEXEC, "Error:: unsupported camera number %d!\n",
126: pelco_GetCamNo(buffer));
127: return NULL;
128: }
129:
130: pd = malloc(sizeof(pelco_d_t));
131: if (!pd) {
132: SETERR;
133: return NULL;
134: } else
135: memcpy(pd, buffer, sizeof(pelco_d_t));
136:
137: p = pd;
138: break;
139: case 'p':
140: pp = malloc(sizeof(pelco_p_t));
141: if (!pp) {
142: SETERR;
143: return NULL;
144: } else
145: memcpy(pp, buffer, sizeof(pelco_p_t));
146:
147: p = pp;
148: break;
149: default:
150: pelcoSetErr(ENOEXEC, "Error:: unsupported Pelco protocol version!\n");
151: }
152:
153: return p;
154: }
1.1 misho 155:
156: /*
157: * pelcoAddCmdData() Add commands and datas for already opened packet
158: * @p = Input Packet structure
159: * @cmd[2] = Input Commands 1 & 2
160: * @data[2] = Input Data for commands 1 & 2
161: * return: 0xFF - error, 0 - ok
162: */
163: inline u_char pelcoAddCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
164: {
165: u_char ret = 0;
166: pelco_d_t *pd = (pelco_d_t *) p;
167: pelco_p_t *pp = (pelco_p_t *) p;
168: u_char *ptr = p;
169:
170: if (!p) {
171: pelcoSetErr(EINVAL, "Error:: invalid argument!\n");
172: return 0xFF;
173: }
174:
175: switch (ptr[0]) {
176: case VER_D_SYNC:
177: if (ptr[1] < FIRST_CAM_D) {
178: pelcoSetErr(ENOEXEC, "Error:: unsupported camera number %d!\n", ptr[1]);
179: return 0xFF;
180: }
181:
182: if (cmd)
183: memcpy(&pd->d_cmd1, cmd, 2);
184: if (data)
185: memcpy(&pd->d_data, data, 2);
186: pd->d_crc = crcPelco('d', p);
187: break;
188: case VER_P_STX:
189: if (VER_P_ETX != ptr[6]) {
190: pelcoSetErr(ENOEXEC, "Error:: Broken Pelco P packet!\n");
191: return 0xFF;
192: }
193:
194: if (cmd)
195: memcpy(&pp->p_cmd1, cmd, 2);
196: if (data)
197: memcpy(&pp->p_data, data, 2);
198: pp->p_crc = crcPelco('p', p);
199: break;
200: default:
201: pelcoSetErr(ENOEXEC, "Error:: Invalid protocol!\n");
202: return 0xFF;
203: }
204:
205: return ret;
206: }
1.1.1.1.2.2 misho 207:
208: /*
209: * pelcoGetCmdData() Get from packet commands and datas with verify packet
210: * @p = Input Packet structure
211: * @cmd[2] = Output Commands 1 & 2
212: * @data[2] = Output Data for commands 1 & 2
213: * return: 'd' - PelcoD, 'p' - PelcoP, 0 - unknown or bad packet
214: */
215: inline u_char pelcoGetCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
216: {
217: return pelco_GetCamCmdData(p, NULL, cmd, data);
218: }
1.1.1.1.2.3! misho 219:
! 220: /*
! 221: * pelcoChkSum() Check ot Correct check sum in packet
! 222: * @p = Input Packet structure
! 223: * @correct = Calculate new check sum if incorrect !=0, if ==0 only check
! 224: * return: 0xFF - bad packet, 1 invalid check sum, 0 check sum is correct.
! 225: */
! 226: inline u_char pelcoChkSum(void * __restrict p, u_char correct)
! 227: {
! 228: u_char sum, *ptr = p;
! 229: pelco_d_t *pd = (pelco_d_t *) p;
! 230: pelco_p_t *pp = (pelco_p_t *) p;
! 231:
! 232: if (!p) {
! 233: pelcoSetErr(EINVAL, "Error:: invalid argument!\n");
! 234: return 0xFF;
! 235: }
! 236:
! 237: switch (ptr[0]) {
! 238: case VER_D_SYNC:
! 239: if (ptr[1] < FIRST_CAM_D) {
! 240: pelcoSetErr(ENOEXEC, "Error:: unsupported camera number %d!\n", ptr[1]);
! 241: return 0xFF;
! 242: }
! 243:
! 244: sum = crcPelco('d', p);
! 245: if (correct)
! 246: pd->d_crc = sum;
! 247: break;
! 248: case VER_P_STX:
! 249: if (VER_P_ETX != ptr[6]) {
! 250: pelcoSetErr(ENOEXEC, "Error:: Broken Pelco P packet!\n");
! 251: return 0xFF;
! 252: }
! 253:
! 254: sum = crcPelco('p', p);
! 255: if (correct)
! 256: pp->p_crc = sum;
! 257: break;
! 258: default:
! 259: pelcoSetErr(ENOEXEC, "Error:: Invalid protocol!\n");
! 260: return 0xFF;
! 261: }
! 262:
! 263: return sum;
! 264: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>