Return to aitpelco.c CVS log

Up to [ELWIX - Embedded LightWeight unIX -] / libaitpelco / src

libaitpelco Pelco-D/P protocol

    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: }