Trying the RPi2 fix from PiFmRds, still no success...

mailbox.c

@@ -0,0 +1,274 @@
+/*
+Copyright (c) 2012, Broadcom Europe Ltd.
+All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+     * Redistributions of source code must retain the above copyright
+        notice, this list of conditions and the following disclaimer.
+     * Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution.
+     * Neither the name of the copyright holder nor the
+        names of its contributors may be used to endorse or promote products
+        derived from this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <assert.h>
+#include <stdint.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include "mailbox.h"
+
+#define PAGE_SIZE (4*1024)
+
+void *mapmem(unsigned base, unsigned size)
+{
+    int mem_fd;
+    unsigned offset = base % PAGE_SIZE;
+    base = base - offset;
+    /* open /dev/mem */
+    if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
+        printf("can't open /dev/mem\nThis program should be run as root. Try prefixing command with: sudo\n");
+        exit (-1);
+    }
+    void *mem = mmap(
+        0,
+        size,
+        PROT_READ|PROT_WRITE,
+        MAP_SHARED/*|MAP_FIXED*/,
+        mem_fd,
+        base);
+#ifdef DEBUG
+    printf("base=0x%x, mem=%p\n", base, mem);
+#endif
+    if (mem == MAP_FAILED) {
+        printf("mmap error %d\n", (int)mem);
+        exit (-1);
+    }
+    close(mem_fd);
+    return (char *)mem + offset;
+}
+
+void *unmapmem(void *addr, unsigned size)
+{
+    int s = munmap(addr, size);
+    if (s != 0) {
+        printf("munmap error %d\n", s);
+        exit (-1);
+    }
+
+    return NULL;
+}
+
+/*
+ * 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);
+    }
+
+#ifdef DEBUG
+    unsigned *p = buf; int i; unsigned size = *(unsigned *)buf;
+    for (i=0; i<size/4; i++)
+        printf("%04x: 0x%08x\n", i*sizeof *p, p[i]);
+#endif
+    return ret_val;
+}
+
+unsigned mem_alloc(int file_desc, unsigned size, unsigned align, unsigned flags)
+{
+    int i=0;
+    unsigned p[32];
+    p[i++] = 0; // size
+    p[i++] = 0x00000000; // process request
+
+    p[i++] = 0x3000c; // (the tag id)
+    p[i++] = 12; // (size of the buffer)
+    p[i++] = 12; // (size of the data)
+    p[i++] = size; // (num bytes? or pages?)
+    p[i++] = align; // (alignment)
+    p[i++] = flags; // (MEM_FLAG_L1_NONALLOCATING)
+
+    p[i++] = 0x00000000; // end tag
+    p[0] = i*sizeof *p; // actual size
+
+    if(mbox_property(file_desc, p) < 0) return 0;
+    return p[5];
+}
+
+unsigned mem_free(int file_desc, unsigned handle)
+{
+    int i=0;
+    unsigned p[32];
+    p[i++] = 0; // size
+    p[i++] = 0x00000000; // process request
+
+    p[i++] = 0x3000f; // (the tag id)
+    p[i++] = 4; // (size of the buffer)
+    p[i++] = 4; // (size of the data)
+    p[i++] = handle;
+
+    p[i++] = 0x00000000; // end tag
+    p[0] = i*sizeof *p; // actual size
+
+    if(mbox_property(file_desc, p) < 0) return 0;
+    return p[5];
+}
+
+unsigned mem_lock(int file_desc, unsigned handle)
+{
+    int i=0;
+    unsigned p[32];
+    p[i++] = 0; // size
+    p[i++] = 0x00000000; // process request
+
+    p[i++] = 0x3000d; // (the tag id)
+    p[i++] = 4; // (size of the buffer)
+    p[i++] = 4; // (size of the data)
+    p[i++] = handle;
+
+    p[i++] = 0x00000000; // end tag
+    p[0] = i*sizeof *p; // actual size
+
+    if(mbox_property(file_desc, p) < 0) return 0;
+    return p[5];
+}
+
+unsigned mem_unlock(int file_desc, unsigned handle)
+{
+    int i=0;
+    unsigned p[32];
+    p[i++] = 0; // size
+    p[i++] = 0x00000000; // process request
+
+    p[i++] = 0x3000e; // (the tag id)
+    p[i++] = 4; // (size of the buffer)
+    p[i++] = 4; // (size of the data)
+    p[i++] = handle;
+
+    p[i++] = 0x00000000; // end tag
+    p[0] = i*sizeof *p; // actual size
+
+    if(mbox_property(file_desc, p) < 0) return 0;
+    return p[5];
+}
+
+unsigned execute_code(int file_desc, unsigned code, unsigned r0, unsigned r1, unsigned r2, unsigned r3, unsigned r4, unsigned r5)
+{
+    int i=0;
+    unsigned p[32];
+    p[i++] = 0; // size
+    p[i++] = 0x00000000; // process request
+
+    p[i++] = 0x30010; // (the tag id)
+    p[i++] = 28; // (size of the buffer)
+    p[i++] = 28; // (size of the data)
+    p[i++] = code;
+    p[i++] = r0;
+    p[i++] = r1;
+    p[i++] = r2;
+    p[i++] = r3;
+    p[i++] = r4;
+    p[i++] = r5;
+
+    p[i++] = 0x00000000; // end tag
+    p[0] = i*sizeof *p; // actual size
+
+    if(mbox_property(file_desc, p) < 0) return 0;
+    return p[5];
+}
+
+unsigned qpu_enable(int file_desc, unsigned enable)
+{
+    int i=0;
+    unsigned p[32];
+
+    p[i++] = 0; // size
+    p[i++] = 0x00000000; // process request
+
+    p[i++] = 0x30012; // (the tag id)
+    p[i++] = 4; // (size of the buffer)
+    p[i++] = 4; // (size of the data)
+    p[i++] = enable;
+
+    p[i++] = 0x00000000; // end tag
+    p[0] = i*sizeof *p; // actual size
+
+    if(mbox_property(file_desc, p) < 0) return 0;
+    return p[5];
+}
+
+unsigned execute_qpu(int file_desc, unsigned num_qpus, unsigned control, unsigned noflush, unsigned timeout) {
+    int i=0;
+    unsigned p[32];
+
+    p[i++] = 0; // size
+    p[i++] = 0x00000000; // process request
+    p[i++] = 0x30011; // (the tag id)
+    p[i++] = 16; // (size of the buffer)
+    p[i++] = 16; // (size of the data)
+    p[i++] = num_qpus;
+    p[i++] = control;
+    p[i++] = noflush;
+    p[i++] = timeout; // ms
+
+    p[i++] = 0x00000000; // end tag
+    p[0] = i*sizeof *p; // actual size
+
+    if(mbox_property(file_desc, p) < 0) return 0;
+    return p[5];
+}
+
+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("Using mbox device " DEVICE_FILE_NAME ".\n");
+        return file_desc;
+    }
+    
+    // Try to create one
+    unlink(LOCAL_DEVICE_FILE_NAME);
+    if(mknod(LOCAL_DEVICE_FILE_NAME, S_IFCHR|0600, makedev(MAJOR_NUM_A, 0)) >= 0 &&
+        (file_desc = open(LOCAL_DEVICE_FILE_NAME, 0)) >= 0) {
+        printf("Using local mbox device file with major %d.\n", MAJOR_NUM_A);
+        return file_desc;
+    }
+
+    unlink(LOCAL_DEVICE_FILE_NAME);
+    if(mknod(LOCAL_DEVICE_FILE_NAME, S_IFCHR|0600, makedev(MAJOR_NUM_B, 0)) >= 0 &&
+        (file_desc = open(LOCAL_DEVICE_FILE_NAME, 0)) >= 0) {
+        printf("Using local mbox device file with major %d.\n", MAJOR_NUM_B);
+        return file_desc;
+    }
+
+    return -1;
+}
+
+void mbox_close(int file_desc) {
+    close(file_desc);
+}

mailbox.h

@@ -0,0 +1,48 @@
+/*
+Copyright (c) 2012, Broadcom Europe Ltd.
+All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the copyright holder nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <linux/ioctl.h>
+
+// Newer kernels (>= 4.1) use major 249, older ones major 100.
+#define MAJOR_NUM_A 249
+#define MAJOR_NUM_B 100
+#define IOCTL_MBOX_PROPERTY _IOWR(MAJOR_NUM_B, 0, char *)
+#define DEVICE_FILE_NAME "/dev/vcio"
+#define LOCAL_DEVICE_FILE_NAME "mbox"
+
+int mbox_open();
+void mbox_close(int file_desc);
+
+unsigned get_version(int file_desc);
+unsigned mem_alloc(int file_desc, unsigned size, unsigned align, unsigned flags);
+unsigned mem_free(int file_desc, unsigned handle);
+unsigned mem_lock(int file_desc, unsigned handle);
+unsigned mem_unlock(int file_desc, unsigned handle);
+void *mapmem(unsigned base, unsigned size);
+void *unmapmem(void *addr, unsigned size);
+
+unsigned execute_code(int file_desc, unsigned code, unsigned r0, unsigned r1, unsigned r2, unsigned r3, unsigned r4, unsigned r5);
+unsigned execute_qpu(int file_desc, unsigned num_qpus, unsigned control, unsigned noflush, unsigned timeout);
+unsigned qpu_enable(int file_desc, unsigned enable);

makefile

@@ -1,15 +1,15 @@
 prefix=/usr/local
 
 archis = $(if $(findstring $(1),$(shell uname -m)),$(2))
-peribase = $(if $(call archis,armv7,dummy-text),0x3F000000,0x20000000)
+pi_version_flag = $(if $(call archis,armv7,dummy-text),-DRPI2,-DRPI1)
 
 all: wspr gpioclk
 
 wspr: wspr.cpp
-	g++ -Wall -lm -DBCM2708_PERI_BASE=$(peribase) wspr.cpp -owspr
+	g++ -Wall -lm $(pi_version_flag) mailbox.c wspr.cpp -owspr
 
 gpioclk: gpioclk.cpp
-	g++ -Wall -lm -DBCM2708_PERI_BASE=$(peribase) gpioclk.cpp -ogpioclk
+	g++ -Wall -lm $(pi_version_flag) gpioclk.cpp -ogpioclk
 
 clean:
 	rm gpioclk

wspr.cpp

@@ -41,6 +41,8 @@ License:
 #include <iomanip>
 #include <sys/timex.h>
 
+#include "mailbox.h"
+
 using namespace std;
 
 #define ABORT(a) exit(a)
@@ -65,14 +67,19 @@ using namespace std;
 #define WSPR_RAND_OFFSET 80
 #define WSPR15_RAND_OFFSET 8
 
-//For the original Raspberry Pi:
-//#define BCM2708_PERI_BASE        0x20000000
-//For Raspberry Pi 2:
-//#define BCM2708_PERI_BASE        0x3F000000
 //Now we autodetect it via makefile.
+#ifdef RPI2
+
+#define BCM2708_PERI_BASE 0x3f000000
+#define MEM_FLAG 0x04
+#pragma message "Raspberry Pi 2 detected."
+
+#else
+
+#define BCM2708_PERI_BASE 0x20000000
+#define MEM_FLAG 0x0c
+#pragma message "Raspberry Pi 1 detected."
 
-#ifndef BCM2708_PERI_BASE
- #error "BCM2708_PERI_BASE not defined!"
 #endif
 	
 #define GPIO_BASE                (BCM2708_PERI_BASE + 0x200000) /* GPIO controller */
