Annotation of libaitpelco/src/aitpelco.c, revision 1.1
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 $
! 6: * $Id: defs.h,v 1.1.1.1 2008/11/05 17:02:55 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>