Initial checkin of unified hierarchy of WPILib 2015

hal/Azalea/src/main/native/cpp/Resource.cpp

@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "HAL/cpp/Resource.h"
+#include "HAL/Errors.h"
+
+ReentrantSemaphore Resource::m_createLock;
+
+/**
+ * Allocate storage for a new instance of Resource.
+ * Allocate a bool array of values that will get initialized to indicate that no resources
+ * have been allocated yet. The indicies of the resources are [0 .. elements - 1].
+ */
+Resource::Resource(uint32_t elements)
+{
+	Synchronized sync(m_createLock);
+	m_size = elements;
+	m_isAllocated = new bool[m_size];
+	for (uint32_t i=0; i < m_size; i++)
+	{
+		m_isAllocated[i] = false;
+	}
+}
+
+/**
+ * Factory method to create a Resource allocation-tracker *if* needed.
+ *
+ * @param r -- address of the caller's Resource pointer. If *r == NULL, this
+ *    will construct a Resource and make *r point to it. If *r != NULL, i.e.
+ *    the caller already has a Resource instance, this won't do anything.
+ * @param elements -- the number of elements for this Resource allocator to
+ *    track, that is, it will allocate resource numbers in the range
+ *    [0 .. elements - 1].
+ */
+/*static*/ void Resource::CreateResourceObject(Resource **r, uint32_t elements)
+{
+	Synchronized sync(m_createLock);
+	if (*r == NULL)
+	{
+		*r = new Resource(elements);
+	}
+}
+
+/**
+ * Delete the allocated array or resources.
+ * This happens when the module is unloaded (provided it was statically allocated).
+ */
+Resource::~Resource()
+{
+	delete[] m_isAllocated;
+}
+
+/**
+ * Allocate a resource.
+ * When a resource is requested, mark it allocated. In this case, a free resource value
+ * within the range is located and returned after it is marked allocated.
+ */
+uint32_t Resource::Allocate(const char *resourceDesc)
+{
+	Synchronized sync(m_allocateLock);
+	for (uint32_t i=0; i < m_size; i++)
+	{
+		if (!m_isAllocated[i])
+		{
+			m_isAllocated[i] = true;
+			return i;
+		}
+	}
+	// TODO: wpi_setWPIErrorWithContext(NoAvailableResources, resourceDesc);
+	return ~0ul;
+}
+
+/**
+ * Allocate a specific resource value.
+ * The user requests a specific resource value, i.e. channel number and it is verified
+ * unallocated, then returned.
+ */
+uint32_t Resource::Allocate(uint32_t index, const char *resourceDesc)
+{
+	Synchronized sync(m_allocateLock);
+	if (index >= m_size)
+	{
+		// TODO: wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
+		return ~0ul;
+	}
+	if ( m_isAllocated[index] )
+	{
+		// TODO: wpi_setWPIErrorWithContext(ResourceAlreadyAllocated, resourceDesc);
+		return ~0ul;
+	}
+	m_isAllocated[index] = true;
+	return index;
+}
+
+
+/**
+ * Free an allocated resource.
+ * After a resource is no longer needed, for example a destructor is called for a channel assignment
+ * class, Free will release the resource value so it can be reused somewhere else in the program.
+ */
+void Resource::Free(uint32_t index)
+{
+	Synchronized sync(m_allocateLock);
+	if (index == ~0ul) return;
+	if (index >= m_size)
+	{
+		// TODO: wpi_setWPIError(NotAllocated);
+		return;
+	}
+	if (!m_isAllocated[index])
+	{
+		// TODO: wpi_setWPIError(NotAllocated);
+		return;
+	}
+	m_isAllocated[index] = false;
+}

hal/Azalea/src/main/native/cpp/StackTrace.cpp

