Initial checkin of unified hierarchy of WPILib 2015

networktables/cpp/Azalea/src/main/native/OSAL/Synchronized.cpp

@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "OSAL/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.
+ */
+NTSynchronized::NTSynchronized(SEM_ID semaphore)
+{
+	usingSem = false;
+	m_semaphore = semaphore;
+	semTake(m_semaphore, WAIT_FOREVER);
+}
+
+NTSynchronized::NTSynchronized(NTReentrantSemaphore& sem)
+{
+	usingSem = true;
+	m_sem = &sem;
+	m_sem->take();
+}
+
+/**
+ * Syncronized destructor.
+ * This destructor frees the semaphore ensuring that the resource is freed for the block
+ * containing the Synchronized object.
+ */
+NTSynchronized::~NTSynchronized()
+{
+	if(usingSem)
+		m_sem->give();
+	else
+		semGive(m_semaphore);
+}

networktables/cpp/Azalea/src/main/native/OSAL/Task.cpp

@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "OSAL/Task.h"
+
+#include <errnoLib.h>
+#include <string.h>
+#include <taskLib.h>
+#include <usrLib.h>
+
+const UINT32 NTTask::kDefaultPriority;
+const INT32 NTTask::kInvalidTaskID;
+
+/**
+ * Create but don't launch a task.
+ * @param name The name of the task.  "FRC_" will be prepended to the task name.
+ * @param function The address of the function to run as the new task.
+ * @param priority The VxWorks priority for the task.
+ * @param stackSize The size of the stack for the task
+ */
+NTTask::NTTask(const char* name, FUNCPTR function, INT32 priority, UINT32 stackSize)
+{
+	m_taskID = kInvalidTaskID;
+	m_function = function;
+	m_priority = priority;
+	m_stackSize = stackSize;
+	m_taskName = new char[strlen(name) + 5];
+	strcpy(m_taskName, "FRC_");
+	strcpy(m_taskName+4, name);
+
+	static INT32 instances = 0;
+	instances++;
+	//nUsageReporting::report(nUsageReporting::kResourceType_Task, instances, 0, m_taskName);
+}
+
+NTTask::~NTTask()
+{
+	if (m_taskID != kInvalidTaskID) Stop();
+	delete [] m_taskName;
+	m_taskName = NULL;
+}
+
+/**
+ * Starts this task.
+ * If it is already running or unable to start, it fails and returns false.
+ */
+bool NTTask::Start(UINT32 arg0, UINT32 arg1, UINT32 arg2, UINT32 arg3, UINT32 arg4, 
+		UINT32 arg5, UINT32 arg6, UINT32 arg7, UINT32 arg8, UINT32 arg9)
+{
+	m_taskID = taskSpawn(m_taskName,
+						m_priority,
+						VX_FP_TASK,							// options
+						m_stackSize,						// stack size
+						m_function,							// function to start
+						arg0, arg1, arg2, arg3, arg4,	// parameter 1 - pointer to this class
+						arg5, arg6, arg7, arg8, arg9);// additional unused parameters
+	bool ok = HandleError(m_taskID);
+	if (!ok) m_taskID = kInvalidTaskID;
+	return ok;
+}
+
+/**
+ * Restarts a running task.
+ * If the task isn't started, it starts it.
+ * @return false if the task is running and we are unable to kill the previous instance
+ */
+bool NTTask::Restart()
+{
+	return HandleError(taskRestart(m_taskID));
+}
+
+/**
+ * Kills the running task.
+ * @returns true on success false if the task doesn't exist or we are unable to kill it.
+ */
+bool NTTask::Stop()
+{
+	bool ok = true;
+	if (Verify())
+	{
+		ok = HandleError(taskDelete(m_taskID));
+	}
+	m_taskID = kInvalidTaskID;
+	return ok;
+}
+
+/**
+ * Returns true if the task is ready to execute (i.e. not suspended, delayed, or blocked).
+ * @return true if ready, false if not ready.
+ */
+bool NTTask::IsReady()
+{
+	return taskIsReady(m_taskID);
+}
+
+/**
+ * Returns true if the task was explicitly suspended by calling Suspend()
+ * @return true if suspended, false if not suspended.
+ */
+bool NTTask::IsSuspended()
+{
+	return taskIsSuspended(m_taskID);
+}
+
+/**
+ * Pauses a running task.
+ * Returns true on success, false if unable to pause or the task isn't running.
+ */
+bool NTTask::Suspend()
+{
+	return HandleError(taskSuspend(m_taskID));
+}
+
+/**
+ * Resumes a paused task.
+ * Returns true on success, false if unable to resume or if the task isn't running/paused.
+ */
+bool NTTask::Resume()
+{
+	return HandleError(taskResume(m_taskID));
+}
+
+/**
+ * Verifies a task still exists.
+ * @returns true on success.
+ */
+bool NTTask::Verify()
+{
+	return taskIdVerify(m_taskID) == OK;
+}
+
+/**
+ * Gets the priority of a task.
+ * @returns task priority or 0 if an error occured
+ */
+INT32 NTTask::GetPriority()
+{
+	if (HandleError(taskPriorityGet(m_taskID, &m_priority)))
+		return m_priority;
+	else
+		return 0;
+}
+
+/**
+ * This routine changes a task's priority to a specified priority.
+ * Priorities range from 0, the highest priority, to 255, the lowest priority.
+ * Default task priority is 100.
+ * @param priority The priority the task should run at.
+ * @returns true on success.
+ */
+bool NTTask::SetPriority(INT32 priority)
+{
+	m_priority = priority;
+	return HandleError(taskPrioritySet(m_taskID, m_priority));
+}
+
+/**
+ * Returns the name of the task.
+ * @returns Pointer to the name of the task or NULL if not allocated
+ */
+const char* NTTask::GetName()
+{
+	return m_taskName;
+}
+
+/**
+ * Get the ID of a task
+ * @returns Task ID of this task.  Task::kInvalidTaskID (-1) if the task has not been started or has already exited.
+ */
+INT32 NTTask::GetID()
+{
+	if (Verify())
+		return m_taskID;
+	return kInvalidTaskID;
+}
+
+/**
+ * Handles errors generated by task related code.
+ */
+bool NTTask::HandleError(STATUS results)
+{
+	if (results != ERROR) return true;
+	switch(errnoGet())
+	{
+	case S_objLib_OBJ_ID_ERROR:
+		wpi_setWPIErrorWithContext(TaskIDError, m_taskName);
+		break;
+		
+	case S_objLib_OBJ_DELETED:
+		wpi_setWPIErrorWithContext(TaskDeletedError, m_taskName);
+		break;
+		
+	case S_taskLib_ILLEGAL_OPTIONS:
+		wpi_setWPIErrorWithContext(TaskOptionsError, m_taskName);
+		break;
+		
+	case S_memLib_NOT_ENOUGH_MEMORY:
+		wpi_setWPIErrorWithContext(TaskMemoryError, m_taskName);
+		break;
+		
+	case S_taskLib_ILLEGAL_PRIORITY:
+		wpi_setWPIErrorWithContext(TaskPriorityError, m_taskName);
+		break;
+
+	default:
+		printErrno(errnoGet());
+		wpi_setWPIErrorWithContext(TaskError, m_taskName);
+	}
+	return false;
+}
+

