Return to aitpelco.c CVS log

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

added new feature

    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.1 2010/03/11 13:39:28 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:  * 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: }
  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: }