gencmd: Add a fallback to mailbox interface if vchiq is not available

host_applications/linux/apps/gencmd/gencmd.c

@@ -33,10 +33,115 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <stdarg.h>
 #include <string.h>
 #include <time.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>		/* ioctl */
 
 #include "interface/vmcs_host/vc_vchi_gencmd.h"
 #include "interface/vmcs_host/vc_gencmd_defs.h"
 
+#define DEVICE_FILE_NAME "/dev/vcio"
+#define MAJOR_NUM 100
+#define IOCTL_MBOX_PROPERTY _IOWR(MAJOR_NUM, 0, char *)
+
+#define MAX_STRING 1024
+
+/*
+ * use ioctl to send mbox property message
+ */
+
+static int mbox_property(int file_desc, void *buf)
+{
+   int ret_val = ioctl(file_desc, IOCTL_MBOX_PROPERTY, buf);
+
+   if (ret_val < 0) {
+      printf("ioctl_set_msg failed:%d\n", ret_val);
+   }
+   return ret_val;
+}
+
+
+static int mbox_open()
+{
+   int file_desc;
+
+   // open a char device file used for communicating with kernel mbox driver
+   file_desc = open(DEVICE_FILE_NAME, 0);
+   if (file_desc < 0) {
+      printf("Can't open device file: %s\n", DEVICE_FILE_NAME);
+      printf("Try creating a device file with: sudo mknod %s c %d 0\n", DEVICE_FILE_NAME, MAJOR_NUM);
+      exit(-1);
+   }
+   return file_desc;
+}
+
+static void mbox_close(int file_desc) {
+  close(file_desc);
+}
+
+
+#define GET_GENCMD_RESULT 0x00030080
+
+unsigned gencmd(int file_desc, const char *command, char *result, int result_len)
+{
+   int i=0;
+   unsigned p[(MAX_STRING>>2) + 7];
+   int len = strlen(command);
+   // maximum length for command or response
+   if (len + 1 >= MAX_STRING)
+   {
+     fprintf(stderr, "gencmd length too long : %d\n", len);
+     return -1;
+   }
+   p[i++] = 0; // size
+   p[i++] = 0x00000000; // process request
+
+   p[i++] = GET_GENCMD_RESULT; // (the tag id)
+   p[i++] = MAX_STRING;// buffer_len
+   p[i++] = 0; // request_len (set to response length)
+   p[i++] = 0; // error repsonse
+
+   memcpy(p+i, command, len + 1);
+   i += MAX_STRING >> 2;
+
+   p[i++] = 0x00000000; // end tag
+   p[0] = i*sizeof *p; // actual size
+
+   mbox_property(file_desc, p);
+   result[0] = 0;
+   strncat(result, (const char *)(p+6), result_len);
+
+   return p[5];
+}
+
+int main_mbox(int argc, char *argv[])
+{
+  int mb = mbox_open();
+  int i;
+  char command[MAX_STRING] = {};
+  char result[MAX_STRING] = {};
+
+  for (i = 1; i < argc; i++)
+  {
+    char *c = command + strlen(command);
+    if (c > command)
+    {
+       strncat(c, " ", command + sizeof command - c);
+       c = command + strlen(command);
+    }
+    strncat(c, argv[i], command + sizeof command - c);
+  }
+
+  int ret = gencmd(mb, command, result, sizeof result);
+  if (ret)
+    printf( "vc_gencmd_read_response returned %d\n", ret );
+
+  mbox_close(mb);
+
+  printf("%s\n", result);
+  return ret;
+}
+
+
 void show_usage()
 {
    puts( "Usage: vcgencmd [-t] command" );
@@ -68,8 +173,8 @@ int main( int argc, char **argv )
 
    if ( vchi_initialise( &vchi_instance ) != 0)
    {
-      fprintf( stderr, "VCHI initialization failed\n" );
-      return -1;
+      // fall back to mbox implementation
+      return main_mbox(argc, argv);
    }
 
    //create a vchi connection