Fix sizeof args bug & cleanup

Turns out uint8_t is passed by reference by default since it's just a
typedef for char. As a result the length of args cannot be computed in
runCommand. The caller is now responsible for computing the number of
args and then passing them to runCommand

openCameraFd now returns the result and writes the file descriptor in
the fd pointer.

Fix an inverted if statement in runComamnd

Fix inverted read logic in snapShotToFile function

2 files changed, 71 insertions, 67 deletions

diff --git a/camera.c b/camera.c
index a50bc36..b838500 100644
--- a/camera.c
+++ b/camera.c
@@ -1,55 +1,48 @@
 #include "legato.h"
 #include "interfaces.h"
 #include "camera.h"
-#include <time.h>
 
 COMPONENT_INIT { }
 
-// Used to wait for the serial port
-void delay (unsigned int msecs) {
-  unsigned int secs = msecs / 1000;
-  unsigned int retTime = time(0) + secs;
-  while (time(0) < retTime);
+// File descriptor is returned in the fd pointer
+le_result_t openCameraFd (char *path, int *fd, tty_Speed_t baud, int nBytes, int timeout) {
+  *fd = le_tty_Open(path, O_RDWR);
+  le_result_t baudRes = le_tty_SetBaudRate(*fd, baud);
+  le_result_t rawRes = le_tty_SetRaw(*fd, nBytes, timeout);
+  return baudRes == LE_OK && rawRes == LE_OK ? LE_OK : LE_FAULT;
 }
 
-// Returns a file descriptor that can be used to
-// talk to the camera
-int openCameraFd (int nBytes, int timeout) {
-  int fd = le_tty_Open(SERIAL_PATH, O_RDWR);
-  le_tty_SetBaudRate(fd, LE_TTY_SPEED_38400);
-  le_tty_SetRaw(fd, nBytes, timeout);
-  return fd;
-}
-
-// Send a command to the camera serial port
-void sendCommand (Camera *cam, uint8_t cmd, uint8_t args[]) {
-  uint8_t nArgs = sizeof(args);
+ssize_t sendCommand (Camera *cam, uint8_t cmd, uint8_t args[], unsigned int nArgs) {
+  LE_INFO("sizeof args: %d", nArgs);
   uint8_t toWrite[100] = { VC0706_PREFIX, cam->serialNum, cmd };
   int start = 3;
   int end = nArgs + start;
   for (int i = start; i < end; i++) {
     toWrite[i] = args[i - start];
   };
-  write(cam->fd, &toWrite[0], end);
+  return write(cam->fd, &toWrite[0], end);
 }
 
-// Run a command
-bool runCommand (Camera *cam, uint8_t cmd, uint8_t args[], int respLen, bool flushFlag) {
+bool runCommand (Camera *cam, uint8_t cmd, uint8_t args[], unsigned int nArgs, int respLen, bool flushFlag) {
   // flush out the buffer
-  if (flushFlag) readResponse(cam, 100, 10);
-  sendCommand(cam, cmd, args);
-  if (readResponse(cam, respLen, TIMEOUT) != respLen)
+  if (flushFlag) {
+    cam->bufferLen = 0;
+    fsync(cam->fd);
+  }
+  sendCommand(cam, cmd, args, nArgs);
+  if (readResponse(cam, respLen, CAM_TIMEOUT) != respLen)
     return false;
-  if (verifyResponse(cam, cmd))
+  if (!verifyResponse(cam, cmd))
     return false;
   return true;
 }
 
-bool runCommandFlush (Camera *cam, uint8_t cmd, uint8_t args[], int respLen) {
-  return runCommand (cam, cmd, args, respLen, true);
+bool runCommandFlush (Camera *cam, uint8_t cmd, uint8_t args[], unsigned int nArgs, int respLen) {
+  return runCommand (cam, cmd, args, nArgs, respLen, true);
 }
 
 // Reads from the camera and returns how many bytes it read
+// TODO kill off un-needed timeout
 uint8_t readResponse (Camera *cam, unsigned int nBytes, unsigned int timeout) {
   cam->bufferLen = 0;
   uint8_t *buffPtr = &(cam->buff[0]);
@@ -64,13 +57,17 @@ uint8_t readResponse (Camera *cam, unsigned int nBytes, unsigned int timeout) {
 void printBuffer (Camera *cam) {
   LE_INFO("Printing cam buffer");
   for(int i = 0; i < NUM_ARRAY_MEMBERS(cam->buff); i++) {
-    LE_INFO("0x%d", cam->buff[i]);
+    LE_INFO("buff[%d]=%d", i, cam->buff[i]);
   }
 }
 
 bool verifyResponse (Camera *cam, uint8_t cmd) {
   // If any of these are not equal than
   // the command failed
+  LE_INFO("buff[0] correct? %d", cam->buff[0] == VC0706_RESP_PREFIX);
+  LE_INFO("buff[1] correct? %d", cam->buff[1] == cam->serialNum);
+  LE_INFO("buff[2] correct? %d", cam->buff[2] == cmd);
+  LE_INFO("buff[3] correct? %d", cam->buff[3] == 0x0);
   return
     cam->buff[0] == VC0706_RESP_PREFIX &&
     cam->buff[1] == cam->serialNum &&
@@ -80,7 +77,7 @@ bool verifyResponse (Camera *cam, uint8_t cmd) {
 
 bool cameraFrameBuffCtrl (Camera *cam, uint8_t cmd) {
   uint8_t args[] = { 0x1, cmd };
-  return runCommandFlush(cam, VC0706_FBUF_CTRL, args, 5);
+  return runCommandFlush(cam, VC0706_FBUF_CTRL, args, sizeof(args), 5);
 }
 
 bool takePicture (Camera *cam) {
@@ -90,27 +87,28 @@ bool takePicture (Camera *cam) {
 
 bool reset (Camera *cam) {
   uint8_t args[] = { 0x0 };
-  return runCommandFlush(cam, VC0706_RESET, args, 5);
+  return runCommandFlush(cam, VC0706_RESET, args, sizeof(args), 5);
 }
 
 bool TVon (Camera *cam) {
   uint8_t args[] = { 0x1, 0x1 };
-  return runCommandFlush(cam, VC0706_TVOUT_CTRL, args, 5);
+  return runCommandFlush(cam, VC0706_TVOUT_CTRL, args, sizeof(args), 5);
 }
 
 bool TVOff (Camera *cam) {
   uint8_t args[] = { 0x1, 0x0 };
-  return runCommandFlush(cam, VC0706_TVOUT_CTRL, args, 5);
+  return runCommandFlush(cam, VC0706_TVOUT_CTRL, args, sizeof(args), 5);
 }
 
-uint8_t *readPicture (Camera *cam, uint8_t n) {
+uint8_t* readPicture (Camera *cam, uint8_t n) {
   uint8_t args[] = { 0x0C, 0x0, 0x0A,
     0, 0, cam->frameptr >> 8, cam->frameptr & 0xFF,
     0, 0, 0, n,
-    DELAY >> 8, DELAY & 0xFF };
-  if (!runCommand(cam, VC0706_READ_FBUF, args, 5, false)) // don't flush
+    CAM_DELAY >> 8, CAM_DELAY & 0xFF };
+  LE_INFO("Got dat frameptr: %d", cam->frameptr);
+  if (!runCommand(cam, VC0706_READ_FBUF, args, sizeof(args), 5, false)) // don't flush
     return 0;
-  if (readResponse(cam, n + 5, DELAY) == 0)
+  if (readResponse(cam, n + 5, CAM_DELAY) == 0)
     return 0;
 
   cam->frameptr += n;
@@ -125,7 +123,7 @@ bool resumeVideo (Camera *cam) {
 uint32_t frameLength (Camera *cam) {
   uint8_t args[] = { 0x01, 0x00 };
   // return 0 if this fails
-  if (!runCommandFlush(cam, VC0706_GET_FBUF_LEN, args, 9))
+  if (!runCommandFlush(cam, VC0706_GET_FBUF_LEN, args, sizeof(args), 9))
     return 0;
 
   uint32_t len;
@@ -140,10 +138,10 @@ uint32_t frameLength (Camera *cam) {
   return len;
 }
 
-char *getVersion (Camera *cam) {
+char* getVersion (Camera *cam) {
   uint8_t args[] = { 0x01 };
-  sendCommand(cam, VC0706_GEN_VERSION, args);
-  if (!readResponse(cam, BUFF_SIZE, 200)) {
+  sendCommand(cam, VC0706_GEN_VERSION, args, sizeof(args));
+  if (!readResponse(cam, CAM_BUFF_SIZE, 200)) {
     LE_INFO("Failed to get version, returning empty string");
     return 0;
   }
@@ -157,38 +155,38 @@ uint8_t available (Camera *cam) {
 
 uint8_t getDownsize (Camera *cam) {
   uint8_t args[] = { 0x0 };
-  if (!runCommandFlush(cam, VC0706_DOWNSIZE_STATUS, args, 6))
+  if (!runCommandFlush(cam, VC0706_DOWNSIZE_STATUS, args, sizeof(args), 6))
     return -1;
   return cam->buff[5];
 }
 
 bool setDownsize (Camera *cam, uint8_t newSize) {
   uint8_t args[] = { 0x01, newSize };
-  return runCommandFlush(cam, VC0706_DOWNSIZE_CTRL, args, 5);
+  return runCommandFlush(cam, VC0706_DOWNSIZE_CTRL, args, sizeof(args), 5);
 }
 
 uint8_t getImageSize (Camera *cam) {
   uint8_t args[] = { 0x4, 0x4, 0x1, 0x00, 0x19 };
-  if (!runCommandFlush(cam, VC0706_READ_DATA, args, 6))
+  if (!runCommandFlush(cam, VC0706_READ_DATA, args, sizeof(args), 6))
     return -1;
   return cam->buff[5];
 }
 
 bool setImageSize (Camera *cam, uint8_t x) {
   uint8_t args[] = { 0x05, 0x04, 0x01, 0x00, 0x19, x };
-  return runCommandFlush(cam, VC0706_WRITE_DATA, args, 5);
+  return runCommandFlush(cam, VC0706_WRITE_DATA, args, sizeof(args), 5);
 }
 
 bool getMotionDetect (Camera *cam) {
   uint8_t args[] = { 0x0 };
-  if (!runCommandFlush(cam, VC0706_COMM_MOTION_STATUS, args, 6))
+  if (!runCommandFlush(cam, VC0706_COMM_MOTION_STATUS, args, sizeof(args), 6))
     return false;
   return cam->buff[5];
 }
 
 uint8_t getMotionStatus(Camera *cam, uint8_t x) {
   uint8_t args[] = { 0x01, x };
-  return runCommandFlush(cam, VC0706_MOTION_STATUS, args, 5);
+  return runCommandFlush(cam, VC0706_MOTION_STATUS, args, sizeof(args), 5);
 }
 
 bool motionDetected (Camera *cam) {
@@ -203,29 +201,29 @@ bool setMotionDetect (Camera *cam, bool flag) {
   if (!setMotionStatus(cam, VC0706_MOTIONCONTROL, VC0706_UARTMOTION, VC0706_ACTIVATEMOTION))
     return false;
   uint8_t args[] = { 0x1, flag };
-  return runCommandFlush(cam, VC0706_MOTION_STATUS, args, 5);
+  return runCommandFlush(cam, VC0706_MOTION_STATUS, args, sizeof(args), 5);
 }
 
 bool setMotionStatus (Camera *cam, uint8_t x, uint8_t d1, uint8_t d2) {
   uint8_t args[] = { 0x03, x, d1, d2 };
-  return runCommandFlush(cam, VC0706_MOTION_CTRL, args, 5);
+  return runCommandFlush(cam, VC0706_MOTION_CTRL, args, sizeof(args), 5);
 }
 
 uint8_t getCompression (Camera *cam) {
   uint8_t args[] = { 0x4, 0x1, 0x1, 0x12, 0x04 };
-  runCommandFlush(cam, VC0706_READ_DATA, args, 6);
+  runCommandFlush(cam, VC0706_READ_DATA, args, sizeof(args), 6);
   return cam->buff[5];
 }
 
 bool setCompression (Camera *cam, uint8_t c) {
   uint8_t args[] = { 0x5, 0x1, 0x1, 0x12, 0x04, c };
-  return runCommandFlush(cam, VC0706_WRITE_DATA, args, 5);
+  return runCommandFlush(cam, VC0706_WRITE_DATA, args, sizeof(args), 5);
 }
 
 bool getPTZ(Camera *cam, uint16_t *w, uint16_t *h, uint16_t *wz, uint16_t *hz, uint16_t *pan, uint16_t *tilt) {
   uint8_t args[] ={ 0x0 };
 
-  if (!runCommandFlush(cam, VC0706_GET_ZOOM, args, 16))
+  if (!runCommandFlush(cam, VC0706_GET_ZOOM, args, sizeof(args), 16))
     return false;
   *w = cam->buff[5];
   *w <<= 8;
@@ -254,14 +252,14 @@ bool getPTZ(Camera *cam, uint16_t *w, uint16_t *h, uint16_t *wz, uint16_t *hz, u
   return true;
 }
 
-bool setPTZ(Camera *cam, uint16_t wz, uint16_t hz, uint16_t pan, uint16_t tilt) {
+bool setPTZ (Camera *cam, uint16_t wz, uint16_t hz, uint16_t pan, uint16_t tilt) {
   uint8_t args[] = {
     0x08, wz >> 8, wz,
     hz >> 8, wz,
-    pan>>8, pan,
-    tilt>>8, tilt
+    pan >> 8, pan,
+    tilt >> 8, tilt
   };
-  return !runCommandFlush(cam, VC0706_SET_ZOOM, args, 5);
+  return !runCommandFlush(cam, VC0706_SET_ZOOM, args, sizeof(args), 5);
 }
 
 bool snapshotToFile (Camera *cam, char *path, uint8_t imgSize) {
@@ -278,10 +276,13 @@ bool snapshotToFile (Camera *cam, char *path, uint8_t imgSize) {
     if (filePtr != NULL) {
       LE_INFO("Got valid file pointer");
       int jpgLen = frameLength(cam);
+      LE_INFO("jpgLen: %d", jpgLen);
       while (jpgLen > 0) {
         uint8_t *buff;
-        uint8_t bytesToRead = 32 < jpgLen ? jpgLen : 32;
+        uint8_t bytesToRead = 32 < jpgLen ? 32 : jpgLen;
+        LE_INFO("jpgLen: %d, bytesToRead: %d", jpgLen, bytesToRead);
         buff = readPicture(cam, bytesToRead);
+        LE_INFO("Made it past a photo data read");
         fwrite(buff, sizeof(*buff), bytesToRead, filePtr);
         jpgLen -= bytesToRead;
       }
diff --git a/camera.h b/camera.h
index 7c6c46b..2595d78 100644
--- a/camera.h
+++ b/camera.h
@@ -1,3 +1,5 @@
+#ifndef CAMERA_H
+#define CAMERA_H
 #define VC0706_RESP_PREFIX 0x76
 #define VC0706_PREFIX 0x56
 #define VC0706_RESET  0x26
@@ -34,9 +36,9 @@
 #define VC0706_SET_ZOOM 0x52
 #define VC0706_GET_ZOOM 0x53
 
-#define BUFF_SIZE 100
-#define DELAY 10
-#define TIMEOUT 20
+#define CAM_BUFF_SIZE 100
+#define CAM_DELAY 10
+#define CAM_TIMEOUT 10
 #define CAM_SERIAL 0
 
 static const char SERIAL_PATH[] = "/dev/ttyHS0";
@@ -44,16 +46,15 @@ static const char SERIAL_PATH[] = "/dev/ttyHS0";
 typedef struct {
   int fd; // file descriptor for the serial port
   uint8_t serialNum; // camera serial number
-  uint8_t buff[BUFF_SIZE]; // uint8_t array to store camera data
+  uint8_t buff[CAM_BUFF_SIZE]; // uint8_t array to store camera data
   uint8_t bufferLen; // current length of data in buffer
   uint16_t frameptr;
 } Camera;
 
-void delay (unsigned int msecs);
-LE_SHARED int openCameraFd ();
-LE_SHARED void sendCommand (Camera *cam, uint8_t cmd, uint8_t args[]);
-LE_SHARED bool runCommand (Camera *cam, uint8_t cmd, uint8_t args[], int respLen, bool flushFlag);
-LE_SHARED bool runCommandFlush (Camera *cam, uint8_t cmd, uint8_t args[], int respLen);
+LE_SHARED le_result_t openCameraFd ();
+LE_SHARED ssize_t sendCommand (Camera *cam, uint8_t cmd, uint8_t args[], unsigned int nArgs);
+LE_SHARED bool runCommand (Camera *cam, uint8_t cmd, uint8_t args[], unsigned int nArgs, int respLen, bool flushFlag);
+LE_SHARED bool runCommandFlush (Camera *cam, uint8_t cmd, uint8_t args[], unsigned int nArgs, int respLen);
 LE_SHARED uint8_t readResponse (Camera *cam, unsigned int nBytes, unsigned int timeout);
 LE_SHARED void printBuffer (Camera *cam);
 LE_SHARED bool verifyResponse (Camera *cam, uint8_t cmd);
@@ -62,10 +63,10 @@ LE_SHARED bool takePicture (Camera *cam);
 LE_SHARED bool reset (Camera *cam);
 LE_SHARED bool TVon (Camera *cam);
 LE_SHARED bool TVOff (Camera *cam);
-LE_SHARED uint8_t *readPicture (Camera *cam, uint8_t n);
+LE_SHARED uint8_t* readPicture (Camera *cam, uint8_t n);
 LE_SHARED bool resumeVideo (Camera *cam);
 LE_SHARED uint32_t frameLength (Camera *cam);
-LE_SHARED char *getVersion (Camera *cam);
+LE_SHARED char* getVersion (Camera *cam);
 LE_SHARED uint8_t available (Camera *cam);
 LE_SHARED uint8_t getDownsize (Camera *cam);
 LE_SHARED bool setDownsize(Camera *cam, uint8_t newSize);
@@ -81,3 +82,5 @@ LE_SHARED bool setCompression(Camera *cam, uint8_t c);
 LE_SHARED bool getPTZ(Camera *cam, uint16_t *w, uint16_t *h, uint16_t *wz, uint16_t *hz, uint16_t *pan, uint16_t *tilt);
 LE_SHARED bool setPTZ(Camera *cam, uint16_t wz, uint16_t hz, uint16_t pan, uint16_t tilt);
 LE_SHARED bool snapshotToFile (Camera *cam, char *path, uint8_t imgSize);
+
+#endif