@@ -0,0 +1,165 @@
+
+#include "HAL/cpp/StackTrace.h"
+
+#include "HAL/HAL.h"
+#include "../ChipObject.h"
+#include <stdio.h>
+#include <dbgLib.h>
+#include <taskLib.h>
+#include <symLib.h>
+#include <sysSymTbl.h>
+
+#define DBG_DEMANGLE_PRINT_LEN 256  /* Num chars of demangled names to print */
+
+extern "C"
+{
+	extern char * cplusDemangle (char *source, char *dest, int32_t n);
+}
+
+char *wpi_getLabel(uint16_t addr, int32_t *found)
+{
+	int pVal;
+	SYM_TYPE pType;
+	char name[MAX_SYS_SYM_LEN + 1];
+	static char label[DBG_DEMANGLE_PRINT_LEN + 1 + 11];
+	bzero(label, DBG_DEMANGLE_PRINT_LEN + 1 + 11);
+
+	if (symFindByValue(sysSymTbl, addr, name, &pVal, &pType) == OK)
+	{
+		cplusDemangle(name, label, sizeof(label) - 11);
+		if ((uint16_t)pVal != addr)
+		{
+			sprintf(&label[strlen(label)], "+0x%04x", addr-pVal);
+			if (found) *found = 2;
+		}
+		else
+		{
+			if (found) *found = 1;
+		}
+	}
+	else
+	{
+		sprintf(label, "0x%04x", addr);
+		if (found) *found = 0;
+	}
+
+	return label;
+}
+/*
+static void wpiTracePrint(INSTR *caller, int32_t func, int32_t nargs, int32_t *args, int32_t taskId, BOOL isKernelAdrs)
+{
+	char buf [MAX_SYS_SYM_LEN * 2];
+	int32_t ix;
+	int32_t len = 0;
+	len += sprintf (&buf [len], "%s <%#010x>: ", wpi_getLabel((uint32_t)caller), (int32_t)caller);
+	len += sprintf (&buf [len], "%s <%#010x> (", wpi_getLabel((uint32_t)func), func);
+	for (ix = 0; ix < nargs; ix++)
+	{
+		if (ix != 0)
+			len += sprintf (&buf [len], ", ");
+		len += sprintf (&buf [len], "%#x", args [ix]);
+	}
+	len += sprintf (&buf [len], ")\n");
+
+	printf(buf);
+}
+*/
+static void wpiCleanTracePrint(INSTR *caller, int32_t func, int32_t nargs, int32_t *args, int32_t taskId, BOOL isKernelAdrs)
+{
+	char buf [MAX_SYS_SYM_LEN];
+	int32_t ix;
+	int32_t len = 0;
+	int32_t nameFound = 0;
+	int32_t params = 0;
+	int32_t totalnargs = nargs;
+	char *funcName = wpi_getLabel((uint32_t)func, &nameFound);
+	// Ignore names that are not exact symbol address matches.
+	if (nameFound != 1) return;
+
+	// Ignore internal function name matches.
+	if (strncmp(funcName, "wpi_assert", 10) == 0) return;
+	if (strncmp(funcName, "wpi_fatal", 9) == 0) return;
+	if (strncmp(funcName, "wpi_selfTrace", 13) == 0) return;
+	if (strncmp(funcName, "Error::Set", 10) == 0) return;
+	if (strncmp(funcName, "ErrorBase::SetError", 19) == 0) return;
+	if (strncmp(funcName, "Error::Report", 13) == 0) return;
+
+	// Find the number of arguments in the name string.
+	char *start = strchr(funcName, '(');
+	char *end = strrchr(funcName, ')');
+	if (start + 1 != end && start != NULL)
+	{
+		do
+		{
+			params++;
+			if(strncmp(start+1, "bool", 4) == 0 || strncmp(start+2, "bool", 4) == 0)
+			{
+				totalnargs++;
+			}
+			start = strchr(start + 1, ',');
+		}
+		while(start < end && start != NULL);
+	}
+	char *funcNameEnd = strchr(funcName, '(');
+	*funcNameEnd = 0;
+	len += sprintf (&buf [len], funcName);
+
+	// If this is a member function, print out the this pointer value.
+	if (totalnargs - params == 1)
+	{
+		len += sprintf (&buf [len], "<this=%#lx>", args [0]);
+	}
+
+	// Print out the argument values.
+	len += sprintf (&buf [len], "(");
+	for (ix = totalnargs - params; ix < nargs; ix++)
+	{
+		if (ix != totalnargs - params)
+			len += sprintf (&buf [len], ", ");
+		len += sprintf (&buf [len], "%#lx", args [ix]);
+	}
+	len += sprintf (&buf [len], ")\n");
+
+	printf(buf);
+}
+
+extern "C"
+{
+	extern void trcStack(REG_SET* pRegs, FUNCPTR printRtn, int32_t tid);
+}
+
+static int32_t wpiStackTask(int32_t taskId)
+{
+	delayTicks(1);
+	//tt(taskId);
+
+	REG_SET regs;
+	taskRegsGet(taskId, &regs);
+	trcStack(&regs, (FUNCPTR) wpiCleanTracePrint, taskId);
+	printf("\n");
+
+	// The task should be resumed because it had to be suspended to get the stack trace.
+	taskResume(taskId);
+	return 0;
+}
+
+void printCurrentStackTrace()
+{
+	int priority=100;
+	taskPriorityGet(0, &priority);
+	// Lower priority than the calling task.
+	// Task traceTask("StackTrace", (FUNCPTR)wpiStackTask, priority + 1);
+	// traceTask.Start(taskIdSelf());
+	spawnTask("StackTrace", priority + 1, VXWORKS_FP_TASK, 20000, (FUNCPTR)wpiStackTask, taskIdSelf(),
+			  0, 0, 0, 0, 0, 0, 0, 0, 0);
+
+	// Task to be traced must be suspended for the stack trace to work.
+	taskSuspend(0);
+}
+
+bool getErrnoToName(int32_t errNo, char* name) {
+	int pval;
+	SYM_TYPE ptype;
+	symFindByValue(statSymTbl, errNo, name, &pval, &ptype);
+	return (pval == errNo);
+}

hal/Azalea/src/main/native/cpp/Synchronized.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "HAL/cpp/Synchronized.h"
+
+/**
+ * Synchronized class deals with critical regions.
+ * Declare a Synchronized object at the beginning of a block. That will take the semaphore.
+ * When the code exits from the block it will call the destructor which will give the semaphore.
+ * This ensures that no matter how the block is exited, the semaphore will always be released.
+ * Use the CRITICAL_REGION(SEM_ID) and END_REGION macros to make the code look cleaner (see header file)
+ * @param semaphore The semaphore controlling this critical region.
+ */
+Synchronized::Synchronized(MUTEX_ID semaphore)
+{
+	m_semaphore = semaphore;
+	takeMutex(m_semaphore, SEMAPHORE_WAIT_FOREVER);
+}
+
+Synchronized::Synchronized(ReentrantSemaphore& semaphore)
+{
+	m_semaphore = semaphore.m_semaphore;
+	takeMutex(m_semaphore, SEMAPHORE_WAIT_FOREVER);
+}
+
+/**
+ * This destructor unlocks the semaphore.
+ */
+Synchronized::~Synchronized()
+{
+	giveMutex(m_semaphore);
+}