networktables/cpp/Azalea/src/main/native/networktables2/stream/FDIOStream.cpp

@@ -0,0 +1,84 @@
+/*
+ * FDIOStream.cpp
+ *
+ *  Created on: Sep 27, 2012
+ *      Author: Mitchell Wills
+ */
+
+#include "networktables2/stream/FDIOStream.h"
+#include "networktables2/util/IOException.h"
+#include "networktables2/util/EOFException.h"
+
+#include <errno.h>
+#include <stdlib.h>
+#include <iolib.h>
+#include <selectLib.h>
+#include <string.h>
+#include <stdio.h>
+
+
+FDIOStream::FDIOStream(int _fd){
+  fd = _fd;
+  //	f = fdopen(_fd, "rbwb");
+  //	if(f==NULL)
+  //		throw IOException("Could not open stream from file descriptor", errno);
+}
+FDIOStream::~FDIOStream(){
+	close();
+}
+
+int FDIOStream::read(void* ptr, int numbytes){
+	if(numbytes==0)
+		return 0;
+	char* bufferPointer = (char*)ptr;
+	int totalRead = 0;
+
+	struct timeval timeout;
+	fd_set fdSet;
+	
+	while (totalRead < numbytes) {
+		FD_ZERO(&fdSet);
+		FD_SET(fd, &fdSet);
+		timeout.tv_sec = 1;
+		timeout.tv_usec = 0;
+		int select_result = select(FD_SETSIZE, &fdSet, NULL, NULL, &timeout);
+		if ( select_result < 0)
+		  throw IOException("Select returned an error on read");
+
+		int numRead = 0;
+		if (FD_ISSET(fd, &fdSet)) {
+		  numRead = ::read(fd, bufferPointer, numbytes-totalRead);
+
+		  if(numRead == 0){
+		    throw EOFException();
+		  }
+		  else if (numRead < 0) {
+		    perror("read error: ");
+		    fflush(stderr);
+		    throw IOException("Error on FDIO read");
+		  }
+		  bufferPointer += numRead;
+		  totalRead += numRead;
+		}
+	}
+	return totalRead;
+}
+int FDIOStream::write(const void* ptr, int numbytes){
+  int numWrote = ::write(fd, (char*)ptr, numbytes);//TODO: this is bad
+  //int numWrote = fwrite(ptr, 1, numbytes, f);
+  if(numWrote==numbytes)
+    return numWrote;
+  perror("write error: ");
+  fflush(stderr);
+  throw IOException("Could not write all bytes to fd stream");
+	
+}
+void FDIOStream::flush(){
+  //if(fflush(f)==EOF)
+  //  throw EOFException();
+}
+void FDIOStream::close(){
+  //fclose(f);//ignore any errors closing
+  ::close(fd);
+}
+