@@ -149,30 +156,43 @@ struct PageInfo {
 //struct PageInfo instrPage;
 //struct PageInfo instrs[1024];
 
+static struct {
+    int handle;		/* From mbox_open() */
+    unsigned mem_ref;	/* From mem_alloc() */
+    unsigned bus_addr;	/* From mem_lock() */
+    unsigned char *virt_addr;	/* From mapmem() */ //@Andris: originally uint8_t
+} mbox;
+
+#define BUS_TO_PHYS(x) ((x)&~0xC0000000)
+
 // Get the physical address of a page of virtual memory
 void getRealMemPage(void** vAddr, void** pAddr) {
-    void* a = (void*)valloc(4096);
+    //void* a = (void*)valloc(4096); //@Andris: allocate aligned memory
+    //((int*)a)[0] = 1;  // use page to force allocation. //@Andris: just write something into it to force allocation flag
+    //mlock(a, 4096);  // lock into ram.
+    //*vAddr = a;  // yay - we know the virtual address
 
-    ((int*)a)[0] = 1;  // use page to force allocation.
+    //unsigned long long frameinfo;
 
-    mlock(a, 4096);  // lock into ram.
+    //int fp = open("/proc/self/pagemap", 'r');
+    //lseek(fp, ((long int)a)/4096*8, SEEK_SET);
+    //read(fp, &frameinfo, sizeof(frameinfo));
 
-    *vAddr = a;  // yay - we know the virtual address
+    //*pAddr = (void*)((long int)(frameinfo*4096));
 
-    unsigned long long frameinfo;
-
-    int fp = open("/proc/self/pagemap", 'r');
-    lseek(fp, ((long int)a)/4096*8, SEEK_SET);
-    read(fp, &frameinfo, sizeof(frameinfo));
-
-    *pAddr = (void*)((long int)(frameinfo*4096));
+    //@Andris: should open mbox first!
+    mbox.mem_ref=mem_alloc(mbox.handle, 4096, 4096, MEM_FLAG);
+    mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
+    mbox.virt_addr = (unsigned char*)mapmem(BUS_TO_PHYS(mbox.bus_addr), 4096);
+	//printf("mbox bus_addr=%x virt_addr=%x mem_ref=%x\n",mbox.bus_addr,mbox.virt_addr,mbox.mem_ref);
+    *pAddr = (void*)mbox.bus_addr;
+    *vAddr = mbox.virt_addr;
 }
 
 void freeRealMemPage(void* vAddr) {
-
-    munlock(vAddr, 4096);  // unlock ram.
-
-    free(vAddr);
+	//@Andris: todo
+    //munlock(vAddr, 4096);  // unlock ram.
+    //free(vAddr);
 }
 
 void txon()
@@ -967,6 +987,15 @@ void timeval_print(struct timeval *tv) {
     printf("%s.%03ld", buffer, (tv->tv_usec+500)/1000);
 }
 
+int open_mbox()
+{
+    unlink(DEVICE_FILE_NAME);
+    if (mknod(DEVICE_FILE_NAME, S_IFCHR|0600, makedev(100, 0)) < 0) { printf("Failed to create mailbox device.\n"); return 1; }
+    mbox.handle = mbox_open();
+    if (mbox.handle < 0) { printf("Failed to open mailbox.\n"); return 1; }
+    return 0;
+}
+
 int main(const int argc, char * const argv[]) {
   // Initialize the RNG
   srand(time(NULL));
@@ -1006,6 +1035,7 @@ int main(const int argc, char * const argv[]) {
   int mem_fd;
   char *gpio_mem, *gpio_map;
   volatile unsigned *gpio = NULL;
+  
   setup_io(mem_fd,gpio_mem,gpio_map,gpio);
   setup_gpios(gpio);
   allof7e = (unsigned *)mmap(
@@ -1020,6 +1050,8 @@ int main(const int argc, char * const argv[]) {
     cerr << "Error: mmap error!" << endl;
     ABORT(-1);
   }
+  printf("open_mbox\n");
+  if (open_mbox()) return 1;
   txon();
   struct PageInfo constPage;
   struct PageInfo instrPage;