Proto server added for better performance of remote application using a binary protocol;

gpio2spi added which can be used to switch the GPIO pins into SPI mode if necessary;


Former-commit-id: 237f495289ce2f4afae49b36684f464937dbd30f

CMakeLists.txt

@@ -32,6 +32,9 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -Wall")
 # Configure the use of QT4
 find_package(Qt4 COMPONENTS QtCore QtGui QtNetwork REQUIRED QUIET)
 
+# add protocol buffers
+find_package(Protobuf REQUIRED)
+
 #SET(QT_DONT_USE_QTGUI   TRUE)
 #SET(QT_USE_QTCONSOLE    TRUE)
 include(${QT_USE_FILE})

include/protoserver/ProtoServer.h

@@ -0,0 +1,59 @@
+#pragma once
+
+// system includes
+#include <cstdint>
+
+// Qt includes
+#include <QTcpServer>
+#include <QSet>
+
+// Hyperion includes
+#include <hyperion/Hyperion.h>
+
+class ProtoClientConnection;
+
+///
+/// This class creates a TCP server which accepts connections wich can then send
+/// in Protocol Buffer encoded commands. This interface to Hyperion is used by
+/// hyperion-remote to control the leds
+///
+class ProtoServer : public QObject
+{
+	Q_OBJECT
+
+public:
+	///
+	/// ProtoServer constructor
+	/// @param hyperion Hyperion instance
+	/// @param port port number on which to start listening for connections
+	///
+	ProtoServer(Hyperion * hyperion, uint16_t port = 19444);
+	~ProtoServer();
+
+	///
+	/// @return the port number on which this TCP listens for incoming connections
+	///
+	uint16_t getPort() const;
+
+private slots:
+	///
+	/// Slot which is called when a client tries to create a new connection
+	///
+	void newConnection();
+
+	///
+	/// Slot which is called when a client closes a connection
+	/// @param connection The Connection object which is being closed
+	///
+	void closedConnection(ProtoClientConnection * connection);
+
+private:
+	/// Hyperion instance
+	Hyperion * _hyperion;
+
+	/// The TCP server object
+	QTcpServer _server;
+
+	/// List with open connections
+	QSet<ProtoClientConnection *> _openConnections;
+};

libsrc/CMakeLists.txt

@@ -7,5 +7,6 @@ add_subdirectory(bootsequence)
 add_subdirectory(dispmanx-grabber)
 add_subdirectory(hyperion)
 add_subdirectory(jsonserver)
+add_subdirectory(protoserver)
 add_subdirectory(utils)
 add_subdirectory(xbmcvideochecker)

libsrc/dispmanx-grabber/DispmanxWrapper.cpp

