Mmap wrapper

mmap-wrapper/Makefile

@@ -0,0 +1,15 @@
+CROSS_COMPILE	:= /home/zcy/tools/gcc-linaro-aarch64-linux-gnu-4.9-2014.09_linux/bin/aarch64-linux-gnu-
+CC				:= $(CROSS_COMPILE)gcc
+CFLAGS			:= --static
+
+STM_USER	= stm_user_example
+
+OBJS_STM_USER_API= example.o stm_user_api.o
+
+all: $(STM_USER)
+
+$(STM_USER):$(OBJS_STM_USER_API)
+	$(CC) $(CFLAGS) -o $@ $^
+
+clean:
+	rm *.o $(STM_USER)

mmap-wrapper/example.c

@@ -0,0 +1,45 @@
+#include <termios.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include "stm_user_api.h"
+
+extern struct stm_dev g_stm_dev;
+
+void main()
+{
+	int i;
+	unsigned int chan_start = 32768;
+	unsigned width = PAGE_SIZE / BYTES_PER_CHANNEL;
+	unsigned int flags = STM_FLAG_TIMESTAMPED;
+	unsigned int dsize;
+	char *offset, *data;
+	unsigned int wrbytes = sizeof(unsigned int) * TEST_DATA_SIZE;
+	unsigned int real_wrbytes;
+	unsigned int trace_data[TEST_DATA_SIZE] = {0x5555aaaa, 0xaaaa5555, 0x66666666, 0x99999999};
+
+	if (request_stm_resource(&g_stm_dev, chan_start, width))
+		return;
+
+	/* 
+	 * You can use any channel between [g_stm_dev.policy->channel ...
+	 * (g_stm_dev.policy->channel + g_stm_dev.policy->width)]
+	 * and width must <= (PAGE_SIZE / BYTES_PER_CHANNEL) 
+         * http://lxr.free-electrons.com/source/drivers/hwtracing/stm/core.c?v=4.6#L542
+	 */
+	real_wrbytes = stm_trace_data(&g_stm_dev, chan_start, flags,
+				      wrbytes, trace_data);
+	if (real_wrbytes != wrbytes)
+		printf("write %d bytes and left % bytes data\n",
+		       real_wrbytes, wrbytes - real_wrbytes);
+
+	printf("Success to write %d bytes\n", real_wrbytes);
+
+	release_stm_resource(&g_stm_dev);
+}

mmap-wrapper/readme

@@ -0,0 +1,28 @@
+Before compiling the program, you must change the items below accroding to your environment:
+
+- 'CROSS_COMPILE' in Makefile
+- STM device name in stm_user_api.h
+- TMC device name in stm_user_api.h
+
+
+And then you may need to mount configfs manually:
+/ # mount -t configfs none /config
+
+
+Create policy directory for STM:
+/ # mkdir /config/stp-policy/10006000.stm.abc
+
+NOTE: 
+The STM directory name consists of the device name to which it applies and the actual policy name,
+separated by a dot.
+The device name (say 10006000.stm) have to be matched the name of STM device under /dev.
+The suffix is a random string without any dot
+
+
+Create policy:
+/ # mkdir /config/stp-policy/10006000.stm.abc/test
+/ # echo 32768 65535 > /config/stp-policy/10006000.stm.abc/test/channels
+
+'chan_start' (here is 32768) must be same with the corresponding value in example.c
+
+

mmap-wrapper/stm_user_api.c

