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 2010/01/22 00:57:32 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, "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:
111:
112: /*
113: * pelcoAddCmdData() Add commands and datas for already opened packet
114: * @p = Input Packet structure
115: * @cmd[2] = Input Commands 1 & 2
116: * @data[2] = Input Data for commands 1 & 2
117: * return: 0xFF - error, 0 - ok
118: */
119: inline u_char pelcoAddCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
120: {
121: u_char ret = 0;
122: pelco_d_t *pd = (pelco_d_t *) p;
123: pelco_p_t *pp = (pelco_p_t *) p;
124: u_char *ptr = p;
125:
126: if (!p) {
127: pelcoSetErr(EINVAL, "Error:: invalid argument!\n");
128: return 0xFF;
129: }
130:
131: switch (ptr[0]) {
132: case VER_D_SYNC:
133: if (ptr[1] < FIRST_CAM_D) {
134: pelcoSetErr(ENOEXEC, "Error:: unsupported camera number %d!\n", ptr[1]);
135: return 0xFF;
136: }
137:
138: if (cmd)
139: memcpy(&pd->d_cmd1, cmd, 2);
140: if (data)
141: memcpy(&pd->d_data, data, 2);
142: pd->d_crc = crcPelco('d', p);
143: break;
144: case VER_P_STX:
145: if (VER_P_ETX != ptr[6]) {
146: pelcoSetErr(ENOEXEC, "Error:: Broken Pelco P packet!\n");
147: return 0xFF;
148: }
149:
150: if (cmd)
151: memcpy(&pp->p_cmd1, cmd, 2);
152: if (data)
153: memcpy(&pp->p_data, data, 2);
154: pp->p_crc = crcPelco('p', p);
155: break;
156: default:
157: pelcoSetErr(ENOEXEC, "Error:: Invalid protocol!\n");
158: return 0xFF;
159: }
160:
161: return ret;
162: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>