@@ -66,7 +66,8 @@ void DispmanxWrapper::setGrabbingMode(const GrabbingMode mode)
 	switch (mode)
 	{
 	case GRABBINGMODE_VIDEO:
-		_frameGrabber->setFlags(DISPMANX_SNAPSHOT_NO_RGB|DISPMANX_SNAPSHOT_FILL);
+		_frameGrabber->setFlags(0);
+		//_frameGrabber->setFlags(DISPMANX_SNAPSHOT_NO_RGB|DISPMANX_SNAPSHOT_FILL);
 		start();
 	case GRABBINGMODE_AUDIO:
 	case GRABBINGMODE_PHOTO:

libsrc/protoserver/CMakeLists.txt

@@ -0,0 +1,52 @@
+
+# Define the current source locations
+set(CURRENT_HEADER_DIR ${CMAKE_SOURCE_DIR}/include/protoserver)
+set(CURRENT_SOURCE_DIR ${CMAKE_SOURCE_DIR}/libsrc/protoserver)
+
+include_directories(
+		${CMAKE_CURRENT_BINARY_DIR}
+		${PROTOBUF_INCLUDE_DIRS})
+
+# Group the headers that go through the MOC compiler
+set(ProtoServer_QT_HEADERS
+		${CURRENT_HEADER_DIR}/ProtoServer.h
+		${CURRENT_SOURCE_DIR}/ProtoClientConnection.h
+)
+
+set(ProtoServer_HEADERS
+)
+
+set(ProtoServer_SOURCES
+		${CURRENT_SOURCE_DIR}/ProtoServer.cpp
+		${CURRENT_SOURCE_DIR}/ProtoClientConnection.cpp
+)
+
+set(ProtoServer_PROTOS
+		${CURRENT_SOURCE_DIR}/message.proto
+)
+
+protobuf_generate_cpp(ProtoServer_PROTO_SRCS ProtoServer_PROTO_HDRS
+		${ProtoServer_PROTOS}
+)
+
+qt4_wrap_cpp(ProtoServer_HEADERS_MOC ${ProtoServer_QT_HEADERS})
+
+add_library(protoserver
+		${ProtoServer_HEADERS}
+		${ProtoServer_QT_HEADERS}
+		${ProtoServer_SOURCES}
+		${ProtoServer_HEADERS_MOC}
+		${ProtoServer_PROTOS}
+		${ProtoServer_PROTO_SRCS}
+		${ProtoServer_PROTO_HDRS}
+)
+
+target_link_libraries(protoserver
+		hyperion
+		hyperion-utils
+		${PROTOBUF_LIBRARIES})
+
+qt4_use_modules(protoserver
+	Core
+	Gui
+	Network)

libsrc/protoserver/ProtoClientConnection.cpp

@@ -0,0 +1,231 @@
+// system includes
+#include <stdexcept>
+#include <cassert>
+
+// stl includes
+#include <iostream>
+#include <sstream>
+#include <iterator>
+
+// Qt includes
+#include <QResource>
+#include <QDateTime>
+
+// hyperion util includes
+#include "hyperion/ImageProcessorFactory.h"
+#include "hyperion/ImageProcessor.h"
+#include "utils/RgbColor.h"
+
+// project includes
+#include "ProtoClientConnection.h"
+
+ProtoClientConnection::ProtoClientConnection(QTcpSocket *socket, Hyperion * hyperion) :
+	QObject(),
+	_socket(socket),
+	_imageProcessor(ImageProcessorFactory::getInstance().newImageProcessor()),
+	_hyperion(hyperion),
+	_receiveBuffer()
+{
+	// connect internal signals and slots
+	connect(_socket, SIGNAL(disconnected()), this, SLOT(socketClosed()));
+	connect(_socket, SIGNAL(readyRead()), this, SLOT(readData()));
+}
+
+ProtoClientConnection::~ProtoClientConnection()
+{
+	delete _socket;
+}
+
+void ProtoClientConnection::readData()
+{
+	_receiveBuffer += _socket->readAll();
+
+	// check if we can read a message size
+	if (_receiveBuffer.size() <= 4)
+	{
+		return;
+	}
+
+	// read the message size
+	uint32_t messageSize =
+			((_receiveBuffer[0]<<24) & 0xFF000000) |
+			((_receiveBuffer[1]<<16) & 0x00FF0000) |
+			((_receiveBuffer[2]<< 8) & 0x0000FF00) |
+			((_receiveBuffer[3]    ) & 0x000000FF);
+
+	// check if we can read a complete message
+	if ((uint32_t) _receiveBuffer.size() < messageSize + 4)
+	{
+		return;
+	}
+
+	// read a message
+	proto::HyperionRequest message;
+	try
+	{
+		message.ParseFromArray(_receiveBuffer.data() + 4, messageSize);
+	}
+	catch(const std::exception & e)
+	{
+		sendErrorReply(std::string("Unable to parse message: ") + e.what());
+	}
+
+	// handle the message
+	handleMessage(message);
+
+	// remove message data from buffer
+	_receiveBuffer = _receiveBuffer.mid(messageSize + 4);
+}
+
+void ProtoClientConnection::socketClosed()
+{
+	emit connectionClosed(this);
+}
+
+void ProtoClientConnection::handleMessage(const proto::HyperionRequest & message)
+{
+	switch (message.command())
+	{
+	case proto::HyperionRequest::COLOR:
+		if (!message.HasExtension(proto::ColorRequest::colorRequest))
+		{
+			sendErrorReply("Received COLOR command without ColorRequest");
+			break;
+		}
+		handleColorCommand(message.GetExtension(proto::ColorRequest::colorRequest));
+		break;
+	case proto::HyperionRequest::IMAGE:
+		if (!message.HasExtension(proto::ImageRequest::imageRequest))
+		{
+			sendErrorReply("Received IMAGE command without ImageRequest");
+			break;
+		}
+		handleImageCommand(message.GetExtension(proto::ImageRequest::imageRequest));
+		break;
+	case proto::HyperionRequest::CLEAR:
+		if (!message.HasExtension(proto::ClearRequest::clearRequest))
+		{
+			sendErrorReply("Received CLEAR command without ClearRequest");
+			break;
+		}
+		handleClearCommand(message.GetExtension(proto::ClearRequest::clearRequest));
+		break;
+	case proto::HyperionRequest::CLEARALL:
+		handleClearallCommand();
+		break;
+	default:
+		handleNotImplemented();
+	}
+}
+
+void ProtoClientConnection::handleColorCommand(const proto::ColorRequest &message)
+{
+	if (message.rgbcolor().size() != 3)
+	{
+		sendErrorReply("The rgbcolor field requires a length of 3");
+		return;
+	}
+
+	// extract parameters
+	int priority = message.priority();
+	int duration = message.has_duration() ? message.duration() : -1;
+	const std::string & rgbColor = message.rgbcolor();
+	RgbColor color = {uint8_t(rgbColor[0]), uint8_t(rgbColor[1]), uint8_t(rgbColor[2])};
+
+	// set output
+	_hyperion->setColor(priority, color, duration);
+
+	// send reply
+	sendSuccessReply();
+}
+
+void ProtoClientConnection::handleImageCommand(const proto::ImageRequest &message)
+{
+	// extract parameters
+	int priority = message.priority();
+	int duration = message.has_duration() ? message.duration() : -1;
+	int width = message.imagewidth();
+	int height = message.imageheight();
+	const std::string & imageData = message.imagedata();
+
+	// check consistency of the size of the received data
+	if ((int) imageData.size() != width*height*3)
+	{
+		sendErrorReply("Size of image data does not match with the width and height");
+		return;
+	}
+
+	// set width and height of the image processor
+	_imageProcessor->setSize(width, height);
+
+	// create RgbImage
+	RgbImage image(width, height);
+	memcpy(image.memptr(), imageData.c_str(), imageData.size());
+
+	// process the image
+	std::vector<RgbColor> ledColors = _imageProcessor->process(image);
+	_hyperion->setColors(priority, ledColors, duration);
+
+	// send reply
+	sendSuccessReply();
+}
+
+
+void ProtoClientConnection::handleClearCommand(const proto::ClearRequest &message)
+{
+	// extract parameters
+	int priority = message.priority();
+
+	// clear priority
+	_hyperion->clear(priority);
+
+	// send reply
+	sendSuccessReply();
+}
+
+void ProtoClientConnection::handleClearallCommand()
+{
+	// clear priority
+	_hyperion->clearall();
+
+	// send reply
+	sendSuccessReply();
+}
+
+
+void ProtoClientConnection::handleNotImplemented()
+{
+	sendErrorReply("Command not implemented");
+}
+
+void ProtoClientConnection::sendMessage(const google::protobuf::Message &message)
+{
+	std::string serializedReply = message.SerializeAsString();
+	uint32_t size = serializedReply.size();
+	uint8_t sizeData[] = {uint8_t(size >> 24), uint8_t(size >> 16), uint8_t(size >> 8), uint8_t(size)};
+	std::cout << sizeData[0] << " " << sizeData[0] << " " << sizeData[0] << " " << sizeData[0] << " <data>" << std::endl;
+	_socket->write((const char *) sizeData, sizeof(sizeData));
+	_socket->write(serializedReply.data(), serializedReply.length());
+	_socket->flush();
+}
+
+void ProtoClientConnection::sendSuccessReply()
+{
+	// create reply
+	proto::HyperionReply reply;
+	reply.set_success(true);
+
+	// send reply
+	sendMessage(reply);
+}
+
+void ProtoClientConnection::sendErrorReply(const std::string &error)
+{
+	// create reply
+	proto::HyperionReply reply;
+	reply.set_success(false);
+	reply.set_error(error);
+
+	// send reply
+	sendMessage(reply);
+}

libsrc/protoserver/ProtoClientConnection.h

@@ -0,0 +1,132 @@
+#pragma once
+
+// stl includes
+#include <string>
+
+// Qt includes
+#include <QByteArray>
+#include <QTcpSocket>
+
+// jsoncpp includes
+#include <json/json.h>
+
+// Hyperion includes
+#include <hyperion/Hyperion.h>
+
+// util includes
+#include <utils/jsonschema/JsonSchemaChecker.h>
+
+// proto includes
+#include "message.pb.h"
+
+class ImageProcessor;
+
+///
+/// The Connection object created by \a ProtoServer when a new connection is establshed
+///
+class ProtoClientConnection : public QObject
+{
+	Q_OBJECT
+
+public:
+	///
+	/// Constructor
+	/// @param socket The Socket object for this connection
+	/// @param hyperion The Hyperion server
+	///
+	ProtoClientConnection(QTcpSocket * socket, Hyperion * hyperion);
+
+	///
+	/// Destructor
+	///
+	~ProtoClientConnection();
+
+signals:
+	///
+	/// Signal which is emitted when the connection is being closed
+	/// @param connection This connection object
+	///
+	void connectionClosed(ProtoClientConnection * connection);
+
+private slots:
+	///
+	/// Slot called when new data has arrived
+	///
+	void readData();
+
+	///
+	/// Slot called when this connection is being closed
+	///
+	void socketClosed();
+
+private:
+	///
+	/// Handle an incoming Proto message
+	///
+	/// @param message the incoming message as string
+	///
+	void handleMessage(const proto::HyperionRequest &message);
+
+	///
+	/// Handle an incoming Proto Color message
+	///
+	/// @param message the incoming message
+	///
+	void handleColorCommand(const proto::ColorRequest & message);
+
+	///
+	/// Handle an incoming Proto Image message
+	///
+	/// @param message the incoming message
+	///
+	void handleImageCommand(const proto::ImageRequest & message);
+
+	///
+	/// Handle an incoming Proto Clear message
+	///
+	/// @param message the incoming message
+	///
+	void handleClearCommand(const proto::ClearRequest & message);
+
+	///
+	/// Handle an incoming Proto Clearall message
+	///
+	void handleClearallCommand();
+
+	///
+	/// Handle an incoming Proto message of unknown type
+	///
+	void handleNotImplemented();
+
+	///
+	/// Send a message to the connected client
+	///
+	/// @param message The Proto message to send
+	///
+	void sendMessage(const google::protobuf::Message &message);
+
+	///
+	/// Send a standard reply indicating success
+	///
+	void sendSuccessReply();
+
+	///
+	/// Send an error message back to the client
+	///
+	/// @param error String describing the error
+	///
+	void sendErrorReply(const std::string & error);
+
+private:
+	/// The TCP-Socket that is connected tot the Proto-client
+	QTcpSocket * _socket;
+
+	/// The processor for translating images to led-values
+	ImageProcessor * _imageProcessor;
+
+	/// Link to Hyperion for writing led-values to a priority channel
+	Hyperion * _hyperion;
+
+	/// The buffer used for reading data from the socket
+	QByteArray _receiveBuffer;
+};

libsrc/protoserver/ProtoServer.cpp

@@ -0,0 +1,57 @@
+// system includes
+#include <stdexcept>
+
+// project includes
+#include <protoserver/ProtoServer.h>
+#include "ProtoClientConnection.h"
+
+ProtoServer::ProtoServer(Hyperion *hyperion, uint16_t port) :
+	QObject(),
+	_hyperion(hyperion),
+	_server(),
+	_openConnections()
+{
+	if (!_server.listen(QHostAddress::Any, port))
+	{
+		throw std::runtime_error("Proto server could not bind to port");
+	}
+
+	// Set trigger for incoming connections
+	connect(&_server, SIGNAL(newConnection()), this, SLOT(newConnection()));
+}
+
+ProtoServer::~ProtoServer()
+{
+	foreach (ProtoClientConnection * connection, _openConnections) {
+		delete connection;
+	}
+}
+
+uint16_t ProtoServer::getPort() const
+{
+	return _server.serverPort();
+}
+
+void ProtoServer::newConnection()
+{
+	QTcpSocket * socket = _server.nextPendingConnection();
+
+	if (socket != nullptr)
+	{
+		std::cout << "New proto connection" << std::endl;
+		ProtoClientConnection * connection = new ProtoClientConnection(socket, _hyperion);
+		_openConnections.insert(connection);
+
+		// register slot for cleaning up after the connection closed
+		connect(connection, SIGNAL(connectionClosed(ProtoClientConnection*)), this, SLOT(closedConnection(ProtoClientConnection*)));
+	}
+}
+
+void ProtoServer::closedConnection(ProtoClientConnection *connection)
+{
+	std::cout << "Proto connection closed" << std::endl;
+	_openConnections.remove(connection);
+
+	// schedule to delete the connection object
+	connection->deleteLater();
+}

libsrc/protoserver/message.proto

@@ -0,0 +1,69 @@
+package proto;
+
+message HyperionRequest {
+	enum Command {
+		COLOR = 1;
+		IMAGE = 2;
+		CLEAR = 3;
+		CLEARALL = 4;
+	}
+
+	// command specification
+	required Command command = 1;
+
+	// extensions to define all specific requests
+	extensions 10 to 100;
+}
+
+message ColorRequest {
+	extend HyperionRequest {
+		required ColorRequest colorRequest = 10;
+	}
+
+	// priority to use when setting the color
+	required int32 priority = 1;
+
+	// 3-byte value containing the rgb color
+	required bytes rgbColor = 2;
+
+	// duration of the request (negative results in infinite)
+	optional int32 duration = 3;
+}
+
+message ImageRequest {
+	extend HyperionRequest {
+		required ImageRequest imageRequest = 11;
+	}
+
+	// priority to use when setting the image
+	required int32 priority = 1;
+
+	// width of the image
+	required int32 imagewidth = 2;
+
+	// height of the image
+	required int32 imageheight = 3;
+
+	// image data
+	required bytes imagedata = 4;
+
+	// duration of the request (negative results in infinite)
+	optional int32 duration = 5;
+}
+
+message ClearRequest {
+	extend HyperionRequest {
+		required ClearRequest clearRequest = 12;
+	}
+
+	// priority which need to be cleared
+	required int32 priority = 1;
+}
+
+message HyperionReply {
+	// flag indication success or failure
+	required bool success = 1;
+
+	// string indicating the reason for failure (if applicable)
+	optional string error = 2;
+}

test/CMakeLists.txt

@@ -7,7 +7,6 @@ add_executable(test_spi
 target_link_libraries(test_spi
 		hyperion)
 
-
 add_executable(test_configfile
 		TestConfigFile.cpp)
 target_link_libraries(test_configfile
@@ -41,3 +40,4 @@ target_link_libraries(test_blackborderprocessor
 		hyperion)
 
 add_executable(spidev_test spidev_test.c)
+add_executable(gpio2spi switchPinCtrl.c)

test/switchPinCtrl.c

@@ -0,0 +1,177 @@
+//
+// Simple byte wise SPI driver
+// Demo how to set up memmap and access SPI registers.
+// Code seems to be working but has not been much tested.
+// G.J. van Loo 15-Jan-2012
+//
+
+
+
+// Access from ARM Running Linux
+
+#define BCM2708_PERI_BASE        0x20000000
+#define UART0_BASE               (BCM2708_PERI_BASE + 0x201000) /* Uart 0 */
+#define UART1_BASE               (BCM2708_PERI_BASE + 0x215000) /* Uart 1 */
+#define MCORE_BASE               (BCM2708_PERI_BASE + 0x0000)   /* Fake frame buffer device */
+#define GPIO_BASE                (BCM2708_PERI_BASE + 0x200000) /* GPIO controller */
+#define SPI0_BASE                (BCM2708_PERI_BASE + 0x204000) /* SPI0 controller */
+
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <dirent.h>
+#include <fcntl.h>
+#include <assert.h>
+#include <sys/mman.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <unistd.h>
+
+#define PAGE_SIZE (4*1024)
+#define BLOCK_SIZE (4*1024)
+
+int  mem_fd;
+char *gpio_mem, *gpio_map;
+char *spi0_mem, *spi0_map;
+
+
+// I/O access
+volatile unsigned *gpio;
+volatile unsigned *spi0;
+
+
+// SPI operation
+
+// GPIO setup macros. Always use INP_GPIO(x) before using OUT_GPIO(x) or SET_GPIO_ALT(x,y)
+#define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3))
+#define OUT_GPIO(g) *(gpio+((g)/10)) |=  (1<<(((g)%10)*3))
+#define SET_GPIO_ALT(g,a) *(gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))
+
+//
+#define SPI0_CNTLSTAT *(spi0 + 0)
+#define SPI0_FIFO     *(spi0 + 1)
+#define SPI0_CLKSPEED *(spi0 + 2)
+
+// SPI0_CNTLSTAT register bits
+
+#define SPI0_CS_CS2ACTHIGH   0x00800000 // CS2 active high
+#define SPI0_CS_CS1ACTHIGH   0x00400000 // CS1 active high
+#define SPI0_CS_CS0ACTHIGH   0x00200000 // CS0 active high
+#define SPI0_CS_RXFIFOFULL   0x00100000 // Receive FIFO full
+#define SPI0_CS_RXFIFO3_4    0x00080000 // Receive FIFO 3/4 full
+#define SPI0_CS_TXFIFOSPCE   0x00040000 // Transmit FIFO has space
+#define SPI0_CS_RXFIFODATA   0x00020000 // Receive FIFO has data
+#define SPI0_CS_DONE         0x00010000 // SPI transfer done. WRT to CLR!
+#define SPI0_CS_MOSI_INPUT   0x00001000 // MOSI is input, read from MOSI (BI-dir mode)
+#define SPI0_CS_DEASRT_CS    0x00000800 // De-assert CS at end
+#define SPI0_CS_RX_IRQ       0x00000400 // Receive irq enable
+#define SPI0_CS_DONE_IRQ     0x00000200 // irq when done
+#define SPI0_CS_DMA_ENABLE   0x00000100 // Run in DMA mode
+#define SPI0_CS_ACTIVATE     0x00000080 // Activate: be high before starting
+#define SPI0_CS_CS_POLARIT   0x00000040 // Chip selects active high
+#define SPI0_CS_CLRTXFIFO    0x00000020 // Clear TX FIFO    (auto clear bit)
+#define SPI0_CS_CLRRXFIFO    0x00000010 // Clear RX FIFO    (auto clear bit)
+#define SPI0_CS_CLRFIFOS     0x00000030 // Clear BOTH FIFOs (auto clear bit)
+#define SPI0_CS_CLK_IDLHI    0x00000008 // Clock pin is high when idle
+#define SPI0_CS_CLKTRANS     0x00000004 // 0=first clock in middle of data bit
+                                        // 1=first clock at begin of data bit
+#define SPI0_CS_CHIPSEL0     0x00000000 // Use chip select 0
+#define SPI0_CS_CHIPSEL1     0x00000001 // Use chip select 1
+#define SPI0_CS_CHIPSEL2     0x00000002 // Use chip select 2
+#define SPI0_CS_CHIPSELN     0x00000003 // No chip select (e.g. use GPIO pin)
+
+#define SPI0_CS_CLRALL      (SPI0_CS_CLRFIFOS|SPI0_CS_DONE)
+
+#define ISASC(x) ((x)>=0x20 && (x)<=0x7F)
+
+void setup_io();
+
+int main(int argc, char **argv)
+{ int g;
+
+  setup_io();  // Set up direct access to I/O for GPIO and SPI
+
+  // Switch GPIO 7..11 to SPI mode (ALT function 0)
+
+ /************************************************************************\
+  * You are about to change the GPIO settings of your computer.          *
+  * Mess this up and it will stop working!                               *
+  * It might be a good idea to 'sync' before running this program        *
+  * so at least you still have your code changes written to the SD-card! *
+ \************************************************************************/
+
+  for (g=7; g<=11; g++)
+  {
+    INP_GPIO(g);       // clear bits (= input)
+    SET_GPIO_ALT(g,0); // set function 0
+  }
+
+  return 0;
+
+} // main
+
+
+//
+// Set up a memory regions to access GPIO and SPI0
+//
+void setup_io()
+{
+
+   /* open /dev/mem */
+   if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
+      printf("can't open /dev/mem \n");
+      exit (-1);
+   }
+
+   /* mmap GPIO */
+   if ((gpio_mem = malloc(BLOCK_SIZE + (PAGE_SIZE-1))) == NULL) {
+      printf("allocation error \n");
+      exit (-1);
+   }
+   if ((unsigned long)gpio_mem % PAGE_SIZE)
+     gpio_mem += PAGE_SIZE - ((unsigned long)gpio_mem % PAGE_SIZE);
+
+   gpio_map = (unsigned char *)mmap(
+      (caddr_t)gpio_mem,
+      BLOCK_SIZE,
+      PROT_READ|PROT_WRITE,
+      MAP_SHARED|MAP_FIXED,
+      mem_fd,
+      GPIO_BASE
+   );
+
+   if ((long)gpio_map < 0) {
+      printf("mmap error %d\n", (int)gpio_map);
+      exit (-1);
+   }
+   gpio = (volatile unsigned *)gpio_map;
+
+   /* mmap SPI0 */
+   if ((spi0_mem = malloc(BLOCK_SIZE + (PAGE_SIZE-1))) == NULL) {
+      printf("allocation error \n");
+      exit (-1);
+   }
+   if ((unsigned long)spi0_mem % PAGE_SIZE)
+     spi0_mem += PAGE_SIZE - ((unsigned long)spi0_mem % PAGE_SIZE);
+
+   spi0_map = (unsigned char *)mmap(
+      (caddr_t)spi0_mem,
+      BLOCK_SIZE,
+      PROT_READ|PROT_WRITE,
+      MAP_SHARED|MAP_FIXED,
+      mem_fd,
+      SPI0_BASE
+   );
+
+
+   printf("SPI mapped from 0x%p to 0x%p\n",SPI0_BASE,spi0_map);
+
+   if ((long)spi0_map < 0) {
+      printf("mmap error %d\n", (int)spi0_map);
+      exit (-1);
+   }
+   spi0 = (volatile unsigned *)spi0_map;
+
+} // setup_io