evb-pru0: implement tachometer

copied from ev3-pru1 and modified for BeagleBone

evb-pru0/.project

@@ -3,6 +3,7 @@
 	<name>evb-pru0</name>
 	<comment></comment>
 	<projects>
+		<project>rpmsg_lib</project>
 	</projects>
 	<buildSpec>
 		<buildCommand>

evb-pru0/main.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2015 Texas Instruments Incorporated - http://www.ti.com/ 
+ * Copyright (C) 2018 David Lechner <david@lechnology.com>
  *  
  *  
  * Redistribution and use in source and binary forms, with or without 
@@ -31,9 +31,213 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <stdbool.h>
 #include <stdint.h>
-#include "resource_table_empty.h"
+#include <stdio.h>
+
+#include <rsc_types.h>
+
+#include <pru_cfg.h>
+#include <pru_ctrl.h>
+#include <pru_intc.h>
+#include <pru_rpmsg.h>
+
+#include <sys_gpio.h>
+#include <sys_timer_1ms.h>
+
+#include "resource_table.h"
+
+/* from Linux */
+
+#define EV3_PRU_TACHO_RING_BUF_SIZE 256 /* must be power of 2! */
+
+enum ev3_pru_tacho_msg_type {
+	/* Host >< PRU: request memory map address of ring buffer data */
+	EV3_PRU_TACHO_MSG_DATA_ADDR,
+};
+
+struct ev3_pru_tacho_msg {
+	uint32_t type;
+	uint32_t value;
+};
+
+enum ev3_pru_tacho {
+	EV3_PRU_TACHO_A,
+	EV3_PRU_TACHO_B,
+	EV3_PRU_TACHO_C,
+	EV3_PRU_TACHO_D,
+	NUM_EV3_PRU_TACHO
+};
+
+struct ev3_pru_tacho_remote_data {
+	uint32_t position[NUM_EV3_PRU_TACHO][EV3_PRU_TACHO_RING_BUF_SIZE];
+	uint32_t timestamp[NUM_EV3_PRU_TACHO][EV3_PRU_TACHO_RING_BUF_SIZE];
+	uint32_t head[NUM_EV3_PRU_TACHO];
+};
+
+/* end Linux */
+
+enum direction {
+	REVERSE = -1,
+	UNKNOWN = 0,
+	FORWARD = 1
+};
+
+/* Host-0 Interrupt sets bit 30 in register R31 */
+#define HOST_INT			((uint32_t) 1 << 30)
+
+/* The PRU-ICSS system events used for RPMsg are defined in the Linux device tree
+ * PRU0 uses system event 16 (To ARM) and 17 (From ARM)
+ * PRU1 uses system event 18 (To ARM) and 19 (From ARM)
+ */
+#define TO_ARM_HOST			16
+#define FROM_ARM_HOST			17
+
+#define PRU_GLOBAL_BASE_ADDR		0x4a300000
+#define PRU_SRAM __far __attribute__((cregister("PRU_SHAREDMEM", far)))
+
+/*
+ * Used to make sure the Linux drivers are ready for RPMsg communication
+ * Found at linux-x.y.z/include/uapi/linux/virtio_config.h
+ */
+#define VIRTIO_CONFIG_S_DRIVER_OK	4
+
+volatile register uint32_t __R31;
+
+#if defined(PLAT_EVB)
+
+#define INTA GPIO2.GPIO_DATAIN_bit.DATAIN4
+#define INTB GPIO0.GPIO_DATAIN_bit.DATAIN26
+#define INTC GPIO0.GPIO_DATAIN_bit.DATAIN27
+#define INTD GPIO2.GPIO_DATAIN_bit.DATAIN11
+
+#define DIRA GPIO2.GPIO_DATAIN_bit.DATAIN3
+#define DIRB GPIO1.GPIO_DATAIN_bit.DATAIN12
+#define DIRC GPIO1.GPIO_DATAIN_bit.DATAIN31
+#define DIRD GPIO2.GPIO_DATAIN_bit.DATAIN9
+
+#elif defined(PLAT_QUEST)
+
+#define INTA GPIO2.GPIO_DATAIN_bit.DATAIN4
+#define INTB GPIO0.GPIO_DATAIN_bit.DATAIN26
+#define INTC GPIO0.GPIO_DATAIN_bit.DATAIN27
+#define INTD GPIO2.GPIO_DATAIN_bit.DATAIN12
+
+#define DIRA GPIO2.GPIO_DATAIN_bit.DATAIN3
+#define DIRB GPIO1.GPIO_DATAIN_bit.DATAIN12
+#define DIRC GPIO1.GPIO_DATAIN_bit.DATAIN31
+#define DIRD GPIO2.GPIO_DATAIN_bit.DATAIN13
+
+#else
+#error Must define PLAT_xyz
+#endif
+
+#define TACHO_STATE(x) ((INT##x << 1) | DIR##x)
+
+static uint8_t tacho_state[NUM_EV3_PRU_TACHO];
+static uint32_t tacho_prev_timestamp[NUM_EV3_PRU_TACHO];
+static uint32_t tacho_counts[NUM_EV3_PRU_TACHO];
+
+static uint8_t payload[RPMSG_BUF_SIZE];
+static PRU_SRAM struct ev3_pru_tacho_remote_data remote_data;
+
+static void update_tacho_state(enum ev3_pru_tacho idx, uint8_t new_state)
+{
+	uint8_t current_state = tacho_state[idx] & 0x3;
+	enum direction new_dir = UNKNOWN;
+	uint32_t now, elapsed;
+
+	switch (current_state) {
+		case 0x0:
+			if (new_state == 0x1) {
+				new_dir = FORWARD;
+			} else if (new_state == 0x2) {
+				new_dir = REVERSE;
+			}
+			break;
+
+		case 0x1:
+			if (new_state == 0x3) {
+				new_dir = FORWARD;
+			} else if (new_state == 0x0) {
+				new_dir = REVERSE;
+			}
+			break;
+
+		case 0x3:
+			if (new_state == 0x2) {
+				new_dir = FORWARD;
+			} else if (new_state == 0x1) {
+				new_dir = REVERSE;
+			}
+			break;
+
+		case 0x2:
+			if (new_state == 0x0) {
+				new_dir = FORWARD;
+			} else if (new_state == 0x3) {
+				new_dir = REVERSE;
+			}
+			break;
+	}
+
+	tacho_state[idx] = new_state;
+	tacho_counts[idx] += new_dir;
+
+	now = DMTIMER1_1MS.TCRR;
+	elapsed = now - tacho_prev_timestamp[idx];
+
+	// if there was a change in count or if count hasn't changed for 50ms
+	if (new_dir || elapsed > (50 * 1000000 * 3 / 125)) {
+		uint32_t new_head = (remote_data.head[idx] + 1) & (EV3_PRU_TACHO_RING_BUF_SIZE - 1);
+
+		tacho_prev_timestamp[idx] = now;
+
+		remote_data.position[idx][new_head] = tacho_counts[idx];
+		remote_data.timestamp[idx][new_head] = now;
+		remote_data.head[idx] = new_head;
+	}
+}
 
 int main(void) {
-	__halt();
+	volatile uint8_t *status;
+	struct pru_rpmsg_transport transport;
+	uint16_t src, dst, len;
+
+	// Clear the status of the PRU-system event that the ARM will use to 'kick' us
+	CT_INTC.SICR_bit.STS_CLR_IDX = FROM_ARM_HOST;
+
+	// Wait until Linux gives us the OK that the driver is loaded
+	status = &resource_table.rpmsg_vdev.status;
+	while (!(*status & VIRTIO_CONFIG_S_DRIVER_OK));
+
+	pru_virtqueue_init(&transport.virtqueue0, &resource_table.rpmsg_vring0, TO_ARM_HOST, FROM_ARM_HOST);
+	pru_virtqueue_init(&transport.virtqueue1, &resource_table.rpmsg_vring1, TO_ARM_HOST, FROM_ARM_HOST);
+
+	while (pru_rpmsg_channel(RPMSG_NS_CREATE, &transport, "ev3-tacho-rpmsg", "", 0) != PRU_RPMSG_SUCCESS);
+
+	while (true) {
+		// wait for the ARM to kick us
+		while (!(__R31 & HOST_INT)) {
+			update_tacho_state(EV3_PRU_TACHO_A, TACHO_STATE(A));
+			update_tacho_state(EV3_PRU_TACHO_B, TACHO_STATE(B));
+			update_tacho_state(EV3_PRU_TACHO_C, TACHO_STATE(C));
+			update_tacho_state(EV3_PRU_TACHO_D, TACHO_STATE(D));
+		}
+
+		// clear the interrupt
+		CT_INTC.SICR_bit.STS_CLR_IDX = FROM_ARM_HOST;
+
+		// Receive all available messages, multiple messages can be sent per kick
+		while (pru_rpmsg_receive(&transport, &src, &dst, payload, &len) == PRU_RPMSG_SUCCESS) {
+			struct ev3_pru_tacho_msg *msg = (void *)payload;
+
+			switch (msg->type) {
+			case EV3_PRU_TACHO_MSG_DATA_ADDR:
+				msg->value = PRU_GLOBAL_BASE_ADDR + (uint32_t)&remote_data;
+				pru_rpmsg_send(&transport, dst, src, msg, sizeof(*msg));
+				break;
+			}
+		}
+	}
 }