networktables/cpp/Azalea/src/main/native/networktables2/stream/SocketServerStreamProvider.cpp

@@ -0,0 +1,127 @@
+/*
+ * SocketServerStreamProvider.cpp
+ *
+ *  Created on: Sep 27, 2012
+ *      Author: Mitchell Wills
+ */
+
+#include "networktables2/stream/SocketServerStreamProvider.h"
+#include "networktables2/stream/FDIOStream.h"
+#include "networktables2/util/IOException.h"
+
+#include <strings.h>
+#include <cstring>
+#include <errno.h>
+#ifdef _WRS_KERNEL
+#include <inetLib.h>
+#include <selectLib.h>
+#include <sockLib.h>
+#include <taskLib.h>
+#include <usrLib.h>
+#include <ioLib.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#else
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/types.h>
+#include <unistd.h>
+#ifdef WIN32
+#include <windows.h>
+#include <winsock.h>
+#include <winsock2.h>
+#include <wininet.h>
+#include <ws2tcpip.h>
+#else
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#endif
+#endif
+
+#ifndef _WRS_KERNEL
+#define ERROR -1
+#endif
+
+#if defined(WIN32) || defined(_WRS_KERNEL)
+typedef int addrlen_t;
+#else
+typedef socklen_t addrlen_t;
+#endif
+
+
+SocketServerStreamProvider::SocketServerStreamProvider(int port){
+	struct sockaddr_in serverAddr;
+	int sockAddrSize = sizeof(serverAddr);
+	memset(&serverAddr, 0, sockAddrSize);
+
+#ifdef _WRS_KERNEL
+	serverAddr.sin_len = (u_char)sockAddrSize;
+#endif
+	serverAddr.sin_family = AF_INET;
+	serverAddr.sin_port = htons(port);
+	serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
+
+	if ((serverSocket = socket(AF_INET, SOCK_STREAM, 0)) == ERROR)
+	{
+		throw IOException("Error creating server socket", errno);
+	}
+
+	// Set the TCP socket so that it can be reused if it is in the wait state.
+	int reuseAddr = 1;
+	setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, (char *)&reuseAddr, sizeof(reuseAddr));
+
+	// Bind socket to local address.
+	if (bind(serverSocket, (struct sockaddr *)&serverAddr, sockAddrSize) == ERROR)
+	{
+		::close(serverSocket);
+		throw IOException("Could not bind server socket", errno);
+	}
+
+	if (listen(serverSocket, 1) == ERROR)
+	{
+		::close(serverSocket);
+		throw IOException("Could not listen on server socket", errno);
+	}
+}
+SocketServerStreamProvider::~SocketServerStreamProvider(){
+	close();
+}
+
+
+IOStream* SocketServerStreamProvider::accept(){
+	struct timeval timeout;
+	// Check for a shutdown once per second
+	while (true)
+	{
+		fd_set fdSet;
+
+		FD_ZERO(&fdSet);
+		FD_SET(serverSocket, &fdSet);
+		timeout.tv_sec = 1;
+		timeout.tv_usec = 0;
+		int select_result = select(FD_SETSIZE, &fdSet, NULL, NULL, &timeout);
+		if ( select_result < 0)
+		  return NULL;
+
+		if (FD_ISSET(serverSocket, &fdSet))
+		  {
+		    struct sockaddr clientAddr;
+		    memset(&clientAddr, 0, sizeof(struct sockaddr));
+		    addrlen_t clientAddrSize = sizeof(clientAddr);
+		    int connectedSocket = ::accept(serverSocket, &clientAddr, &clientAddrSize);
+		    if (connectedSocket == ERROR)
+		      return NULL;
+				
+		    return new FDIOStream(connectedSocket);
+		  }
+
+		
+	}
+	return NULL;
+}
+
+void SocketServerStreamProvider::close(){
+	::close(serverSocket);
+}