@@ -0,0 +1,223 @@
+#include <termios.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include "stm_user_api.h"
+
+void release_stm_resource(struct stm_dev *dev);
+
+static void enable_sink(const char *dev_name, unsigned int enable)
+{
+	char buf[256] = {0};
+	sprintf(buf, "echo %u > /sys/bus/coresight/devices/%s/enable_sink",
+		enable, dev_name);
+	system(buf);
+}
+
+static int set_policy(int fd, struct stp_policy_id *policy,
+		      unsigned int chan, unsigned int width)
+{
+	unsigned int chan_perpage = PAGE_SIZE / BYTES_PER_CHANNEL;
+
+	if(width % chan_perpage || chan % chan_perpage) {
+		chan = chan / chan_perpage;
+		width = (width / chan_perpage + 1) * BYTES_PER_CHANNEL;
+	}
+
+	policy->channel = chan;
+	policy->__reserved_0 = 0;
+	policy->__reserved_1 = 0;
+	policy->width = width;
+	policy->size = sizeof(struct stp_policy_id) + POLICY_NAME_LEN;
+	memcpy(policy->id, STP_POLICY_NAME, POLICY_NAME_LEN);
+
+	if (ioctl(fd, STP_POLICY_ID_SET, policy) == -1) {
+		printf("STP_POLICY_ID_SET failed %s %d\n",
+		       strerror(errno), errno);
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * dev	- storing the information of stimulus resources and STM device
+ * chan	- the start index of channels
+ * width- the number of channels for request
+ */
+int request_stm_resource(struct stm_dev *dev, unsigned int chan,
+			 unsigned int width)
+{
+	int fd;
+	int ret = 0;
+	char *map;
+	struct stp_policy_id *policy;
+	unsigned int length = STM_MAP_SIZE;
+	unsigned long offset = 0;
+
+	if ((fd = open(STM_DEVICE_NAME, O_RDWR | O_SYNC)) == -1) {
+		printf("Failed to open %s %s\n", STM_DEVICE_NAME,
+			strerror(errno));
+		return -1;
+	}
+	dev->fd = fd;
+
+	/*
+	 * Before allocating a policy for STM, the sink connected with STM must
+	 * be enabled.
+	 */
+	enable_sink(ETF_SYS_NAME, 1);
+	/* enable_sink(ETR_SYS_NAME, 1); */
+
+	/* set a master/channel policy for this STM device, this
+	 * is because that kernel have to know how many channels
+	 * would be mapped, and the size of mapped memory must be
+	 * a multiple of page size.
+	 */
+	dev->policy = malloc(sizeof(struct stp_policy_id) + POLICY_NAME_LEN);
+	if (!dev->policy) {
+		ret = -1;
+		printf("Failed to malloc policy.\n");
+		goto out;
+	}
+
+	if (set_policy(fd, dev->policy, chan, width)) {
+		ret = -1;
+		printf("Failed to set policy.\n");
+		goto out;
+	}
+
+	map = (char *)mmap(0, length, PROT_READ|PROT_WRITE,
+			   MAP_SHARED, fd, offset);
+	if (map == MAP_FAILED) {
+		ret = -1;
+		printf("Failed to map %s\n", strerror(errno));
+		goto out;
+	}
+
+	dev->mmap.map = map;
+	dev->mmap.start = 0;
+	dev->mmap.length = length;
+	printf("Success to map channel(%u~%u) to 0x%lx\n",
+		dev->policy->channel,
+		(dev->policy->width + dev->policy->channel - 1),
+		(unsigned long)map);
+
+	return ret;
+
+out:
+	release_stm_resource(dev);
+	return ret;
+}
+
+void release_stm_resource(struct stm_dev *dev)
+{
+	/* unmap the area & error checking */
+	if (dev->mmap.map) {
+		if (munmap(dev->mmap.map, dev->mmap.length) == -1)
+			perror("user: Error un-mmapping the file");
+		dev->mmap.map = NULL;
+	}
+	if (dev->policy) {
+		free(dev->policy);
+		dev->policy = NULL;
+	}
+	if (dev->fd) {
+		close(dev->fd);
+		dev->fd = 0;
+	}
+}
+
+static char *stm_channel_addr(struct stm_dev *dev, unsigned int chan,
+			      unsigned int flags, unsigned int type)
+{
+	if (chan < dev->policy->channel ||
+	    chan >= dev->policy->channel + dev->policy->width) {
+		printf("Channel index should be in [%u...%u]\n",
+		       dev->policy->channel,
+		       dev->policy->channel + dev->policy->width);
+		return NULL;
+	}
+
+	chan -= dev->policy->channel;
+
+	return (char *)(((unsigned long)dev->mmap.map +
+			 chan * BYTES_PER_CHANNEL) |
+			((~flags) & type));
+}
+
+static unsigned int stm_dsize(const char *dev_name)
+{
+	FILE *file;
+	unsigned int size = 0;
+	char buf[128] = {0};
+	char spfeat2r[16] = {0};
+	int dsize = 0;
+
+	sprintf(buf, "/sys/bus/coresight/devices/%s/mgmt/spfeat2r", dev_name);
+	file = fopen(buf, "r");
+	/* 
+	 * the first two characters in file are '0x', like:
+	 * # cat /sys/bus/coresight/devices/10006000.stm/mgmt/spfeat2r 
+	 * 0x104f2
+	 */
+	fseek(file, 2, SEEK_END);
+	size = ftell (file);
+	fseek(file, 2, SEEK_SET);
+	if (fgets(spfeat2r, size, file))
+		dsize = strtol(spfeat2r, NULL, 16);
+	fclose(file);
+
+	return (dsize >> 12) & 0xf;
+}
+
+unsigned int stm_wrbytes(const char *dev_name)
+{
+	/* 
+	 * Fundamental data size:
+	 * 0b0001 - 64-bit data.
+	 */
+	return stm_dsize(dev_name) ? 8: 4;
+}
+
+static unsigned int stm_write(char *addr, void *data, unsigned int size)
+{
+	unsigned int wrbytes = stm_wrbytes(STM_SYS_NAME);
+	if (size > wrbytes)
+		size = wrbytes;
+
+	memcpy(addr, (char *)data, size);
+	printf("memcpy %u bytes data to the address %p\n", size ,addr);
+	return size;
+}
+
+int stm_trace_data(struct stm_dev *dev, unsigned int chan, int flags,
+		   unsigned int size, void *data)
+{
+	int i = 0;
+	char *addr = stm_channel_addr(dev, chan, flags, STM_PKT_TYPE_DATA);
+	unsigned int real_wrbytes, len = size;
+	char *pdata = data, nil = 0;
+
+	if (!addr)
+		return -1;
+
+	do {
+		real_wrbytes = stm_write(addr, pdata, len);
+		pdata += real_wrbytes;
+		len -= real_wrbytes;
+		if (++i == 1)
+			addr = stm_channel_addr(dev, chan, 0, STM_PKT_TYPE_DATA);
+	} while(len);
+
+	addr = stm_channel_addr(dev, chan, 0, STM_PKT_TYPE_FLAG);
+	stm_write(addr, &nil, 1);
+
+	return size;
+}

mmap-wrapper/stm_user_api.h

@@ -0,0 +1,68 @@
+#ifndef __STM_USER_API_H
+#define __STM_USER_API_H
+
+#define BYTES_PER_CHANNEL	256
+#define PAGE_SIZE		sysconf(_SC_PAGE_SIZE) //4096
+#define MAP_MASK		(PAGE_SIZE - 1)
+#define STM_MAP_SIZE		PAGE_SIZE
+
+#define STM_DEVICE_NAME		"/dev/10006000.stm"
+#define ETF_SYS_NAME		"10003000.etf"
+/* Juno ETR */
+/*#define ETR_SYS_NAME		"20070000.etr"*/
+#define STM_SYS_NAME		"10006000.stm"
+
+#define STP_POLICY_NAME		"test"
+#define TEST_DATA_SIZE		4
+#define POLICY_NAME_LEN		8
+
+#define STP_POLICY_ID_SET	_IOWR('%', 0, struct stp_policy_id)
+
+enum stm_flags {
+	STM_FLAG_TIMESTAMPED	= 0x08,
+	STM_FLAG_MARKED		= 0x10,
+	STM_FLAG_GUARANTEED	= 0x80,
+};
+
+enum stm_pkt_type {
+	STM_PKT_TYPE_DATA	= 0x98,
+	STM_PKT_TYPE_FLAG	= 0xE8,
+	STM_PKT_TYPE_TRIG	= 0xF8,
+};
+
+enum error_no {
+	E_COMMON = -1,
+};
+
+struct stp_policy_id {
+	unsigned int	size;
+	unsigned short	master;
+	unsigned short	channel;
+	unsigned short	width;
+	/* padding */
+	unsigned short	__reserved_0;
+	unsigned int	__reserved_1;
+	/* policy name */
+	char		id[0];
+};
+
+struct mem_map {
+	unsigned long start;
+	unsigned long length;
+	char *map;
+};
+
+struct stm_dev {
+	int fd;
+	struct stp_policy_id *policy;
+	struct mem_map mmap;
+} g_stm_dev;
+
+unsigned int stm_wrbytes(const char *dev_name);
+int request_stm_resource(struct stm_dev *dev, unsigned int chan,
+			 unsigned int width);
+void release_stm_reaource(struct stm_dev *dev);
+int stm_trace_data(struct stm_dev *dev, unsigned int chan,
+		   int flags, unsigned int size, void *data);
+
+#endif /* __STM_USER_API_H */