--- libaitpelco/src/aitpelco.c	2010/03/12 14:57:18	1.1.1.1.2.2
+++ libaitpelco/src/aitpelco.c	2010/03/12 15:30:46	1.1.1.1.2.3
@@ -3,7 +3,7 @@
 *  by Michael Pounov <misho@openbsd-bg.org>
 *
 * $Author: misho $
-* $Id: aitpelco.c,v 1.1.1.1.2.2 2010/03/12 14:57:18 misho Exp $
+* $Id: aitpelco.c,v 1.1.1.1.2.3 2010/03/12 15:30:46 misho Exp $
 *
 *************************************************************************/
 #include "global.h"
@@ -215,4 +215,50 @@ inline u_char pelcoAddCmdData(void * __restrict p, u_c
 inline u_char pelcoGetCmdData(void * __restrict p, u_char * __restrict cmd, u_char * __restrict data)
 {
 	return pelco_GetCamCmdData(p, NULL, cmd, data);
+}
+
+/*
+ * pelcoChkSum() Check ot Correct check sum in packet
+ * @p = Input Packet structure
+ * @correct = Calculate new check sum if incorrect !=0, if ==0 only check
+ * return: 0xFF - bad packet, 1 invalid check sum, 0 check sum is correct.
+ */
+inline u_char pelcoChkSum(void * __restrict p, u_char correct)
+{
+	u_char sum, *ptr = p;
+	pelco_d_t *pd = (pelco_d_t *) p;
+	pelco_p_t *pp = (pelco_p_t *) p;
+
+	if (!p) {
+		pelcoSetErr(EINVAL, "Error:: invalid argument!\n");
+		return 0xFF;
+	}
+
+	switch (ptr[0]) {
+		case VER_D_SYNC:
+			if (ptr[1] < FIRST_CAM_D) {
+				pelcoSetErr(ENOEXEC, "Error:: unsupported camera number %d!\n", ptr[1]);
+				return 0xFF;
+			}
+
+			sum = crcPelco('d', p);
+			if (correct)
+				pd->d_crc = sum;
+			break;
+		case VER_P_STX:
+			if (VER_P_ETX != ptr[6]) {
+				pelcoSetErr(ENOEXEC, "Error:: Broken Pelco P packet!\n");
+				return 0xFF;
+			}
+
+			sum = crcPelco('p', p);
+			if (correct)
+				pp->p_crc = sum;
+			break;
+		default:
+			pelcoSetErr(ENOEXEC, "Error:: Invalid protocol!\n");
+			return 0xFF;
+	}
+
+	return sum;
 }