networktables/cpp/Azalea/src/main/native/networktables2/stream/SocketStreamFactory.cpp

@@ -0,0 +1,67 @@
+/*
+ * SocketStreamFactory.cpp
+ *
+ *  Created on: Nov 3, 2012
+ *      Author: Mitchell Wills
+ */
+
+
+#include <cstring>
+#ifdef _WRS_KERNEL
+#else
+	#include <stdlib.h>
+	#include <stdio.h>
+	#include <sys/types.h>
+	#include <unistd.h>
+	#ifdef WIN32
+	   #include <winsock.h>
+	   #include <winsock2.h>
+	#else
+		#include <sys/socket.h>
+		#include <sys/un.h>
+                #include <netdb.h>
+		#include <netinet/in.h>
+		#include <netinet/tcp.h>
+	#endif
+#endif
+#include "networktables2/stream/FDIOStream.h"
+#include "networktables2/stream/SocketStreamFactory.h"
+
+
+SocketStreamFactory::SocketStreamFactory(const char* _host, int _port):host(_host), port(_port){}
+
+SocketStreamFactory::~SocketStreamFactory(){}
+
+IOStream* SocketStreamFactory::createStream(){
+#ifdef _WRS_KERNEL
+	//crio client not supported
+	return NULL;
+#else
+    struct sockaddr_in serv_addr;
+    struct hostent *server;
+
+    int sockfd = socket(AF_INET, SOCK_STREAM, 0);
+    if (sockfd < 0){
+        //error("ERROR opening socket");
+        return NULL;
+    }
+    server = gethostbyname(host);
+    if (server == NULL) {
+        //fprintf(stderr,"ERROR, no such host\n");
+        return NULL;
+    }
+	memset(&serv_addr, 0, sizeof(serv_addr));
+    serv_addr.sin_family = AF_INET;
+    memcpy(&serv_addr.sin_addr.s_addr, server->h_addr, server->h_length);
+    serv_addr.sin_port = htons(port);
+    if (connect(sockfd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0) {
+        //error("ERROR connecting");
+    	return NULL;
+    }//TODO close fd if an error occured
+
+	//int on = 1;
+	//setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&on, sizeof(on));
+
+	return new FDIOStream(sockfd);
+#endif
+}

networktables/cpp/Azalea/src/main/native/networktables2/stream/SocketStreams.cpp

@@ -0,0 +1,21 @@
+/*
+ * SocketStreams.cpp
+ *
+ *  Created on: Sep 27, 2012
+ *      Author: Mitchell Wills
+ */
+
+#include "networktables2/stream/SocketStreams.h"
+#include "networktables2/stream/SocketStreamFactory.h"
+#include "networktables2/stream/SocketServerStreamProvider.h"
+
+
+
+IOStreamFactory& SocketStreams::newStreamFactory(const char* host, int port){
+	return *new SocketStreamFactory(host, port);
+}
+
+IOStreamProvider& SocketStreams::newStreamProvider(int port){
+	return *new SocketServerStreamProvider(port);
+}
+

networktables/cpp/Azalea/src/main/native/networktables2/thread/DefaultThreadManger.cpp

@@ -0,0 +1,49 @@
+/*
+ * DefaultThreadManger.cpp
+ *
+ *  Created on: Sep 21, 2012
+ *      Author: Mitchell Wills
+ */
+
+#include "networktables2/thread/DefaultThreadManager.h"
+#include <stdio.h>
+
+
+PeriodicNTThread::PeriodicNTThread(PeriodicRunnable* _r, const char* _name) : 
+			name(_name), thread(new NTTask(name, (FUNCPTR)PeriodicNTThread::taskMain)), r(_r), run(true){
+	fprintf(stdout, "Starting task: %s\n", name);
+	fflush(stdout);
+	thread->Start((UINT32)this);
+}
+
+PeriodicNTThread::~PeriodicNTThread(){
+	//TODO somehow do this async
+	//delete thread;
+}
+int PeriodicNTThread::taskMain(PeriodicNTThread* o){//static wrapper
+	return o->_taskMain();
+}
+int PeriodicNTThread::_taskMain(){
+	try {
+		while(run){
+			r->run();
+		}
+	} catch (...) {
+		fprintf(stdout, "NTTask exited with uncaught exception %s\n", name);
+		fflush(stdout);
+		return 1;
+	}
+	fprintf(stdout, "NTTask exited normally: %s\n", name);
+	fflush(stdout);
+	return 0;
+}
+void PeriodicNTThread::stop() {
+	run = false;
+}
+bool PeriodicNTThread::isRunning() {
+	return thread->IsReady();
+}
+
+NTThread* DefaultThreadManager::newBlockingPeriodicThread(PeriodicRunnable* r, const char* name) {
+	return new PeriodicNTThread(r, name);
+}

networktables/cpp/Azalea/src/main/native/networktables2/util/System.cpp

@@ -0,0 +1,26 @@
+/*
+ * System.cpp
+ *
+ *  Created on: Sep 26, 2012
+ *      Author: Mitchell Wills
+ */
+
+#include "networktables2/util/System.h"
+#include "semLib.h"
+#include <stdio.h>
+#include <sysLib.h> // for sysClkRateGet
+#include <usrLib.h> // for taskDelay
+
+void sleep_ms(unsigned long ms){
+	taskDelay((INT32)((double)sysClkRateGet() * ms / 1000));
+}
+unsigned long currentTimeMillis(){
+	struct timespec tp;
+	clock_gettime(CLOCK_REALTIME,&tp);
+	return tp.tv_sec*1000 + tp.tv_nsec/1000;
+}
+void writeWarning(const char* message){
+	fprintf(stderr, "%s\n", message);
+	fflush(stderr);
+	//TODO implement write warning with wpilib error stuff
+}