BUGFIX: Enable QPU when accessing Register Map

Lib/VideoCore/Mailbox.cpp

@@ -42,6 +42,8 @@ namespace QPULib {
 
 void *mapmem(unsigned base, unsigned size)
 {
+   //printf("mapmem requested base: 0x%x, size: %d\n", base, size);
+
    int mem_fd;
    unsigned offset = base % PAGE_SIZE;
    base = base - offset;
@@ -63,7 +65,9 @@ void *mapmem(unsigned base, unsigned size)
 #endif  // DEBUG
 
    if (mem == MAP_FAILED) {
-      printf("mmap error %p\n", mem);
+      printf("mmap error value %p: ", mem);
+      fflush(stdout);
+      perror(nullptr);  // Prints error message on stdout
       exit (-1);
    }
    close(mem_fd);

Lib/VideoCore/RegisterMap.cpp

@@ -3,10 +3,114 @@
 #include "RegisterMap.h"
 #include <cassert>
 #include <cstring>
-#include <stdio.h>
 #include <unistd.h>
+#include <stdlib.h>   // needed for 'old' compilers, no harm done for new
+#include <errno.h>    // idem
+#include <stdio.h>    // idem
+#include "Mailbox.h"  // mapmem()
+
+#ifdef USE_BCM_HEADERS
+// Following works for newer distro's
 #include <bcm_host.h>
-#include "Mailbox.h"  // for mapmem()
+#else  // USE_BCM_HEADERS
+//#pragma message "This is an old pi!"
+//
+// For old Pi's, calls bcm_host_get_peripheral_address() and
+// bcm_host_get_peripheral_size() don't exist, so we need
+// to supply them ourselves.
+//
+
+
+/**
+ * @brief This returns the ARM-side physical address where peripherals are mapped.
+ *
+ * Values:
+ *
+ * - 0x20000000  - Pi Zero, Zero W, and the first generation of the Pi and Compute Module
+ * - 0x3f000000  - Pi 2, Pi 3 and Compute Module 3
+ *
+ * Source: https://www.elinux.org/RPi_HardwareHistory
+ *
+ * At time of writing (20180723), the list of handled revisions is complete.
+ * New values may be added as new models appear.
+ */
+unsigned bcm_host_get_peripheral_address() {
+	unsigned md = QPULib::mbox_open();
+	unsigned revision = QPULib::get_version(md);
+	QPULib::mbox_close(md);
+	printf("bcm_host_get_peripheral_address revision: %x\n", revision);
+
+	switch(revision) {
+		case 0x0007: 	  // A
+		case 0x0008:
+		case 0x0009:
+		case 0x0012: 	  // A+
+		case 0x0015:
+		case 0x900021:
+		case 0x0002: 	  // B
+		case 0x0003:
+		case 0x0004:
+		case 0x0005:
+		case 0x0006:
+		case 0x000d:
+		case 0x000e:
+		case 0x000f:
+		case 0x0010:    // B+
+		case 0x0013:
+		case 0x900032:
+		case 0x0011:    // Compute Module 1
+		case 0x0014:
+		case 0x900092:  // Zero
+		case 0x900093:
+		case 0x920093:
+		case 0x9000c1:  // Zero W
+ 			return 0x20000000;
+
+		case 0xa01040:  // 2 Model B
+		case 0xa01041:
+		case 0xa21041:
+		case 0xa22042:
+		case 0xa02082:  // 3 Model B
+		case 0xa22082:
+		case 0xa32082:
+		case 0xa020d3:  // 3 Model B+ 
+		case 0xa020a0:  // Compute Module 3 (and CM3 Lite)
+			return 0x3f000000;
+
+		default:
+#ifdef DEBUG
+			printf("bcm_host_get_peripheral_address unknown hardware revision %x\n", revision);
+			exit(-1);
+#else
+			// All newer models have this address, so let's assume it 
+			// for new revision numbers.
+			return 0x3f000000;
+#endif
+	}
+}
+
+
+// following is the same for all Pi models
+unsigned bcm_host_get_peripheral_size() { return 0x01000000; }
+
+//
+// These are the only things we need from bcm_host.h, we declare
+// them explicitly to avoid dragging in all the stuff that throws compile errors.
+//
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void bcm_host_init(void);
+void bcm_host_deinit(void);
+
+#ifdef __cplusplus
+}
+#endif
+// End things we need from bcm_host.h
+
+#endif  // USE_BCM_HEADERS
+
 
 namespace QPULib {
 
@@ -20,7 +124,7 @@ enum {
 std::unique_ptr<RegisterMap> RegisterMap::m_instance;
 
 
-RegisterMap::RegisterMap() {
+RegisterMap::RegisterMap() : m_addr(nullptr), m_size(0) {
 	bcm_host_init();
 	unsigned addr = bcm_host_get_peripheral_address();
 	m_size = bcm_host_get_peripheral_size();
@@ -35,7 +139,6 @@ RegisterMap::RegisterMap() {
 
 
 RegisterMap::~RegisterMap() {
-	// printf("Closing down register map\n");
 	unmapmem((void *) m_addr, m_size);
 	bcm_host_deinit();
 }
@@ -45,6 +148,13 @@ RegisterMap::~RegisterMap() {
  * @brief Get the 32-bit value at the given offset in the map
  */
 uint32_t RegisterMap::read(int offset) const {
+#ifdef DEBUG
+	// Do a spot check on the loaded memory
+	if (m_addr[V3D_BASE] == 0XDEADBEEF) {
+		printf("WARNING: RegisterMap can not read QPU registers, the VideoCore is not enabled.\n");
+	}
+#endif
+
 	return m_addr[V3D_BASE + offset];
 }
 
@@ -55,7 +165,39 @@ uint32_t RegisterMap::read(int offset) const {
  * This avoids having to use `instance()->` for every read access.
  */
 uint32_t RegisterMap::readRegister(int offset) {
-	return instance()->read(V3D_IDENT1);
+  //printf("Called readRegister, m_instance: %p\n", m_instance.get());
+	return instance()->read(offset);
+}
+
+
+/**
+ * @brief Check if the register map is accessible.
+ *
+ * This depends on the VideoCore being enabled. Enabling and disabling
+ * is done with mailbox call `qpu_enable()`.
+ *
+ * This method can thus be used to detect if the VideoCore is running.
+ *
+ * @return true if register map accessible, false otherwise
+ */
+bool RegisterMap::enabled() {
+	// Detect the signature in register 0
+	uint32_t reg = readRegister(V3D_IDENT0);
+	char *p = (char *) &reg;
+#ifdef DEBUG
+	printf("Reg 0: '%c%c%c'\n", p[0], p[1], p[2]);
+#endif
+
+	bool canRead = (p[0] == 'V' && p[1] == '3' && p[2] == 'D');
+
+#ifdef PROB_NOT_REQUIRED
+	if (!canRead) {
+		// Reset singleton for a next attempt
+		m_instance.reset(nullptr);
+	}
+#endif  // PROB_NOT_REQUIRED
+
+	return canRead;
 }
 
 
@@ -73,10 +215,14 @@ int RegisterMap::numQPUPerSlice() {
 
 
 RegisterMap *RegisterMap::instance() {
+  //printf("Called instance(), m_instance: %p\n", m_instance.get());
+
 	if (m_instance.get() == nullptr) {
+		//printf("RegisterMap initializing singleton\n");
 		m_instance.reset(new RegisterMap);
 	}
 
+ // printf("m_instance post: %p\n", m_instance.get());
 	return m_instance.get();
 }
 

Lib/VideoCore/RegisterMap.h

@@ -24,14 +24,15 @@ class RegisterMap {
 
 	~RegisterMap();
 
-	static int numSlices();
-	static int numQPUPerSlice();
+	static bool enabled();
+	static int  numSlices();
+	static int  numQPUPerSlice();
 
 private:
 	RegisterMap();
 
-	volatile uint32_t *m_addr{nullptr};
-	unsigned m_size{0};
+	volatile uint32_t *m_addr;
+	unsigned m_size;
 
 	static std::unique_ptr<RegisterMap> m_instance;
 

Makefile

@@ -1,14 +1,26 @@
 #
-# There are four builds possible, with output directories:
+# NOTES
+# =====
+#
+# * There are four builds possible, with output directories:
 #
 #   obj              - using emulator
 #   obj-debug        - output debug info, using emulator
 #   obj-qpu          - using hardware
 #   obj-debug-qpu    - output debug info, using hardware
 #
-# To compile for debugging, add flag '-g' to CXX_FLAGS.
 #
-###########################################################
+# * To compile for debugging, add flag '-g' to CXX_FLAGS.
+#
+#
+# * USE_BCM_HEADERS is set to 'no' by default here. This
+#   is to allow a proper build on old distro's (e.g. Raspbian wheezy).
+#
+#   For newer distro's it is recommended to comment this out and use the 
+#   longer assignment to USE_BCM_HEADERS. This works on 'Raspian stretch`,
+#   for 'jessie' it's unknown.
+#
+###########################################################################
 
 # Root directory of QPULib repository
 ROOT = Lib
@@ -33,16 +45,30 @@ ifeq ($(QPU), 1)
 
 # Check platform before building. Can't be indented, otherwise make complains.
 RET := $(shell Tools/detectPlatform.sh 1>/dev/null && echo "yes" || echo "no")
-#$(info  info: '$(RET)')
 ifneq ($(RET), yes)
 $(error QPU-mode specified on a non-Pi platform; aborting)
 else
 $(info Building on a Pi platform)
 endif
 
-  CXX_FLAGS += -DQPU_MODE -I /opt/vc/include
-  OBJ_DIR := $(OBJ_DIR)-qpu
+  CXX_FLAGS += -DQPU_MODE
 	LIBS := -L /opt/vc/lib -l bcm_host
+
+  OBJ_DIR := $(OBJ_DIR)-qpu
+
+#
+# Detect if Pi has bcm_host support (new) or not (old)
+#
+#USE_BCM_HEADERS:= $(shell grep bcm_host_get_peripheral_address /opt/vc/include/bcm_host.h && echo "no" || echo "yes")
+USE_BCM_HEADERS:=no
+ifeq ($(USE_BCM_HEADERS), no)
+$(info not using bcm headers)
+else
+$(info using bcm headers)
+CXX_FLAGS+= -DUSE_BCM_HEADERS
+CXX_FLAGS+= -I /opt/vc/include
+endif
+
 else
   CXX_FLAGS += -DEMULATION_MODE
 endif
@@ -233,7 +259,7 @@ $(OBJ_DIR)/bin/runTests: $(UNIT_TESTS) $(EXAMPLES_OBJ) | $(QPU_LIB)
 
 make_test: $(OBJ_DIR)/bin/runTests
 
-test : | make_test AutoTest
+test : | make_test AutoTest detectPlatform
 	@echo Running unit tests with '$(RUN_TESTS)'
 	@$(RUN_TESTS)
 

Tools/detectPlatform.cpp

@@ -119,12 +119,30 @@ int main(int argc, char *argv[]) {
 	unsigned revision = get_version(mb);
 	printf("Hardware revision: %04x\n", revision);
 
+	bool wasEnabled = true;  // Default to prevent final call to qpu_enable()
 	if (geteuid() == 0) {  // Only do this as root (sudo)
+		wasEnabled = RegisterMap::enabled();
+		if (!wasEnabled) {
+			// VideoCore needs to be enabled, otherwise the registers can't be accessed.
+			//printf("VideoCore not running, enabling for this app\n");
+			qpu_enable(mb, 1);
+
+			// Videocore apparently needs some time on initial startup
+      // After first, it starts up fast.
+			// Would be better if we could detect the very first call
+			sleep(3);
+		}
+
 		printf("Number of slices: %d\n", RegisterMap::numSlices());
 		printf("Number of QPU's per slice: %d\n", RegisterMap::numQPUPerSlice());
 	} else {
 		printf("You can see more if you use sudo\n");
   }
+
+	if (!wasEnabled) {
+		//printf("Disabling the VideoCore again\n");
+		qpu_enable(mb, 0);
+	}
 #endif  // QPU_MODE
 
 	return 0;