Return to aitpelco.c CVS log

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

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