evb-pru0/resource_table.h

@@ -0,0 +1,147 @@
+/*
+ * Copyright (C) 2016 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * 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 Texas Instruments Incorporated 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
+ * OWNER 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.
+ */
+
+#ifndef _RSC_TABLE_PRU_H_
+#define _RSC_TABLE_PRU_H_
+
+#include <stddef.h>
+#include <rsc_types.h>
+#include "pru_virtio_ids.h"
+
+/*
+ * Sizes of the virtqueues (expressed in number of buffers supported,
+ * and must be power of 2)
+ */
+#define PRU_RPMSG_VQ0_SIZE	16
+#define PRU_RPMSG_VQ1_SIZE	16
+
+/*
+ * The feature bitmap for virtio rpmsg
+ */
+#define VIRTIO_RPMSG_F_NS	0		//name service notifications
+
+/* This firmware supports name service notifications as one of its features */
+#define RPMSG_PRU_C0_FEATURES	(1 << VIRTIO_RPMSG_F_NS)
+
+/* Definition for unused interrupts */
+#define HOST_UNUSED		255
+
+/* Number of entries in the table */
+#define NUM_RESORUCES	2
+
+/* Mapping sysevts to a channel. Each pair contains a sysevt, channel. */
+struct ch_map pru_intc_map[] = {
+	{ .evt = 16, .ch = 2 },
+	{ .evt = 17, .ch = 0 },
+};
+
+struct pru0_resource_table {
+	struct resource_table base;
+
+	uint32_t offset[NUM_RESORUCES];
+
+	/* rpmsg vdev entry */
+	struct fw_rsc_vdev rpmsg_vdev;
+	struct fw_rsc_vdev_vring rpmsg_vring0;
+	struct fw_rsc_vdev_vring rpmsg_vring1;
+
+	/* intc definition */
+	struct fw_rsc_custom pru_ints;
+};
+
+#pragma DATA_SECTION(resource_table, ".resource_table")
+#pragma RETAIN(resource_table)
+struct pru0_resource_table resource_table = {
+	.base = {
+		.ver = 1,	/* only version 1 is supported by the current driver */
+		.num = NUM_RESORUCES,
+		.reserved = { 0, 0 },	/* must be zero */
+	},
+	/* offsets to entries */
+	.offset = {
+		offsetof(struct pru0_resource_table, rpmsg_vdev),
+		offsetof(struct pru0_resource_table, pru_ints),
+	},
+
+	/* rpmsg vdev entry */
+	.rpmsg_vdev = {
+		.type          = TYPE_VDEV,
+		.id            = VIRTIO_ID_RPMSG,
+		.notifyid      = 0,
+		.dfeatures     = RPMSG_PRU_C0_FEATURES,
+		.gfeatures     = 0,
+		.config_len    = 0,
+		.status        = 0,
+		.num_of_vrings = 2, // only two is supported
+		.reserved      = { 0, 0 },
+	},
+	/* the two vrings */
+	.rpmsg_vring0 = {
+		.da       = 0,                      //will be populated by host, can't pass it in
+		.align    = 16,                     //(bytes),
+		.num      = PRU_RPMSG_VQ0_SIZE,     //num of descriptors
+		.notifyid = 0,                      //will be populated, can't pass right now
+		.reserved = 0,
+	},
+	.rpmsg_vring1 = {
+		.da       = 0,                      //will be populated by host, can't pass it in
+		.align    = 16,                     //(bytes),
+		.num      = PRU_RPMSG_VQ1_SIZE,     //num of descriptors
+		.notifyid = 0,                      //will be populated, can't pass right now
+		.reserved = 0,
+	},
+
+	.pru_ints = {
+		.type = TYPE_POSTLOAD_VENDOR,
+		.sub_type = TYPE_PRU_INTS,
+		.rsc_size = sizeof(struct fw_rsc_custom_ints),
+		.rsc.pru_ints = {
+			.version = 0x0000,
+			.channel_host = {
+				0,
+				HOST_UNUSED,
+				2,
+				HOST_UNUSED,
+				HOST_UNUSED,
+				HOST_UNUSED,
+				HOST_UNUSED,
+				HOST_UNUSED,
+				HOST_UNUSED,
+				HOST_UNUSED,
+			},
+			.num_evts = (sizeof(pru_intc_map) / sizeof(struct ch_map)),
+			.event_channel = pru_intc_map,
+		},
+	},
+};
+
+#endif /* _RSC_TABLE_PRU_H_ */