<commit_before>/* Copyright 2018 Iakov Kirilenko, CyberTech Labs Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include <QProcess>
#include <QsLog.h>
#include <QVector>

#include <trikNetwork/mailboxInterface.h>
#include <trikKernel/paths.h>
#include <trikKernel/exceptions/internalErrorException.h>
#include <trikScriptRunnerInterface.h>

#include "pythonEngineWorker.h"
#include <Python.h>
#include "PythonQtConversion.h"

using namespace trikScriptRunner;

QAtomicInt PythonEngineWorker::initCounter = 0;

static int quitFromPython(void*) {
	PyErr_SetInterrupt();
	return 0;
}

static void abortPythonInterpreter() {
	if(!Py_IsInitialized()) {
		return;
	}
	PythonQtGILScope _;
	Py_AddPendingCall(&quitFromPython, nullptr);
}

PythonEngineWorker::PythonEngineWorker(trikControl::BrickInterface &brick
		, trikNetwork::MailboxInterface * const mailbox
		)
	: mBrick(brick)
	, mScriptExecutionControl(new ScriptExecutionControl(brick))
	, mMailbox(mailbox)
	, mState(ready)
{}

PythonEngineWorker::~PythonEngineWorker()
{
	stopScript();
	{
		// In python at least before 3.7 (3.5,3.6)
		// we need to make all pending calls before the context
		// is destroyed, otherwise python crashes
		PythonQtGILScope _;
		Py_MakePendingCalls();
		mMainContext = nullptr;
		if (mPyInterpreter) {
			Py_EndInterpreter(mPyInterpreter);
			mPyInterpreter = nullptr;
		}
	}

	if (--initCounter == 0) {
		Py_Finalize();
		PyMem_RawFree(mProgramName);
		PyMem_RawFree(mPythonPath);
		if (PythonQt::self()) {
			PythonQt::cleanup();
		}
	}
}

void PythonEngineWorker::init()
{
	if (initCounter++ == 0) {
		mProgramName = Py_DecodeLocale("trikPythonRuntime", nullptr);
		Py_SetProgramName(mProgramName);
		constexpr auto varName = "TRIK_PYTHONPATH";
		auto const &path = QProcessEnvironment::systemEnvironment().value(varName);
		if (path.isEmpty()) {
			auto const &e = QString("%1 must be set to correct value").arg(varName);
			QLOG_FATAL() << e;
			throw trikKernel::InternalErrorException(e);
		} else {
			QLOG_INFO() << varName << ":" << path;
		}

		/// TODO: Must point to local .zip file
		mPythonPath = Py_DecodeLocale(path.toStdString().data(), nullptr);
		Py_SetPath(mPythonPath);

/* uncomment for verbosity
		Py_VerboseFlag = 3;
		Py_InspectFlag = 1;
		Py_DebugFlag = 2;
// */
		Py_IsolatedFlag = 1;
		Py_BytesWarningFlag = 3;
		Py_DontWriteBytecodeFlag = 1;
		Py_NoSiteFlag = 1;
		Py_NoUserSiteDirectory = 1;

		Py_Initialize();
		PyEval_InitThreads(); // For Python < 3.7
	}

	if (!mPyInterpreter) {
//		mPyInterpreter = Py_NewInterpreter();
	}

	if (!PythonQt::self()) {
		PythonQt::setEnableThreadSupport(true);
		PythonQtGILScope _;
		PythonQt::init(PythonQt::RedirectStdOut | PythonQt::PythonAlreadyInitialized);
		connect(PythonQt::self(), &PythonQt::pythonStdErr, this, &PythonEngineWorker::updateErrorMessage);
		connect(PythonQt::self(), &PythonQt::pythonStdOut, this, &PythonEngineWorker::textInStdOut);
		PythonQtRegisterListTemplateConverter(QVector, uint8_t)
		PythonQt_QtAll::init();
	}
	if (!mMainContext) {
		mMainContext = PythonQt::self()->getMainModule();
		recreateContext();
	}
	emit inited();
}

bool PythonEngineWorker::recreateContext()
{
	{
		PythonQtGILScope _;
		Py_MakePendingCalls();
		PyErr_CheckSignals();
		PyErr_Clear();
	}
	PythonQt::self()->clearError();
	return initTrik();
}

bool PythonEngineWorker::evalSystemPy()
{
	const QString systemPyPath = trikKernel::Paths::systemScriptsPath() + "system.py";

	if (!QFileInfo::exists(systemPyPath)) {
		QLOG_ERROR() << "system.py not found, path:" << systemPyPath;
		return false;
	}
	mMainContext.evalFile(systemPyPath);
	if (PythonQt::self()->hadError()) {
		QLOG_ERROR() << "Failed to eval system.py";
		return false;
	}
	return true;
}

bool PythonEngineWorker::initTrik()
{
	PythonQt_init_PyTrikControl(mMainContext);
	mMainContext.addObject("brick", &mBrick);
	mMainContext.addObject("script_cpp", mScriptExecutionControl.data());

	return evalSystemPy();
}

void PythonEngineWorker::resetBrick()
{
	QLOG_INFO() << "Stopping robot";

	if (mMailbox) {
		mMailbox->stopWaiting();
		mMailbox->clearQueue();
	}

	mBrick.reset();
}

void PythonEngineWorker::brickBeep()
{
	mBrick.playSound(trikKernel::Paths::mediaPath() + "media/beep_soft.wav");
}

void PythonEngineWorker::stopScript()
{
	QMutexLocker locker(&mScriptStateMutex);

	if (mState == stopping) {
		// Already stopping, so we can do nothing.
		return;
	}

	if (mState == ready) {
		// Engine is ready for execution.
		return;
	}

	QLOG_INFO() << "PythonEngineWorker: stopping script";

	mState = stopping;

	if (QThread::currentThread() != thread()) {
		abortPythonInterpreter();
	} else {
		QLOG_FATAL() << "Attempt to abort Python from main thread.";
	}

	if (mMailbox) {
		mMailbox->stopWaiting();
	}

	mState = ready;

	/// @todo: is it actually stopped?

	QLOG_INFO() << "PythonEngineWorker: stopping complete";
}

QStringList PythonEngineWorker::knownNames() const
{
	QSet<QString> result = {"brick", "script", "threading"};
	TrikScriptRunnerInterface::Helper::collectMethodNames(result, &trikControl::BrickInterface::staticMetaObject);
/// TODO:	TrikScriptRunnerInterface::Helper::collectMethodNames(result, mScriptControl.metaObject());
	if (mMailbox) {
		result.insert("mailbox");
		TrikScriptRunnerInterface::Helper::collectMethodNames(result, mMailbox->metaObject());
	}
/// TODO:	TrikScriptRunnerInterface::Helper::collectMethodNames(result, mThreading.metaObject());
	return result.toList();
}

void PythonEngineWorker::run(const QString &script, const QFileInfo &scriptFile)
{
	QMutexLocker locker(&mScriptStateMutex);
	mState = starting;
	QMetaObject::invokeMethod(this, [this, script, scriptFile](){this->doRun(script, scriptFile);});
}

void PythonEngineWorker::doRun(const QString &script, const QFileInfo &scriptFile)
{
	emit startedScript("", 0);
	mErrorMessage.clear();
	/// When starting script execution (by any means), clear button states.
	mBrick.keys()->reset();
	mState = running;
	auto ok = recreateContext();
	if (!ok) {
		emit completed(mErrorMessage,0);
		return;
	}

	if (scriptFile.isFile()) {
		mMainContext.evalScript("import sys; (lambda x: sys.path.append(x) if not x in sys.path else None)('"
								+ scriptFile.canonicalPath() + "')");
	}
	mMainContext.evalScript(script);

	QLOG_INFO() << "PythonEngineWorker: evaluation ended";

	auto wasError = mState != ready && PythonQt::self()->hadError();
	mState = ready;
	if (wasError) {
		emit completed(mErrorMessage, 0);
	} else {
		emit completed("", 0);
	}
}

void PythonEngineWorker::runDirect(const QString &command)
{
	QMutexLocker locker(&mScriptStateMutex);
	QMetaObject::invokeMethod(this, "doRunDirect", Q_ARG(QString, command));
}

void PythonEngineWorker::doRunDirect(const QString &command)
{
	emit startedDirectScript(0);
	if (PythonQt::self()->hadError()) {
		PythonQt::self()->clearError();
		mErrorMessage.clear();
		recreateContext();
	}
	mMainContext.evalScript(command);
	emit completed(mErrorMessage, 0);
}

void PythonEngineWorker::updateErrorMessage(const QString &err)
{
	mErrorMessage += err;
}

void PythonEngineWorker::onScriptRequestingToQuit()
{
	throw std::logic_error("Not implemented");
}
<commit_msg>No twice system.py<commit_after>/* Copyright 2018 Iakov Kirilenko, CyberTech Labs Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include <QProcess>
#include <QsLog.h>
#include <QVector>

#include <trikNetwork/mailboxInterface.h>
#include <trikKernel/paths.h>
#include <trikKernel/exceptions/internalErrorException.h>
#include <trikScriptRunnerInterface.h>

#include "pythonEngineWorker.h"
#include <Python.h>
#include "PythonQtConversion.h"

using namespace trikScriptRunner;

QAtomicInt PythonEngineWorker::initCounter = 0;

static int quitFromPython(void*) {
	PyErr_SetInterrupt();
	return 0;
}

static void abortPythonInterpreter() {
	if(!Py_IsInitialized()) {
		return;
	}
	PythonQtGILScope _;
	Py_AddPendingCall(&quitFromPython, nullptr);
}

PythonEngineWorker::PythonEngineWorker(trikControl::BrickInterface &brick
		, trikNetwork::MailboxInterface * const mailbox
		)
	: mBrick(brick)
	, mScriptExecutionControl(new ScriptExecutionControl(brick))
	, mMailbox(mailbox)
	, mState(ready)
{}

PythonEngineWorker::~PythonEngineWorker()
{
	stopScript();
	{
		// In python at least before 3.7 (3.5,3.6)
		// we need to make all pending calls before the context
		// is destroyed, otherwise python crashes
		PythonQtGILScope _;
		Py_MakePendingCalls();
		mMainContext = nullptr;
		if (mPyInterpreter) {
			Py_EndInterpreter(mPyInterpreter);
			mPyInterpreter = nullptr;
		}
	}

	if (--initCounter == 0) {
		Py_Finalize();
		PyMem_RawFree(mProgramName);
		PyMem_RawFree(mPythonPath);
		if (PythonQt::self()) {
			PythonQt::cleanup();
		}
	}
}

void PythonEngineWorker::init()
{
	if (initCounter++ == 0) {
		mProgramName = Py_DecodeLocale("trikPythonRuntime", nullptr);
		Py_SetProgramName(mProgramName);
		constexpr auto varName = "TRIK_PYTHONPATH";
		auto const &path = QProcessEnvironment::systemEnvironment().value(varName);
		if (path.isEmpty()) {
			auto const &e = QString("%1 must be set to correct value").arg(varName);
			QLOG_FATAL() << e;
			throw trikKernel::InternalErrorException(e);
		} else {
			QLOG_INFO() << varName << ":" << path;
		}

		/// TODO: Must point to local .zip file
		mPythonPath = Py_DecodeLocale(path.toStdString().data(), nullptr);
		Py_SetPath(mPythonPath);

/* uncomment for verbosity
		Py_VerboseFlag = 3;
		Py_InspectFlag = 1;
		Py_DebugFlag = 2;
// */
		Py_IsolatedFlag = 1;
		Py_BytesWarningFlag = 3;
		Py_DontWriteBytecodeFlag = 1;
		Py_NoSiteFlag = 1;
		Py_NoUserSiteDirectory = 1;

		Py_Initialize();
		PyEval_InitThreads(); // For Python < 3.7
	}

	if (!mPyInterpreter) {
//		mPyInterpreter = Py_NewInterpreter();
	}

	if (!PythonQt::self()) {
		PythonQt::setEnableThreadSupport(true);
		PythonQtGILScope _;
		PythonQt::init(PythonQt::RedirectStdOut | PythonQt::PythonAlreadyInitialized);
		connect(PythonQt::self(), &PythonQt::pythonStdErr, this, &PythonEngineWorker::updateErrorMessage);
		connect(PythonQt::self(), &PythonQt::pythonStdOut, this, &PythonEngineWorker::textInStdOut);
		PythonQtRegisterListTemplateConverter(QVector, uint8_t)
		PythonQt_QtAll::init();
	}
	if (!mMainContext) {
		mMainContext = PythonQt::self()->getMainModule();
		recreateContext();
	}
	emit inited();
}

bool PythonEngineWorker::recreateContext()
{
	{
		PythonQtGILScope _;
		Py_MakePendingCalls();
		PyErr_CheckSignals();
		PyErr_Clear();
	}
	PythonQt::self()->clearError();
	return initTrik();
}

bool PythonEngineWorker::evalSystemPy()
{
	const QString systemPyPath = trikKernel::Paths::systemScriptsPath() + "system.py";

	if (!QFileInfo::exists(systemPyPath)) {
		QLOG_ERROR() << "system.py not found, path:" << systemPyPath;
		return false;
	}

	mMainContext.evalScript("script.wait(0)");
	if (PythonQt::self()->hadError()) {
		PythonQt::self()->clearError();
		mMainContext.evalFile(systemPyPath);
		if (PythonQt::self()->hadError()) {
			QLOG_ERROR() << "Failed to eval system.py";
			return false;
		}
	}

	return true;
}

bool PythonEngineWorker::initTrik()
{
	PythonQt_init_PyTrikControl(mMainContext);
	mMainContext.addObject("brick", &mBrick);
	mMainContext.addObject("script_cpp", mScriptExecutionControl.data());

	return evalSystemPy();
}

void PythonEngineWorker::resetBrick()
{
	QLOG_INFO() << "Stopping robot";

	if (mMailbox) {
		mMailbox->stopWaiting();
		mMailbox->clearQueue();
	}

	mBrick.reset();
}

void PythonEngineWorker::brickBeep()
{
	mBrick.playSound(trikKernel::Paths::mediaPath() + "media/beep_soft.wav");
}

void PythonEngineWorker::stopScript()
{
	QMutexLocker locker(&mScriptStateMutex);

	if (mState == stopping) {
		// Already stopping, so we can do nothing.
		return;
	}

	if (mState == ready) {
		// Engine is ready for execution.
		return;
	}

	QLOG_INFO() << "PythonEngineWorker: stopping script";

	mState = stopping;

	if (QThread::currentThread() != thread()) {
		abortPythonInterpreter();
	} else {
		QLOG_FATAL() << "Attempt to abort Python from main thread.";
	}

	if (mMailbox) {
		mMailbox->stopWaiting();
	}

	mState = ready;

	/// @todo: is it actually stopped?

	QLOG_INFO() << "PythonEngineWorker: stopping complete";
}

QStringList PythonEngineWorker::knownNames() const
{
	QSet<QString> result = {"brick", "script", "threading"};
	TrikScriptRunnerInterface::Helper::collectMethodNames(result, &trikControl::BrickInterface::staticMetaObject);
/// TODO:	TrikScriptRunnerInterface::Helper::collectMethodNames(result, mScriptControl.metaObject());
	if (mMailbox) {
		result.insert("mailbox");
		TrikScriptRunnerInterface::Helper::collectMethodNames(result, mMailbox->metaObject());
	}
/// TODO:	TrikScriptRunnerInterface::Helper::collectMethodNames(result, mThreading.metaObject());
	return result.toList();
}

void PythonEngineWorker::run(const QString &script, const QFileInfo &scriptFile)
{
	QMutexLocker locker(&mScriptStateMutex);
	mState = starting;
	QMetaObject::invokeMethod(this, [this, script, scriptFile](){this->doRun(script, scriptFile);});
}

void PythonEngineWorker::doRun(const QString &script, const QFileInfo &scriptFile)
{
	emit startedScript("", 0);
	mErrorMessage.clear();
	/// When starting script execution (by any means), clear button states.
	mBrick.keys()->reset();
	mState = running;
	auto ok = recreateContext();
	if (!ok) {
		emit completed(mErrorMessage,0);
		return;
	}

	if (scriptFile.isFile()) {
		mMainContext.evalScript("import sys; (lambda x: sys.path.append(x) if not x in sys.path else None)('"
								+ scriptFile.canonicalPath() + "')");
	}
	mMainContext.evalScript(script);

	QLOG_INFO() << "PythonEngineWorker: evaluation ended";

	auto wasError = mState != ready && PythonQt::self()->hadError();
	mState = ready;
	if (wasError) {
		emit completed(mErrorMessage, 0);
	} else {
		emit completed("", 0);
	}
}

void PythonEngineWorker::runDirect(const QString &command)
{
	QMutexLocker locker(&mScriptStateMutex);
	QMetaObject::invokeMethod(this, "doRunDirect", Q_ARG(QString, command));
}

void PythonEngineWorker::doRunDirect(const QString &command)
{
	emit startedDirectScript(0);
	if (PythonQt::self()->hadError()) {
		PythonQt::self()->clearError();
		mErrorMessage.clear();
		recreateContext();
	}
	mMainContext.evalScript(command);
	emit completed(mErrorMessage, 0);
}

void PythonEngineWorker::updateErrorMessage(const QString &err)
{
	mErrorMessage += err;
}

void PythonEngineWorker::onScriptRequestingToQuit()
{
	throw std::logic_error("Not implemented");
}
<|endoftext|>


<commit_before><commit_msg>make CalcScore Method<commit_after><|endoftext|>


<commit_before>//=============================================================================
// ■ VMtest.cxx
//-----------------------------------------------------------------------------
//   虚拟机测试程序。
//   扩展名用了.cxx，意思是“×不要自动编译”。
//=============================================================================

#include <ratio>
#include <chrono>
using namespace std;

#include "ASM76.hpp"
using namespace ASM76;

Instruct test_mem_i[] = {
	{LCMM, 0x4000000, 0},
	{DATI, 0xABCD1234, 1},
	{DATB, 'V', 5},
	{DATB, 'M', 6},
	{DATB, '7', 7},
	{DATB, '6', 8},
	{SLLA, 0x2000000, 1},
	{SLIA, 0x2500000, 2},
	{SLBA, 0x2700000, 3},
	{LDLA, 0x2500000, 51},
	{LDIA, 0x2500000, 51},
	{LDBA, 0x2500000, 61},
	{HALT, 0, 0},
};
Program test_mem_p {
	.size = sizeof(test_mem_i),
	.instruct = test_mem_i
};

Instruct test_basic_algebra_i[] = {
	{LCMM, 0x100, 0},
	{DATI, 0x1, 1},
	// $1 = $1 + $1
	// 1, 2, 4, 8, 16, ...
	{ADDL, 1, 1},
	{ADDL, 1, 1},
	{ADDL, 1, 1},
	{ADDL, 1, 1},
	// Put the result somewhere else
	{MVRL, 1, 31},
	// Then divide it by 4
	{DATI, 0x4, 11},
	{DIVL, 1, 11},
	{HALT, 0, 0},
};
Program test_basic_algebra_p {
	.size = sizeof(test_basic_algebra_i),
	.instruct = test_basic_algebra_i
};

Instruct test_flow_control_i[] = {
	{DATB, 0x1, 20},
	{DATB, 0xA, 10},
	{DATB, 0x2, 15},
	{ADDL, 15, 15},
	{ADDL, 3, 20},
	{CMPI, 3, 10},
	{PUSH, 15, 1},
	{JILA, 3 * sizeof(Instruct), 0},
	{HALT, 0, 0},
};
Program test_flow_control_p {
	.size = sizeof(test_flow_control_i),
	.instruct = test_flow_control_i
};

const char* const test_asm =
	"# Comments.\n"
	"DATB	0x1 $20\n"
	"DATB	0xA $10\n"
	"DATB	0x2 $15\n"
	"[loop_start]\n"
	"ADDL	$15 $15\n"
	"ADDL	$3 $20\n"
	"CMPI	$3 $10\n"
	"PUSH	$15 1\n"
	"JILA	[loop_start]\n"
	"HALT\n"
	"DD 0x7676 0xEFEF 0xABAB 0xCDCD 0x0000\n"
	"# EOF\n";

const char* const test_bios =
	"DATI	0x76 $0\n"
	"SLIA	0x100 $0\n"
	"INTX	0x1 0x100\n"
	"DATI	0xAB $0\n"
	"SLIA	0x100 $0\n"
	"INTX	0x1 0x100\n"
	"HALT\n"
	"# EOF\n";

static uint32_t test_bios_call(uint8_t* input) {
	printf("TEST BIOS CALL: %x\n", *((uint32_t*) input));

	return 0x89ABCDEF;
}

Instruct test_speed_i[] = {
	{DATI, 0x1, 20},
	{DATI, 0x3000000, 60},
	{DATI, 0x2, 15},
	{ADDL, 3, 20},
	{CMPI, 3, 60},
	{JILA, 3 * sizeof(Instruct), 0},
	{HALT, 0, 0},
};
Program test_speed_p {
	.size = sizeof(test_speed_i),
	.instruct = test_speed_i
};

const char* const test_tag =
	"JMPA [end]\n"
	"ADDI $50 $100\n"
	"MINI $25 $50\n"
	"[end]\n"
	"HALT\n"
	"# EOF\n";

int main() {
	puts("+---------------------+");
	puts("| ASM 76 Test Program |");
	puts("+---------------------+");
	init();

	{
		puts("===== Memory =====");
		VM v(test_mem_p);
		v.execute(true);
		v.dump_registers();
	}

	{
		puts("===== Basic Algebra =====");
		VM v(test_basic_algebra_p);
		v.execute(true);
		v.dump_registers();
	}

	{
		puts("===== Disassembler =====");
		Disassembler d(test_flow_control_p);
		char* s = d.disassemble();
		puts(s);
		free(s);
	}

	{
		puts("===== Assembler =====");
		Assembler a(test_asm);
		a.scan();
		Program p = a.assemble();
		// Test assembler DD function
		printf("Data DD: %04x%08x%08x\n", p.instruct[9].opcode, p.instruct[9].a, p.instruct[9].b);
		// Test disassembler
		Disassembler d(p);
		char* s = d.disassemble();
		puts(s);
		free(s);

		puts("===== Flow Control & Logistics =====");
		VM v(p);
		v.execute(false);
		v.dump_registers();
		free(p.instruct);
	}

	{
		puts("===== Assembler: Tag Test =====");
		Assembler a(test_tag);
		a.scan();
		Program p = a.assemble();
		Disassembler d(p);
		char* s = d.disassemble();
		puts(s);
		free(s);
	}

	{
		puts("===== BIOS Test =====");
		BIOS* b = new BIOS(5);
		b->function_table[1] = &test_bios_call;

		Assembler a(test_bios);
		a.scan();
		Program p = a.assemble();

		VM v(p);
		v.firmware = b;
		v.execute(true);
		v.dump_registers();
	}

	{
		puts("===== Object Code =====");
		ObjectCode::write_file("test.obj", test_mem_p);
		Program p = ObjectCode::read_file("test.obj");
		Disassembler d(p);
		char* s = d.disassemble();
		puts(s);
		free(s);
		free(p.instruct);
	}

	{
		puts("===== Speed Test: 0x3000000 cycles =====");
		VM v(test_speed_p);

		// The type is chrono::time_point<chrono::high_resolution_clock>
		// and that is why people used auto.
		auto t1 = chrono::high_resolution_clock::now();
		v.execute(false);
		auto t2 = chrono::high_resolution_clock::now();
		chrono::duration<double, milli> delay = t2 - t1;
		printf("Elapsed time: %lfms\nMIPS: %lf\n",
			delay.count(),
			((double) 0x3000000) * 3.0 / ((double) delay.count()) / 10000.0
		);
	}

	puts("\n===== TEST END =====");

	return 0;
}
<commit_msg>Refine VMtest.cxx × 2<commit_after>//=============================================================================
// ■ VMtest.cxx
//-----------------------------------------------------------------------------
//   虚拟机、汇编器、反汇编器、目标代码的测试程序。
//   扩展名用了.cxx，意思是“× 不要自动编译”。
//=============================================================================

#include <ratio>
#include <chrono>
using namespace std;

#include "ASM76.hpp"
using namespace ASM76;

void test_mem() {
	puts("===== Memory =====");
	Instruct instruct[] = {
		{LCMM, 0x4000000, 0},
		{DATI, 0xABCD1234, 1},
		{DATB, 'V', 5},
		{DATB, 'M', 6},
		{DATB, '7', 7},
		{DATB, '6', 8},
		{SLLA, 0x2000000, 1},
		{SLIA, 0x2500000, 2},
		{SLBA, 0x2700000, 3},
		{LDLA, 0x2500000, 51},
		{LDIA, 0x2500000, 51},
		{LDBA, 0x2500000, 61},
		{HALT, 0, 0},
	};
	Program p {
		.size = sizeof(instruct),
		.instruct = instruct
	};
	VM v(p);
	v.execute(true);
	v.dump_registers();
}

void test_basic_algebra() {
	puts("===== Basic Algebra =====");
	Instruct instruct[] = {
		{LCMM, 0x100, 0},
		{DATI, 0x1, 1},
		// $1 = $1 + $1
		// 1, 2, 4, 8, 16, ...
		{ADDL, 1, 1},
		{ADDL, 1, 1},
		{ADDL, 1, 1},
		{ADDL, 1, 1},
		// Put the result somewhere else
		{MVRL, 1, 31},
		// Then divide it by 4
		{DATI, 0x4, 11},
		{DIVL, 1, 11},
		{HALT, 0, 0},
	};
	Program p {
		.size = sizeof(instruct),
		.instruct = instruct
	};
	VM v(p);
	v.execute(true);
	v.dump_registers();
}

void test_disassembler_assembler() {
	puts("===== Assembler =====");
	const char* const s1 =
		"# Comments.\n"
		"DATB	0x1 $20\n"
		"DATB	0xA $10\n"
		"DATB	0x2 $15\n"
		"[loop_start]\n"
		"ADDL	$15 $15\n"
		"ADDL	$3 $20\n"
		"CMPI	$3 $10\n"
		"PUSH	$15 1\n"
		"JILA	[loop_start]\n"
		"HALT\n"
		"DD 0x7676 0xEFEF 0xABAB 0xCDCD 0x0000\n"
		"# EOF\n";
	Assembler a(s1);
	a.scan();
	Program p = a.assemble();
	// Test assembler DD function
	printf(
		"Data DD: %04x%08x%08x\n",
		p.instruct[9].opcode,
		p.instruct[9].a,
		p.instruct[9].b
	);
	// Test disassembler
	Disassembler d(p);
	char* s2 = d.disassemble();
	puts(s2);
	free(s2);
}

void test_tag() {
	puts("===== Assembler: Tag Test =====");
	const char* const s1 =
		"JMPA [end]\n"
		"ADDI $50 $100\n"
		"MINI $25 $50\n"
		"[end]\n"
		"HALT\n"
		"# EOF\n";
	Assembler a(s1);
	a.scan();
	Program p = a.assemble();
	Disassembler d(p);
	char* s2 = d.disassemble();
	puts(s2);
	free(s2);
}

void test_flow_control() {
	puts("===== Flow Control & Logistics =====");
	Instruct instruct[] = {
		{DATB, 0x1, 20},
		{DATB, 0xA, 10},
		{DATB, 0x2, 15},
		{ADDL, 15, 15},
		{ADDL, 3, 20},
		{CMPI, 3, 10},
		{PUSH, 15, 1},
		{JILA, 3 * sizeof(Instruct), 0},
		{HALT, 0, 0},
	};
	Program p {
		.size = sizeof(instruct),
		.instruct = instruct
	};
	VM v(p);
	v.execute(false);
	v.dump_registers();
}

static uint32_t test_bios_call(uint8_t* input) {
	printf("TEST BIOS CALL: %x\n", *((uint32_t*) input));
	return 0x89ABCDEF;
}

void test_bios() {
	puts("===== BIOS Test =====");
	const char* const s =
		"DATI	0x76 $0\n"
		"SLIA	0x100 $0\n"
		"INTX	0x1 0x100\n"
		"DATI	0xAB $0\n"
		"SLIA	0x100 $0\n"
		"INTX	0x1 0x100\n"
		"HALT\n"
		"# EOF\n";

	BIOS b(5);
	b.function_table[1] = &test_bios_call;

	Assembler a(s);
	a.scan();
	Program p = a.assemble();

	VM v(p);
	v.firmware = &b;
	v.execute(true);
	v.dump_registers();
}

void test_object_code() {
	puts("===== Object Code =====");
	Instruct instruct[] = {
		{ADDL, 7, 6},
		{NOOP, 0, 0},
		{HALT, 0, 0},
	};
	Program p1 {
		.size = sizeof(instruct),
		.instruct = instruct
	};
	ObjectCode::write_file("test.obj", p1);
	Program p2 = ObjectCode::read_file("test.obj");
	Disassembler d(p2);
	char* s = d.disassemble();
	puts(s);
	free(s);
	free(p2.instruct);
}

void test_speed() {
	puts("===== Speed Test: 0x3000000 cycles =====");
	Instruct instruct[] = {
		{DATI, 0x1, 20},
		{DATI, 0x3000000, 60},
		{DATI, 0x2, 15},
		{ADDL, 3, 20},
		{CMPI, 3, 60},
		{JILA, 3 * sizeof(Instruct), 0},
		{HALT, 0, 0},
	};
	Program p {
		.size = sizeof(instruct),
		.instruct = instruct
	};
	VM v(p);

	// The type is chrono::time_point<chrono::high_resolution_clock>
	// and that is why people used auto.
	auto t1 = chrono::high_resolution_clock::now();
	v.execute(false);
	auto t2 = chrono::high_resolution_clock::now();
	chrono::duration<double, milli> delay = t2 - t1;
	printf("Elapsed time: %lfms\nMIPS: %lf\n",
		delay.count(),
		((double) 0x3000000) * 3.0 / ((double) delay.count()) / 10000.0
	);
}

int main() {
	puts("+---------------------+");
	puts("| ASM 76 Test Program |");
	puts("+---------------------+");
	init();

	test_mem();
	test_basic_algebra();
	test_disassembler_assembler();
	test_tag();
	test_flow_control();
	test_bios();
	test_object_code();
	test_speed();

	putchar('\n');
	terminate();
	puts("# TEST END #");
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2003 The Regents of The University of Michigan
 * 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 holders 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.
 */

/* @file
 * Reference counted class containing ethernet packet data
 */

#ifndef __ETHERPKT_HH__
#define __ETHERPKT_HH__

#include <memory>

#include "sim/host.hh"

#include "base/refcnt.hh"

/*
 * Reference counted class containing ethernet packet data
 */
class EtherPacket : public RefCounted
{
  public:
    uint8_t *data;
    int length;

  public:
    EtherPacket() : data(NULL), length(0) {}
    EtherPacket(std::auto_ptr<uint8_t> d, int l)
        : data(d.release()), length(l) {}
    ~EtherPacket() { if (data) delete [] data; }

  public:
    bool IsUnicast() { return data[0] == 0x00; }
    bool IsMulticast() { return data[0] == 0x01; }
    bool IsBroadcast() { return data[0] == 0xff; }
};

typedef RefCountingPtr<EtherPacket> PacketPtr;

#endif // __ETHERPKT_HH__
<commit_msg>Add serialization for etherdev These changes don't seem to break anything, and may even be correct.<commit_after>/*
 * Copyright (c) 2003 The Regents of The University of Michigan
 * 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 holders 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.
 */

/* @file
 * Reference counted class containing ethernet packet data
 */

#ifndef __ETHERPKT_HH__
#define __ETHERPKT_HH__

#include <memory>

#include "sim/host.hh"

#include "base/refcnt.hh"

class IniFile;

/*
 * Reference counted class containing ethernet packet data
 */
class EtherPacket : public RefCounted
{
  public:
    uint8_t *data;
    int length;

  public:
    EtherPacket() : data(NULL), length(0) {}
    EtherPacket(std::auto_ptr<uint8_t> d, int l)
        : data(d.release()), length(l) {}
    ~EtherPacket() { if (data) delete [] data; }

  public:
    bool IsUnicast() { return data[0] == 0x00; }
    bool IsMulticast() { return data[0] == 0x01; }
    bool IsBroadcast() { return data[0] == 0xff; }

    virtual void serialize(std::ostream &os);
    virtual void unserialize(const IniFile *db, const std::string &section);
};

typedef RefCountingPtr<EtherPacket> PacketPtr;

#endif // __ETHERPKT_HH__
<|endoftext|>


<commit_before>/**
 * This file is part of the "tsdb" project
 *   Copyright (c) 2015 Paul Asmuth, FnordCorp B.V.
 *
 * FnordMetric is free software: you can redistribute it and/or modify it under
 * the terms of the GNU General Public License v3.0. You should have received a
 * copy of the GNU General Public License along with this program. If not, see
 * <http://www.gnu.org/licenses/>.
 */
#include <stx/io/fileutil.h>
#include <zbase/core/Partition.h>
#include <zbase/core/LSMPartitionWriter.h>
#include <zbase/core/RecordVersionMap.h>
#include <stx/protobuf/msg.h>
#include <stx/logging.h>
#include <stx/wallclock.h>
#include <stx/logging.h>
#include <sstable/SSTableWriter.h>
#include <stx/protobuf/MessageDecoder.h>
#include <cstable/RecordShredder.h>
#include <cstable/CSTableWriter.h>
#include <sstable/sstablereader.h>

using namespace stx;

namespace zbase {

LSMPartitionWriter::LSMPartitionWriter(
    RefPtr<Partition> partition,
    PartitionSnapshotRef* head) :
    PartitionWriter(head),
    partition_(partition),
    compaction_strategy_(
        new SimpleCompactionStrategy(
            partition_->getTable(),
            head->getSnapshot()->base_path)),
    max_datafile_size_(kDefaultMaxDatafileSize) {}

Set<SHA1Hash> LSMPartitionWriter::insertRecords(const Vector<RecordRef>& records) {
  std::unique_lock<std::mutex> lk(mutex_);
  if (frozen_) {
    RAISE(kIllegalStateError, "partition is frozen");
  }

  auto snap = head_->getSnapshot();

  stx::logTrace(
      "tsdb",
      "Insert $0 record into partition $1/$2/$3",
      records.size(),
      snap->state.tsdb_namespace(),
      snap->state.table_key(),
      snap->key.toString());

  HashMap<SHA1Hash, uint64_t> rec_versions;
  for (const auto& r : records) {
    if (snap->head_arena->fetchRecordVersion(r.record_id) < r.record_version) {
      rec_versions.emplace(r.record_id, 0);
    }
  }

  if (snap->compacting_arena.get() != nullptr) {
    for (auto& r : rec_versions) {
      auto v = snap->compacting_arena->fetchRecordVersion(r.first);
      if (v > r.second) {
        r.second = v;
      }
    }
  }

  const auto& tables = snap->state.lsm_tables();
  for (auto tbl = tables.rbegin(); tbl != tables.rend(); ++tbl) {
    RecordVersionMap::lookup(
        &rec_versions,
        FileUtil::joinPaths(snap->base_path, tbl->filename() + ".idx"));

    // FIMXE early exit...
  }

  Set<SHA1Hash> inserted_ids;
  if (!rec_versions.empty()) {
    for (auto r : records) {
      auto headv = rec_versions[r.record_id];
      if (headv > 0) {
        r.is_update = true;
      }

      if (r.record_version <= headv) {
        continue;
      }

      if (snap->head_arena->insertRecord(r)) {
        inserted_ids.emplace(r.record_id);
      }
    }
  }

  return inserted_ids;
}

bool LSMPartitionWriter::needsCommit() {
  ScopedLock<std::mutex> write_lk(mutex_);
  return head_->getSnapshot()->head_arena->size() > 0;
}

bool LSMPartitionWriter::needsCompaction() {
  if (needsCommit() || needsUpgradeFromV1()) {
    return true;
  }

  auto snap = head_->getSnapshot();
  return compaction_strategy_->needsCompaction(
      Vector<LSMTableRef>(
          snap->state.lsm_tables().begin(),
          snap->state.lsm_tables().end()));
}

bool LSMPartitionWriter::commit() {
  ScopedLock<std::mutex> commit_lk(commit_mutex_);
  RefPtr<RecordArena> arena;

  // flip arenas if records pending
  {
    ScopedLock<std::mutex> write_lk(mutex_);
    auto snap = head_->getSnapshot()->clone();
    if (snap->compacting_arena.get() == nullptr &&
        snap->head_arena->size() > 0) {
      snap->compacting_arena = snap->head_arena;
      snap->head_arena = mkRef(new RecordArena());
      head_->setSnapshot(snap);
    }
    arena = snap->compacting_arena;
  }

  // flush arena to disk if pending
  if (arena.get() && arena->size() > 0) {
    auto snap = head_->getSnapshot();
    auto filename = Random::singleton()->hex64();
    auto filepath = FileUtil::joinPaths(snap->base_path, filename);
    auto t0 = WallClock::unixMicros();
    writeArenaToDisk(arena, snap->state.lsm_sequence() + 1, filepath);
    auto t1 = WallClock::unixMicros();

    stx::logDebug(
        "z1.core",
        "Committing partition $1/$2/$3 ($0 records), took $4s",
        arena->size(),
        snap->state.tsdb_namespace(),
        snap->state.table_key(),
        snap->key.toString(),
        (double) (t1 - t0) / 1000000.0f);

    ScopedLock<std::mutex> write_lk(mutex_);
    snap = head_->getSnapshot()->clone();
    auto tblref = snap->state.add_lsm_tables();
    tblref->set_filename(filename);
    tblref->set_first_sequence(snap->state.lsm_sequence() + 1);
    tblref->set_last_sequence(snap->state.lsm_sequence() + arena->size());
    snap->state.set_lsm_sequence(snap->state.lsm_sequence() + arena->size());
    snap->compacting_arena = nullptr;
    snap->writeToDisk();
    head_->setSnapshot(snap);
    return true;
  } else {
    return false;
  }
}

bool LSMPartitionWriter::compact() {
  if (needsUpgradeFromV1()) {
    upgradeFromV1();
  }

  auto dirty = commit();

  // fetch current table list
  auto snap = head_->getSnapshot()->clone();

  Vector<LSMTableRef> new_tables;
  Vector<LSMTableRef> old_tables(
      snap->state.lsm_tables().begin(),
      snap->state.lsm_tables().end());

  // compact
  auto t0 = WallClock::unixMicros();
  if (!compaction_strategy_->compact(old_tables, &new_tables)) {
    return dirty;
  }
  auto t1 = WallClock::unixMicros();

  stx::logDebug(
      "z1.core",
      "Compacting partition $0/$1/$2, took $3s",
      snap->state.tsdb_namespace(),
      snap->state.table_key(),
      snap->key.toString(),
      (double) (t1 - t0) / 1000000.0f);

  // commit table list
  ScopedLock<std::mutex> write_lk(mutex_);
  snap = head_->getSnapshot()->clone();

  if (snap->state.lsm_tables().size() < old_tables.size()) {
    RAISE(kConcurrentModificationError, "concurrent compaction");
  }

  size_t i = 0;
  for (const auto& tbl : snap->state.lsm_tables()) {
    if (i < old_tables.size()) {
      if (old_tables[i].filename() != tbl.filename()) {
        RAISE(kConcurrentModificationError, "concurrent compaction");
      }
    } else {
      new_tables.push_back(tbl);
    }

    ++i;
  }

  snap->state.mutable_lsm_tables()->Clear();
  for (const auto& tbl :  new_tables) {
    *snap->state.add_lsm_tables() = tbl;
  }

  snap->writeToDisk();
  head_->setSnapshot(snap);
  write_lk.unlock();

  Set<String> delete_filenames;
  for (const auto& tbl : old_tables) {
    delete_filenames.emplace(tbl.filename());
  }
  for (const auto& tbl : new_tables) {
    delete_filenames.erase(tbl.filename());
  }

  for (const auto& f : delete_filenames) {
    // FIXME: delayed delete
    FileUtil::rm(FileUtil::joinPaths(snap->base_path, f + ".cst"));
    FileUtil::rm(FileUtil::joinPaths(snap->base_path, f + ".idx"));
  }

  return true;
}

void LSMPartitionWriter::writeArenaToDisk(
      RefPtr<RecordArena> arena,
      uint64_t sequence,
      const String& filename) {
  auto schema = partition_->getTable()->schema();

  {
    OrderedMap<SHA1Hash, uint64_t> vmap;
    auto cstable_schema = cstable::TableSchema::fromProtobuf(*schema);
    auto cstable_schema_ext = cstable_schema;
    cstable_schema_ext.addBool("__lsm_is_update", false);
    cstable_schema_ext.addString("__lsm_id", false);
    cstable_schema_ext.addUnsignedInteger("__lsm_version", false);
    cstable_schema_ext.addUnsignedInteger("__lsm_sequence", false);

    auto cstable = cstable::CSTableWriter::createFile(
        filename + ".cst",
        cstable::BinaryFormatVersion::v0_1_0,
        cstable_schema_ext);

    cstable::RecordShredder shredder(cstable.get(), &cstable_schema);
    auto is_update_col = cstable->getColumnWriter("__lsm_is_update");
    auto id_col = cstable->getColumnWriter("__lsm_id");
    auto version_col = cstable->getColumnWriter("__lsm_version");
    auto sequence_col = cstable->getColumnWriter("__lsm_sequence");

    arena->fetchRecords([&] (const RecordRef& r) {
      msg::MessageObject obj;
      msg::MessageDecoder::decode(r.record, *schema, &obj);
      shredder.addRecordFromProtobuf(obj, *schema);
      is_update_col->writeBoolean(0, 0, r.is_update);
      String id_str((const char*) r.record_id.data(), r.record_id.size());
      id_col->writeString(0, 0, id_str);
      version_col->writeUnsignedInt(0, 0, r.record_version);
      sequence_col->writeUnsignedInt(0, 0, sequence++);
      vmap.emplace(r.record_id, r.record_version);
    });

    cstable->commit();
    RecordVersionMap::write(vmap, filename + ".idx");
  }
}

ReplicationState LSMPartitionWriter::fetchReplicationState() const {
  auto snap = head_->getSnapshot();
  auto repl_state = snap->state.replication_state();
  String tbl_uuid((char*) snap->uuid().data(), snap->uuid().size());

  if (repl_state.uuid() == tbl_uuid) {
    return repl_state;
  } else {
    ReplicationState state;
    state.set_uuid(tbl_uuid);
    return state;
  }
}

void LSMPartitionWriter::commitReplicationState(const ReplicationState& state) {
  ScopedLock<std::mutex> write_lk(mutex_);
  auto snap = head_->getSnapshot()->clone();
  *snap->state.mutable_replication_state() = state;
  snap->writeToDisk();
  head_->setSnapshot(snap);
}

void LSMPartitionWriter::upgradeFromV1() {
  ScopedLock<std::mutex> upgrade_lk(upgrade_mutex_);
  auto snap = head_->getSnapshot();

  logNotice(
      "z1.core",
      "Upgrading partition $0/$1/$2 to LSM storage v0.2.0",
      snap->state.tsdb_namespace(),
      snap->state.table_key(),
      snap->key.toString());

  auto cst_filepath = FileUtil::joinPaths(snap->base_path, "_cstable");
  if (FileUtil::exists(cst_filepath)) {
    FileUtil::rm(cst_filepath);
  }

  auto cstm_filepath = FileUtil::joinPaths(snap->base_path, "_cstable_state");
  if (FileUtil::exists(cstm_filepath)) {
    FileUtil::rm(cstm_filepath);
  }

  auto files = snap->state.sstable_files();
  size_t nrecs = 0;
  for (const auto& f : files) {
    auto fpath = FileUtil::joinPaths(snap->base_path, f);
    sstable::SSTableReader reader(fpath);

    Vector<RecordRef> records;
    auto cursor = reader.getCursor();
    while (cursor->valid()) {
      void* key;
      size_t key_size;
      cursor->getKey(&key, &key_size);
      if (key_size != SHA1Hash::kSize) {
        RAISE(kRuntimeError, "invalid row");
      }

      void* data;
      size_t data_size;
      cursor->getData(&data, &data_size);

      ++nrecs;
      auto record_id = SHA1Hash(key, key_size);
      records.emplace_back(record_id, 1, Buffer(data, data_size));

      if (records.size() == 8192) {
        insertRecords(records);
        records.clear();
        commit();
      }

      if (!cursor->next()) {
        break;
      }
    }

    insertRecords(records);
  }

  commit();

  {
    ScopedLock<std::mutex> write_lk(mutex_);
    auto snap = head_->getSnapshot()->clone();
    snap->state.mutable_sstable_files()->Clear();
    snap->writeToDisk();
    head_->setSnapshot(snap);
  }

  for (const auto& f : files) {
    auto fpath = FileUtil::joinPaths(snap->base_path, f);
    FileUtil::mv(fpath, fpath + ".DELETED_BY_UPGRADE");
  }

  auto idset_filepath = FileUtil::joinPaths(snap->base_path, "_idset");
  if (FileUtil::exists(idset_filepath)) {
    FileUtil::mv(idset_filepath, idset_filepath + ".DELETED_BY_UPGRADE");
  }

  auto repl_filepath = FileUtil::joinPaths(snap->base_path, "_repl");
  if (FileUtil::exists(repl_filepath)) {
    FileUtil::rm(repl_filepath);
  }

  logNotice(
      "z1.core",
      "Upgrade for partition $1/$2/$3 ($0 records) to LSM storage v0.2.0 done",
      nrecs,
      snap->state.tsdb_namespace(),
      snap->state.table_key(),
      snap->key.toString());
}

bool LSMPartitionWriter::needsUpgradeFromV1() {
  auto snap = head_->getSnapshot();
  return
      snap->state.sstable_files().size() > 0 ||
      FileUtil::exists(FileUtil::joinPaths(snap->base_path, "_idset")) ||
      FileUtil::exists(FileUtil::joinPaths(snap->base_path, "_repl")) ||
      FileUtil::exists(FileUtil::joinPaths(snap->base_path, "_cstable_state")) ||
      FileUtil::exists(FileUtil::joinPaths(snap->base_path, "_cstable"));
}

} // namespace tdsb
<commit_msg>compact after upgrade<commit_after>/**
 * This file is part of the "tsdb" project
 *   Copyright (c) 2015 Paul Asmuth, FnordCorp B.V.
 *
 * FnordMetric is free software: you can redistribute it and/or modify it under
 * the terms of the GNU General Public License v3.0. You should have received a
 * copy of the GNU General Public License along with this program. If not, see
 * <http://www.gnu.org/licenses/>.
 */
#include <stx/io/fileutil.h>
#include <zbase/core/Partition.h>
#include <zbase/core/LSMPartitionWriter.h>
#include <zbase/core/RecordVersionMap.h>
#include <stx/protobuf/msg.h>
#include <stx/logging.h>
#include <stx/wallclock.h>
#include <stx/logging.h>
#include <sstable/SSTableWriter.h>
#include <stx/protobuf/MessageDecoder.h>
#include <cstable/RecordShredder.h>
#include <cstable/CSTableWriter.h>
#include <sstable/sstablereader.h>

using namespace stx;

namespace zbase {

LSMPartitionWriter::LSMPartitionWriter(
    RefPtr<Partition> partition,
    PartitionSnapshotRef* head) :
    PartitionWriter(head),
    partition_(partition),
    compaction_strategy_(
        new SimpleCompactionStrategy(
            partition_->getTable(),
            head->getSnapshot()->base_path)),
    max_datafile_size_(kDefaultMaxDatafileSize) {}

Set<SHA1Hash> LSMPartitionWriter::insertRecords(const Vector<RecordRef>& records) {
  std::unique_lock<std::mutex> lk(mutex_);
  if (frozen_) {
    RAISE(kIllegalStateError, "partition is frozen");
  }

  auto snap = head_->getSnapshot();

  stx::logTrace(
      "tsdb",
      "Insert $0 record into partition $1/$2/$3",
      records.size(),
      snap->state.tsdb_namespace(),
      snap->state.table_key(),
      snap->key.toString());

  HashMap<SHA1Hash, uint64_t> rec_versions;
  for (const auto& r : records) {
    if (snap->head_arena->fetchRecordVersion(r.record_id) < r.record_version) {
      rec_versions.emplace(r.record_id, 0);
    }
  }

  if (snap->compacting_arena.get() != nullptr) {
    for (auto& r : rec_versions) {
      auto v = snap->compacting_arena->fetchRecordVersion(r.first);
      if (v > r.second) {
        r.second = v;
      }
    }
  }

  const auto& tables = snap->state.lsm_tables();
  for (auto tbl = tables.rbegin(); tbl != tables.rend(); ++tbl) {
    RecordVersionMap::lookup(
        &rec_versions,
        FileUtil::joinPaths(snap->base_path, tbl->filename() + ".idx"));

    // FIMXE early exit...
  }

  Set<SHA1Hash> inserted_ids;
  if (!rec_versions.empty()) {
    for (auto r : records) {
      auto headv = rec_versions[r.record_id];
      if (headv > 0) {
        r.is_update = true;
      }

      if (r.record_version <= headv) {
        continue;
      }

      if (snap->head_arena->insertRecord(r)) {
        inserted_ids.emplace(r.record_id);
      }
    }
  }

  return inserted_ids;
}

bool LSMPartitionWriter::needsCommit() {
  ScopedLock<std::mutex> write_lk(mutex_);
  return head_->getSnapshot()->head_arena->size() > 0;
}

bool LSMPartitionWriter::needsCompaction() {
  if (needsCommit() || needsUpgradeFromV1()) {
    return true;
  }

  auto snap = head_->getSnapshot();
  return compaction_strategy_->needsCompaction(
      Vector<LSMTableRef>(
          snap->state.lsm_tables().begin(),
          snap->state.lsm_tables().end()));
}

bool LSMPartitionWriter::commit() {
  ScopedLock<std::mutex> commit_lk(commit_mutex_);
  RefPtr<RecordArena> arena;

  // flip arenas if records pending
  {
    ScopedLock<std::mutex> write_lk(mutex_);
    auto snap = head_->getSnapshot()->clone();
    if (snap->compacting_arena.get() == nullptr &&
        snap->head_arena->size() > 0) {
      snap->compacting_arena = snap->head_arena;
      snap->head_arena = mkRef(new RecordArena());
      head_->setSnapshot(snap);
    }
    arena = snap->compacting_arena;
  }

  // flush arena to disk if pending
  if (arena.get() && arena->size() > 0) {
    auto snap = head_->getSnapshot();
    auto filename = Random::singleton()->hex64();
    auto filepath = FileUtil::joinPaths(snap->base_path, filename);
    auto t0 = WallClock::unixMicros();
    writeArenaToDisk(arena, snap->state.lsm_sequence() + 1, filepath);
    auto t1 = WallClock::unixMicros();

    stx::logDebug(
        "z1.core",
        "Committing partition $1/$2/$3 ($0 records), took $4s",
        arena->size(),
        snap->state.tsdb_namespace(),
        snap->state.table_key(),
        snap->key.toString(),
        (double) (t1 - t0) / 1000000.0f);

    ScopedLock<std::mutex> write_lk(mutex_);
    snap = head_->getSnapshot()->clone();
    auto tblref = snap->state.add_lsm_tables();
    tblref->set_filename(filename);
    tblref->set_first_sequence(snap->state.lsm_sequence() + 1);
    tblref->set_last_sequence(snap->state.lsm_sequence() + arena->size());
    snap->state.set_lsm_sequence(snap->state.lsm_sequence() + arena->size());
    snap->compacting_arena = nullptr;
    snap->writeToDisk();
    head_->setSnapshot(snap);
    return true;
  } else {
    return false;
  }
}

bool LSMPartitionWriter::compact() {
  if (needsUpgradeFromV1()) {
    upgradeFromV1();
  }

  auto dirty = commit();

  // fetch current table list
  auto snap = head_->getSnapshot()->clone();

  Vector<LSMTableRef> new_tables;
  Vector<LSMTableRef> old_tables(
      snap->state.lsm_tables().begin(),
      snap->state.lsm_tables().end());

  // compact
  auto t0 = WallClock::unixMicros();
  if (!compaction_strategy_->compact(old_tables, &new_tables)) {
    return dirty;
  }
  auto t1 = WallClock::unixMicros();

  stx::logDebug(
      "z1.core",
      "Compacting partition $0/$1/$2, took $3s",
      snap->state.tsdb_namespace(),
      snap->state.table_key(),
      snap->key.toString(),
      (double) (t1 - t0) / 1000000.0f);

  // commit table list
  ScopedLock<std::mutex> write_lk(mutex_);
  snap = head_->getSnapshot()->clone();

  if (snap->state.lsm_tables().size() < old_tables.size()) {
    RAISE(kConcurrentModificationError, "concurrent compaction");
  }

  size_t i = 0;
  for (const auto& tbl : snap->state.lsm_tables()) {
    if (i < old_tables.size()) {
      if (old_tables[i].filename() != tbl.filename()) {
        RAISE(kConcurrentModificationError, "concurrent compaction");
      }
    } else {
      new_tables.push_back(tbl);
    }

    ++i;
  }

  snap->state.mutable_lsm_tables()->Clear();
  for (const auto& tbl :  new_tables) {
    *snap->state.add_lsm_tables() = tbl;
  }

  snap->writeToDisk();
  head_->setSnapshot(snap);
  write_lk.unlock();

  Set<String> delete_filenames;
  for (const auto& tbl : old_tables) {
    delete_filenames.emplace(tbl.filename());
  }
  for (const auto& tbl : new_tables) {
    delete_filenames.erase(tbl.filename());
  }

  for (const auto& f : delete_filenames) {
    // FIXME: delayed delete
    FileUtil::rm(FileUtil::joinPaths(snap->base_path, f + ".cst"));
    FileUtil::rm(FileUtil::joinPaths(snap->base_path, f + ".idx"));
  }

  return true;
}

void LSMPartitionWriter::writeArenaToDisk(
      RefPtr<RecordArena> arena,
      uint64_t sequence,
      const String& filename) {
  auto schema = partition_->getTable()->schema();

  {
    OrderedMap<SHA1Hash, uint64_t> vmap;
    auto cstable_schema = cstable::TableSchema::fromProtobuf(*schema);
    auto cstable_schema_ext = cstable_schema;
    cstable_schema_ext.addBool("__lsm_is_update", false);
    cstable_schema_ext.addString("__lsm_id", false);
    cstable_schema_ext.addUnsignedInteger("__lsm_version", false);
    cstable_schema_ext.addUnsignedInteger("__lsm_sequence", false);

    auto cstable = cstable::CSTableWriter::createFile(
        filename + ".cst",
        cstable::BinaryFormatVersion::v0_1_0,
        cstable_schema_ext);

    cstable::RecordShredder shredder(cstable.get(), &cstable_schema);
    auto is_update_col = cstable->getColumnWriter("__lsm_is_update");
    auto id_col = cstable->getColumnWriter("__lsm_id");
    auto version_col = cstable->getColumnWriter("__lsm_version");
    auto sequence_col = cstable->getColumnWriter("__lsm_sequence");

    arena->fetchRecords([&] (const RecordRef& r) {
      msg::MessageObject obj;
      msg::MessageDecoder::decode(r.record, *schema, &obj);
      shredder.addRecordFromProtobuf(obj, *schema);
      is_update_col->writeBoolean(0, 0, r.is_update);
      String id_str((const char*) r.record_id.data(), r.record_id.size());
      id_col->writeString(0, 0, id_str);
      version_col->writeUnsignedInt(0, 0, r.record_version);
      sequence_col->writeUnsignedInt(0, 0, sequence++);
      vmap.emplace(r.record_id, r.record_version);
    });

    cstable->commit();
    RecordVersionMap::write(vmap, filename + ".idx");
  }
}

ReplicationState LSMPartitionWriter::fetchReplicationState() const {
  auto snap = head_->getSnapshot();
  auto repl_state = snap->state.replication_state();
  String tbl_uuid((char*) snap->uuid().data(), snap->uuid().size());

  if (repl_state.uuid() == tbl_uuid) {
    return repl_state;
  } else {
    ReplicationState state;
    state.set_uuid(tbl_uuid);
    return state;
  }
}

void LSMPartitionWriter::commitReplicationState(const ReplicationState& state) {
  ScopedLock<std::mutex> write_lk(mutex_);
  auto snap = head_->getSnapshot()->clone();
  *snap->state.mutable_replication_state() = state;
  snap->writeToDisk();
  head_->setSnapshot(snap);
}

void LSMPartitionWriter::upgradeFromV1() {
  ScopedLock<std::mutex> upgrade_lk(upgrade_mutex_);
  auto snap = head_->getSnapshot();

  logNotice(
      "z1.core",
      "Upgrading partition $0/$1/$2 to LSM storage v0.2.0",
      snap->state.tsdb_namespace(),
      snap->state.table_key(),
      snap->key.toString());

  auto cst_filepath = FileUtil::joinPaths(snap->base_path, "_cstable");
  if (FileUtil::exists(cst_filepath)) {
    FileUtil::rm(cst_filepath);
  }

  auto cstm_filepath = FileUtil::joinPaths(snap->base_path, "_cstable_state");
  if (FileUtil::exists(cstm_filepath)) {
    FileUtil::rm(cstm_filepath);
  }

  auto files = snap->state.sstable_files();
  size_t nrecs = 0;
  for (const auto& f : files) {
    auto fpath = FileUtil::joinPaths(snap->base_path, f);
    sstable::SSTableReader reader(fpath);

    Vector<RecordRef> records;
    auto cursor = reader.getCursor();
    while (cursor->valid()) {
      void* key;
      size_t key_size;
      cursor->getKey(&key, &key_size);
      if (key_size != SHA1Hash::kSize) {
        RAISE(kRuntimeError, "invalid row");
      }

      void* data;
      size_t data_size;
      cursor->getData(&data, &data_size);

      ++nrecs;
      auto record_id = SHA1Hash(key, key_size);
      records.emplace_back(record_id, 1, Buffer(data, data_size));

      if (records.size() == 8192) {
        insertRecords(records);
        records.clear();
        commit();
      }

      if (!cursor->next()) {
        break;
      }
    }

    insertRecords(records);
  }

  commit();

  {
    ScopedLock<std::mutex> write_lk(mutex_);
    auto snap = head_->getSnapshot()->clone();
    snap->state.mutable_sstable_files()->Clear();
    snap->writeToDisk();
    head_->setSnapshot(snap);
  }

  for (const auto& f : files) {
    auto fpath = FileUtil::joinPaths(snap->base_path, f);
    FileUtil::mv(fpath, fpath + ".DELETED_BY_UPGRADE");
  }

  auto idset_filepath = FileUtil::joinPaths(snap->base_path, "_idset");
  if (FileUtil::exists(idset_filepath)) {
    FileUtil::mv(idset_filepath, idset_filepath + ".DELETED_BY_UPGRADE");
  }

  auto repl_filepath = FileUtil::joinPaths(snap->base_path, "_repl");
  if (FileUtil::exists(repl_filepath)) {
    FileUtil::rm(repl_filepath);
  }

  logNotice(
      "z1.core",
      "Upgrade for partition $1/$2/$3 ($0 records) to LSM storage v0.2.0 done",
      nrecs,
      snap->state.tsdb_namespace(),
      snap->state.table_key(),
      snap->key.toString());

  compact();
}

bool LSMPartitionWriter::needsUpgradeFromV1() {
  auto snap = head_->getSnapshot();
  return
      snap->state.sstable_files().size() > 0 ||
      FileUtil::exists(FileUtil::joinPaths(snap->base_path, "_idset")) ||
      FileUtil::exists(FileUtil::joinPaths(snap->base_path, "_repl")) ||
      FileUtil::exists(FileUtil::joinPaths(snap->base_path, "_cstable_state")) ||
      FileUtil::exists(FileUtil::joinPaths(snap->base_path, "_cstable"));
}

} // namespace tdsb
<|endoftext|>


<commit_before>// moderngpu copyright (c) 2016, Sean Baxter http://www.moderngpu.com
#pragma once
#include "kernel_sortedsearch.hxx"
#include "kernel_scan.hxx"
#include "kernel_load_balance.hxx"

BEGIN_MGPU_NAMESPACE

template<typename launch_arg_t = empty_t,
  typename a_it, typename b_it, typename comp_t>
mem_t<int2> inner_join(a_it a, int a_count, b_it b, int b_count,
  comp_t comp, context_t& context) {

  // Compute lower and upper bounds of a into b.
  mem_t<int> lower(a_count, context);
  mem_t<int> upper(b_count, context);
  sorted_search<bounds_lower, launch_arg_t>(a, a_count, b, b_count,
    lower.data(), comp, context);
  sorted_search<bounds_upper, launch_arg_t>(a, a_count, b, b_count,
    upper.data(), comp, context);

  // Compute output ranges by scanning upper - lower. Retrieve the reduction
  // of the scan, which specifies the size of the output array to allocate.
  mem_t<int> scanned_sizes(a_count, context);
  const int* lower_data = lower.data();
  const int* upper_data = upper.data();

  mem_t<int> count(1, context);
  transform_scan([=]MGPU_DEVICE(int index) {
    return upper_data[index] - lower_data[index];
  }, a_count, scanned_sizes.data(), plus_t<int>(), count.data(), context);

  // Allocate an int2 output array and use load-balancing search to compute
  // the join.
  int join_count = from_mem(count)[0];
  mem_t<int2> output(join_count, context);
  int2* output_data = output.data();

  // Use load-balancing search on the segmens. The output is a pair with
  // a_index = seg and b_index = lower_data[seg] + rank.
  auto k = [=]MGPU_DEVICE(int index, int seg, int rank, tuple<int> lower) {
    output_data[index] = make_int2(seg, get<0>(lower) + rank);
  };
  transform_lbs<launch_arg_t>(k, join_count, scanned_sizes.data(), a_count,
    make_tuple(lower_data), context);

  return output;
}

END_MGPU_NAMESPACE
<commit_msg>Fixed array sizing bug in inner_join<commit_after>// moderngpu copyright (c) 2016, Sean Baxter http://www.moderngpu.com
#pragma once
#include "kernel_sortedsearch.hxx"
#include "kernel_scan.hxx"
#include "kernel_load_balance.hxx"

BEGIN_MGPU_NAMESPACE

template<typename launch_arg_t = empty_t,
  typename a_it, typename b_it, typename comp_t>
mem_t<int2> inner_join(a_it a, int a_count, b_it b, int b_count,
  comp_t comp, context_t& context) {

  // Compute lower and upper bounds of a into b.
  mem_t<int> lower(a_count, context);
  mem_t<int> upper(a_count, context);
  sorted_search<bounds_lower, launch_arg_t>(a, a_count, b, b_count,
    lower.data(), comp, context);
  sorted_search<bounds_upper, launch_arg_t>(a, a_count, b, b_count,
    upper.data(), comp, context);

  // Compute output ranges by scanning upper - lower. Retrieve the reduction
  // of the scan, which specifies the size of the output array to allocate.
  mem_t<int> scanned_sizes(a_count, context);
  const int* lower_data = lower.data();
  const int* upper_data = upper.data();

  mem_t<int> count(1, context);
  transform_scan([=]MGPU_DEVICE(int index) {
    return upper_data[index] - lower_data[index];
  }, a_count, scanned_sizes.data(), plus_t<int>(), count.data(), context);

  // Allocate an int2 output array and use load-balancing search to compute
  // the join.
  int join_count = from_mem(count)[0];
  mem_t<int2> output(join_count, context);
  int2* output_data = output.data();

  // Use load-balancing search on the segmens. The output is a pair with
  // a_index = seg and b_index = lower_data[seg] + rank.
  auto k = [=]MGPU_DEVICE(int index, int seg, int rank, tuple<int> lower) {
    output_data[index] = make_int2(seg, get<0>(lower) + rank);
  };
  transform_lbs<launch_arg_t>(k, join_count, scanned_sizes.data(), a_count,
    make_tuple(lower_data), context);

  return output;
}

END_MGPU_NAMESPACE
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2007 The Hewlett-Packard Development Company
 * All rights reserved.
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * 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 holders 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.
 *
 * Authors: Gabe Black
 */

#include <string>

#include "arch/x86/pagetable_walker.hh"
#include "arch/x86/tlb.hh"
#include "arch/x86/vtophys.hh"
#include "base/trace.hh"
#include "config/full_system.hh"
#include "cpu/thread_context.hh"
#include "sim/fault.hh"

using namespace std;

namespace X86ISA
{
    Addr
    vtophys(Addr vaddr)
    {
#if FULL_SYSTEM
        panic("Need access to page tables\n");
#endif
        return vaddr;
    }

    Addr
    vtophys(ThreadContext *tc, Addr vaddr)
    {
#if FULL_SYSTEM
        Walker *walker = tc->getDTBPtr()->getWalker();
        Addr size;
        Addr addr = vaddr;
        Fault fault = walker->startFunctional(tc, addr, size, BaseTLB::Read);
        if (fault != NoFault)
            panic("vtophys page walk returned fault\n");
        Addr masked_addr = vaddr & (size - 1);
        Addr paddr = addr | masked_addr;
        DPRINTF(VtoPhys, "vtophys(%#x) -> %#x\n", vaddr, paddr);
        return paddr;
#endif
        return vaddr;
    }
}
<commit_msg>X86: Fix compiling vtophys.cc<commit_after>/*
 * Copyright (c) 2007 The Hewlett-Packard Development Company
 * All rights reserved.
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * 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 holders 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.
 *
 * Authors: Gabe Black
 */

#include <string>

#include "arch/x86/pagetable_walker.hh"
#include "arch/x86/tlb.hh"
#include "arch/x86/vtophys.hh"
#include "base/trace.hh"
#include "config/full_system.hh"
#include "cpu/thread_context.hh"
#include "sim/fault_fwd.hh"

using namespace std;

namespace X86ISA
{
    Addr
    vtophys(Addr vaddr)
    {
#if FULL_SYSTEM
        panic("Need access to page tables\n");
#endif
        return vaddr;
    }

    Addr
    vtophys(ThreadContext *tc, Addr vaddr)
    {
#if FULL_SYSTEM
        Walker *walker = tc->getDTBPtr()->getWalker();
        Addr size;
        Addr addr = vaddr;
        Fault fault = walker->startFunctional(tc, addr, size, BaseTLB::Read);
        if (fault != NoFault)
            panic("vtophys page walk returned fault\n");
        Addr masked_addr = vaddr & (size - 1);
        Addr paddr = addr | masked_addr;
        DPRINTF(VtoPhys, "vtophys(%#x) -> %#x\n", vaddr, paddr);
        return paddr;
#endif
        return vaddr;
    }
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) [2014-2015] Novell, Inc.
 * Copyright (c) [2016-2017] SUSE LLC
 *
 * All Rights Reserved.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as published
 * by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, contact Novell, Inc.
 *
 * To contact Novell about this file by physical or electronic mail, you may
 * find current contact information at www.novell.com.
 */

#include <boost/algorithm/string.hpp>

#include "storage/Devices/PartitionableImpl.h"
#include "storage/Devices/PartitionTableImpl.h"
#include "storage/Devices/Msdos.h"
#include "storage/Devices/Gpt.h"
#include "storage/Devices/DasdPt.h"
#include "storage/Devices/ImplicitPt.h"
#include "storage/Holders/User.h"
#include "storage/Devicegraph.h"
#include "storage/Action.h"
#include "storage/Utils/XmlFile.h"
#include "storage/Utils/Enum.h"
#include "storage/Utils/StorageTmpl.h"
#include "storage/Utils/StorageDefines.h"
#include "storage/Utils/SystemCmd.h"
#include "storage/SystemInfo/SystemInfo.h"
#include "storage/StorageImpl.h"
#include "storage/Prober.h"

namespace storage
{

    using namespace std;

    const char* DeviceTraits<Partitionable>::classname = "Partitionable";

    const unsigned int Partitionable::Impl::default_range;

    Partitionable::Impl::Impl(const xmlNode* node)
	: BlkDevice::Impl(node), topology(), range(0)
    {
	getChildValue(node, "topology", topology);
	getChildValue(node, "range", range);
    }

    void
    Partitionable::Impl::check(const CheckCallbacks* check_callbacks) const
    {
	BlkDevice::Impl::check(check_callbacks);

	if (get_region().get_start() != 0)
	    ST_THROW(Exception("start of region of partitionable not zero"));
    }

    void
    Partitionable::Impl::save(xmlNode* node) const
    {
	BlkDevice::Impl::save(node);

	setChildValue(node, "topology", topology);
	setChildValue(node, "range", range);
    }

    void
    Partitionable::Impl::probe_pass_1a(Prober& prober)
    {
	BlkDevice::Impl::probe_pass_1a(prober);

	const File size_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() + "/size");
	const File logical_block_size_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() +
									      "/queue/logical_block_size");

	// size is always in 512 byte blocks
	unsigned long long a = size_file.get<unsigned long long>();
	unsigned long long b = logical_block_size_file.get<unsigned long long>();
	unsigned long long c = a * 512 / b;
	set_region(Region(0, c, b));

	const File alignment_offset_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() +
									    "/alignment_offset");
	topology.set_alignment_offset(alignment_offset_file.get<int>());

	const File optimal_io_size_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() +
									   "/queue/optimal_io_size");
	topology.set_optimal_io_size(optimal_io_size_file.get<int>());

	if (get_dm_table_name().empty())
	{
	    const File range_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() +
								     "/ext_range");
	    range = range_file.get<int>();
	}
    }

    void
    Partitionable::Impl::probe_pass_1c(Prober& prober)
    {
	if (has_children() || !is_active() || get_size() == 0)
	    return;

	const Parted& parted = prober.get_system_info().getParted(get_name());
	if (parted.get_label() == PtType::MSDOS || parted.get_label() == PtType::GPT ||
	    parted.get_label() == PtType::DASD)
	{
	    if (get_region().get_block_size() != parted.get_region().get_block_size())
		ST_THROW(Exception("different block size reported by kernel and parted"));

	    if (get_region().get_length() != parted.get_region().get_length())
		ST_THROW(Exception("different size reported by kernel and parted"));

	    PtType label = parted.get_label();

	    // parted reports DASD partition table for implicit partition
	    // tables. Convert that to implicit partition table.

	    if (is_dasd(get_non_impl()))
	    {
		const Dasd* dasd = to_dasd(get_non_impl());

		if (dasd->get_type() == DasdType::ECKD && dasd->get_format() == DasdFormat::LDL)
		    label = PtType::IMPLICIT;

		if (dasd->get_type() == DasdType::FBA && parted.is_implicit())
		    label = PtType::IMPLICIT;
	    }

	    PartitionTable* partition_table = create_partition_table(label);
	    partition_table->get_impl().probe_pass_1c(prober);
	}
    }

    PtType
    Partitionable::Impl::get_default_partition_table_type() const
    {
	vector<PtType> possible = get_possible_partition_table_types();

	if (possible.empty())
	    ST_THROW(Exception("no partition table possible"));

	PtType ret = possible.front();

	y2mil("ret:" << toString(ret));

	return ret;
    }

    vector<PtType>
    Partitionable::Impl::get_possible_partition_table_types() const
    {
	// ECKD DASDs are handled completely and FBA DASDs partly in Dasd
	// class.

	unsigned long long int num_sectors = get_region().get_length();
	bool size_ok_for_msdos = num_sectors <= UINT32_MAX;
	y2mil("num-sectors:" << num_sectors << " size-ok-for-msdos:" << size_ok_for_msdos);

	PtType best = PtType::GPT;
	vector<PtType> ret = { best };

	if (size_ok_for_msdos)
	    ret.push_back(PtType::MSDOS);

	return ret;
    }

    PartitionTable*
    Partitionable::Impl::create_partition_table(PtType pt_type)
    {
	if (num_children() != 0)
	    ST_THROW(WrongNumberOfChildren(num_children(), 0));

	PartitionTable* ret = nullptr;

	switch (pt_type)
	{
	    case PtType::MSDOS:
		ret = Msdos::create(get_devicegraph());
		break;

	    case PtType::GPT:
		ret = Gpt::create(get_devicegraph());
		break;

	    case PtType::DASD:
		ret = DasdPt::create(get_devicegraph());
		break;

	    case PtType::IMPLICIT:
		ret = ImplicitPt::create(get_devicegraph());
		break;

	    default:
		ST_THROW(UnsupportedException("unsupported partition table type " + toString(pt_type)));
	}

	User::create(get_devicegraph(), get_non_impl(), ret);

	if (pt_type == PtType::IMPLICIT)
	{
	    to_implicit_pt(ret)->create_implicit_partition();
	}

	return ret;
    }

    bool
    Partitionable::Impl::has_partition_table() const
    {
	return has_single_child_of_type<const PartitionTable>();
    }

    PartitionTable*
    Partitionable::Impl::get_partition_table()
    {
	return get_single_child_of_type<PartitionTable>();
    }

    const PartitionTable*
    Partitionable::Impl::get_partition_table() const
    {
	return get_single_child_of_type<const PartitionTable>();
    }

    string
    Partitionable::Impl::partition_name(int number) const
    {
	if (boost::starts_with(get_name(), DEV_MAPPER_DIR "/"))
	    return get_name() + "-part" + to_string(number);
	else if (isdigit(get_name().back()))
	    return get_name() + "p" + to_string(number);
	else
	    return get_name() + to_string(number);
    }

    bool
    Partitionable::Impl::equal(const Device::Impl& rhs_base) const
    {
	const Impl& rhs = dynamic_cast<const Impl&>(rhs_base);

	if (!BlkDevice::Impl::equal(rhs))
	    return false;

	return topology == rhs.topology && range == rhs.range;
    }

    void
    Partitionable::Impl::log_diff(std::ostream& log, const Device::Impl& rhs_base) const
    {
	const Impl& rhs = dynamic_cast<const Impl&>(rhs_base);

	BlkDevice::Impl::log_diff(log, rhs);

	storage::log_diff(log, "topology", topology, rhs.topology);
	storage::log_diff(log, "range", range, rhs.range);
    }

    void
    Partitionable::Impl::print(std::ostream& out) const
    {
	BlkDevice::Impl::print(out);

	out << " topology:" << topology
	    << " range:" << range;
    }

}
<commit_msg>- improved error message (see bsc#1089554)<commit_after>/*
 * Copyright (c) [2014-2015] Novell, Inc.
 * Copyright (c) [2016-2017] SUSE LLC
 *
 * All Rights Reserved.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as published
 * by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, contact Novell, Inc.
 *
 * To contact Novell about this file by physical or electronic mail, you may
 * find current contact information at www.novell.com.
 */

#include <boost/algorithm/string.hpp>

#include "storage/Devices/PartitionableImpl.h"
#include "storage/Devices/PartitionTableImpl.h"
#include "storage/Devices/Msdos.h"
#include "storage/Devices/Gpt.h"
#include "storage/Devices/DasdPt.h"
#include "storage/Devices/ImplicitPt.h"
#include "storage/Holders/User.h"
#include "storage/Devicegraph.h"
#include "storage/Action.h"
#include "storage/Utils/XmlFile.h"
#include "storage/Utils/Enum.h"
#include "storage/Utils/StorageTmpl.h"
#include "storage/Utils/StorageDefines.h"
#include "storage/Utils/SystemCmd.h"
#include "storage/SystemInfo/SystemInfo.h"
#include "storage/StorageImpl.h"
#include "storage/Prober.h"

namespace storage
{

    using namespace std;

    const char* DeviceTraits<Partitionable>::classname = "Partitionable";

    const unsigned int Partitionable::Impl::default_range;

    Partitionable::Impl::Impl(const xmlNode* node)
	: BlkDevice::Impl(node), topology(), range(0)
    {
	getChildValue(node, "topology", topology);
	getChildValue(node, "range", range);
    }

    void
    Partitionable::Impl::check(const CheckCallbacks* check_callbacks) const
    {
	BlkDevice::Impl::check(check_callbacks);

	if (get_region().get_start() != 0)
	    ST_THROW(Exception("start of region of partitionable not zero"));
    }

    void
    Partitionable::Impl::save(xmlNode* node) const
    {
	BlkDevice::Impl::save(node);

	setChildValue(node, "topology", topology);
	setChildValue(node, "range", range);
    }

    void
    Partitionable::Impl::probe_pass_1a(Prober& prober)
    {
	BlkDevice::Impl::probe_pass_1a(prober);

	const File size_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() + "/size");
	const File logical_block_size_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() +
									      "/queue/logical_block_size");

	// size is always in 512 byte blocks
	unsigned long long a = size_file.get<unsigned long long>();
	unsigned long long b = logical_block_size_file.get<unsigned long long>();
	unsigned long long c = a * 512 / b;
	set_region(Region(0, c, b));

	const File alignment_offset_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() +
									    "/alignment_offset");
	topology.set_alignment_offset(alignment_offset_file.get<int>());

	const File optimal_io_size_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() +
									   "/queue/optimal_io_size");
	topology.set_optimal_io_size(optimal_io_size_file.get<int>());

	if (get_dm_table_name().empty())
	{
	    const File range_file = prober.get_system_info().getFile(SYSFS_DIR + get_sysfs_path() +
								     "/ext_range");
	    range = range_file.get<int>();
	}
    }

    void
    Partitionable::Impl::probe_pass_1c(Prober& prober)
    {
	if (has_children() || !is_active() || get_size() == 0)
	    return;

	const Parted& parted = prober.get_system_info().getParted(get_name());
	if (parted.get_label() == PtType::MSDOS || parted.get_label() == PtType::GPT ||
	    parted.get_label() == PtType::DASD)
	{
	    if (get_region().get_block_size() != parted.get_region().get_block_size())
		ST_THROW(Exception(sformat("different block size reported by kernel and parted for %s",
					   get_name().c_str())));

	    if (get_region().get_length() != parted.get_region().get_length())
		ST_THROW(Exception(sformat("different size reported by kernel and parted for %s",
					   get_name().c_str())));

	    PtType label = parted.get_label();

	    // parted reports DASD partition table for implicit partition
	    // tables. Convert that to implicit partition table.

	    if (is_dasd(get_non_impl()))
	    {
		const Dasd* dasd = to_dasd(get_non_impl());

		if (dasd->get_type() == DasdType::ECKD && dasd->get_format() == DasdFormat::LDL)
		    label = PtType::IMPLICIT;

		if (dasd->get_type() == DasdType::FBA && parted.is_implicit())
		    label = PtType::IMPLICIT;
	    }

	    PartitionTable* partition_table = create_partition_table(label);
	    partition_table->get_impl().probe_pass_1c(prober);
	}
    }

    PtType
    Partitionable::Impl::get_default_partition_table_type() const
    {
	vector<PtType> possible = get_possible_partition_table_types();

	if (possible.empty())
	    ST_THROW(Exception("no partition table possible"));

	PtType ret = possible.front();

	y2mil("ret:" << toString(ret));

	return ret;
    }

    vector<PtType>
    Partitionable::Impl::get_possible_partition_table_types() const
    {
	// ECKD DASDs are handled completely and FBA DASDs partly in Dasd
	// class.

	unsigned long long int num_sectors = get_region().get_length();
	bool size_ok_for_msdos = num_sectors <= UINT32_MAX;
	y2mil("num-sectors:" << num_sectors << " size-ok-for-msdos:" << size_ok_for_msdos);

	PtType best = PtType::GPT;
	vector<PtType> ret = { best };

	if (size_ok_for_msdos)
	    ret.push_back(PtType::MSDOS);

	return ret;
    }

    PartitionTable*
    Partitionable::Impl::create_partition_table(PtType pt_type)
    {
	if (num_children() != 0)
	    ST_THROW(WrongNumberOfChildren(num_children(), 0));

	PartitionTable* ret = nullptr;

	switch (pt_type)
	{
	    case PtType::MSDOS:
		ret = Msdos::create(get_devicegraph());
		break;

	    case PtType::GPT:
		ret = Gpt::create(get_devicegraph());
		break;

	    case PtType::DASD:
		ret = DasdPt::create(get_devicegraph());
		break;

	    case PtType::IMPLICIT:
		ret = ImplicitPt::create(get_devicegraph());
		break;

	    default:
		ST_THROW(UnsupportedException("unsupported partition table type " + toString(pt_type)));
	}

	User::create(get_devicegraph(), get_non_impl(), ret);

	if (pt_type == PtType::IMPLICIT)
	{
	    to_implicit_pt(ret)->create_implicit_partition();
	}

	return ret;
    }

    bool
    Partitionable::Impl::has_partition_table() const
    {
	return has_single_child_of_type<const PartitionTable>();
    }

    PartitionTable*
    Partitionable::Impl::get_partition_table()
    {
	return get_single_child_of_type<PartitionTable>();
    }

    const PartitionTable*
    Partitionable::Impl::get_partition_table() const
    {
	return get_single_child_of_type<const PartitionTable>();
    }

    string
    Partitionable::Impl::partition_name(int number) const
    {
	if (boost::starts_with(get_name(), DEV_MAPPER_DIR "/"))
	    return get_name() + "-part" + to_string(number);
	else if (isdigit(get_name().back()))
	    return get_name() + "p" + to_string(number);
	else
	    return get_name() + to_string(number);
    }

    bool
    Partitionable::Impl::equal(const Device::Impl& rhs_base) const
    {
	const Impl& rhs = dynamic_cast<const Impl&>(rhs_base);

	if (!BlkDevice::Impl::equal(rhs))
	    return false;

	return topology == rhs.topology && range == rhs.range;
    }

    void
    Partitionable::Impl::log_diff(std::ostream& log, const Device::Impl& rhs_base) const
    {
	const Impl& rhs = dynamic_cast<const Impl&>(rhs_base);

	BlkDevice::Impl::log_diff(log, rhs);

	storage::log_diff(log, "topology", topology, rhs.topology);
	storage::log_diff(log, "range", range, rhs.range);
    }

    void
    Partitionable::Impl::print(std::ostream& out) const
    {
	BlkDevice::Impl::print(out);

	out << " topology:" << topology
	    << " range:" << range;
    }

}
<|endoftext|>


<commit_before>#include "controller_ros.h"

#include <tf/transform_datatypes.h>
#include <eigen_conversions/eigen_msg.h>

Controller::Controller(ros::NodeHandle nh) : nh(nh)
{
  command_sub = nh.subscribe("joystick_motion_command", 10, &Controller::commandCallback, this);
  state_sub   = nh.subscribe("state_estimate", 10, &Controller::stateCallback, this);
  wrench_pub  = nh.advertise<geometry_msgs::Wrench>("rov_forces", 10);

  control_mode = ControlModes::OPEN_LOOP;
  prev_time_valid = false;

  position_state.setZero();
  orientation_state.setIdentity();
  velocity_state.setZero();
  position_setpoint.setZero();
  orientation_setpoint.setIdentity();
  wrench_setpoint.setZero();

  getParams();

  // Read controller gains from parameter server
  std::map<std::string,double> gains;
  if (!nh.getParam("/controller/gains", gains))
    ROS_ERROR("Failed to read parameter controller gains.");

  // Read center of gravity and buoyancy vectors
  std::vector<double> r_G_vec, r_B_vec;
  if (!nh.getParam("/physical/center_of_mass", r_G_vec))
    ROS_ERROR("Failed to read robot center of mass parameter.");
  if (!nh.getParam("/physical/center_of_buoyancy", r_B_vec))
    ROS_ERROR("Failed to read robot center of buoyancy parameter.");
  Eigen::Vector3d r_G(r_G_vec.data());
  Eigen::Vector3d r_B(r_B_vec.data());

  // Read and calculate ROV weight and buoyancy
  double mass, displacement, acceleration_of_gravity, density_of_water;
  if (!nh.getParam("/physical/mass_kg", mass))
    ROS_ERROR("Failed to read parameter mass.");
  if (!nh.getParam("/physical/displacement_m3", displacement))
    ROS_ERROR("Failed to read parameter displacement.");
  if (!nh.getParam("/gravity/acceleration", acceleration_of_gravity))
    ROS_ERROR("Failed to read parameter acceleration of gravity");
  if (!nh.getParam("/water/density", density_of_water))
    ROS_ERROR("Failed to read parameter density of water");
  double W = mass * acceleration_of_gravity;
  double B = density_of_water * displacement * acceleration_of_gravity;

  position_hold_controller = new QuaternionPdController(gains["a"], gains["b"], gains["c"], W, B, r_G, r_B);

  // Set up a dynamic reconfigure server
  dynamic_reconfigure::Server<uranus_dp::ControllerConfig>::CallbackType cb;
  cb = boost::bind(&Controller::configCallback, this, _1, _2);
  dr_srv.setCallback(cb);

  ROS_INFO("Controller: Initialized with dynamic reconfigure.");
}

void Controller::commandCallback(const vortex_msgs::JoystickMotionCommand& msg)
{
  if (!healthyMessage(msg))
  {
    ROS_WARN("Controller: Propulsion command message out of range, ignoring...");
    return;
  }

  if (msg.control_mode != control_mode)
  {
    control_mode = static_cast<ControlMode>(msg.control_mode);
    switch (control_mode)
    {
      case ControlModes::OPEN_LOOP:
      ROS_INFO("Controller: Changing mode to OPEN LOOP.");
      break;

      case ControlModes::POSITION_HOLD:
      ROS_INFO("Controller: Changing mode to POSITION HOLD.");
      position_setpoint    = position_state;
      orientation_setpoint = orientation_state;
      prev_time_valid = false;
      break;
    }
  }

  updateSetpoints(msg);
}

void Controller::stateCallback(const nav_msgs::Odometry &msg)
{
  tf::pointMsgToEigen(msg.pose.pose.position, position_state);
  tf::quaternionMsgToEigen(msg.pose.pose.orientation, orientation_state);
}

void Controller::configCallback(uranus_dp::ControllerConfig &config, uint32_t level)
{
  ROS_INFO_STREAM("Setting quat pd gains:   a = " << config.a << ",   b = " << config.b << ",   c = " << config.c);
  position_hold_controller->setGains(config.a, config.b, config.c);
}

void Controller::spin()
{
  ros::Rate rate(frequency);
  while (ros::ok())
  {
    Eigen::Vector6d tau;
    switch (control_mode)
    {
      case ControlModes::POSITION_HOLD:
      tau = position_hold_controller->compute(position_state,
                                              orientation_state,
                                              velocity_state,
                                              position_setpoint,
                                              orientation_setpoint);
      break;

      case ControlModes::OPEN_LOOP:
      tau = wrench_setpoint;
      break;
    }

    geometry_msgs::Wrench msg;
    tf::wrenchEigenToMsg(tau, msg);
    wrench_pub.publish(msg);

    ros::spinOnce();
    rate.sleep();
  }
}

void Controller::updateSetpoints(const vortex_msgs::JoystickMotionCommand& msg)
{
  // Update wrench setpoints
  for (int i = 0; i < 6; ++i) // TODO: Fix magic number
    wrench_setpoint(i) = wrench_command_scaling[i] * wrench_command_max[i] * msg.motion[i];

  // Update pose setpoints
  if (!prev_time_valid)
  {
    prev_time = msg.header.stamp;
    prev_time_valid = true;
    return;
  }

  // Calculate time difference
  ros::Time curr_time = msg.header.stamp;
  double dt = (curr_time - prev_time).toSec();
  prev_time = curr_time;

  if (dt == 0)
  {
    // ROS_WARN("Zero time difference between propulsion command messages.");
    return;
  }

  // Increment position setpoints
  for (int i = 0; i < 3; ++i) // TODO: Fix magic number
    position_setpoint(i) += pose_command_rate[i] * dt * msg.motion[i];

  // Calc euler setpoints
  Eigen::Vector3d orientation_setpoint_euler;
  orientation_setpoint_euler = orientation_setpoint.toRotationMatrix().eulerAngles(0,1,2);
  // Increment euler setpoints
  for (int i = 0; i < 3; ++i) // TODO: Fix magic number
    orientation_setpoint_euler(i) += pose_command_rate[3+i] * dt * msg.motion[3+i];
  // Calc incremented quat setpoints
  Eigen::Matrix3d R;
  R = Eigen::AngleAxisd(orientation_setpoint_euler(0), Eigen::Vector3d::UnitX())
    * Eigen::AngleAxisd(orientation_setpoint_euler(1), Eigen::Vector3d::UnitY())
    * Eigen::AngleAxisd(orientation_setpoint_euler(2), Eigen::Vector3d::UnitZ());
  Eigen::Quaterniond q(R);
  orientation_setpoint = q;
}

void Controller::getParams()
{
  // Read control command parameters
  if (!nh.getParam("/propulsion/command/wrench/max", wrench_command_max))
    ROS_FATAL("Failed to read parameter max wrench command.");
  if (!nh.getParam("/propulsion/command/wrench/scaling", wrench_command_scaling))
    ROS_FATAL("Failed to read parameter scaling wrench command.");
  if (!nh.getParam("/propulsion/command/pose/rate", pose_command_rate))
    ROS_FATAL("Failed to read parameter pose command rate.");

  if (!nh.getParam("/controller/frequency", frequency))
  {
    ROS_WARN("Failed to read parameter controller frequency, defaulting to 10 Hz.");
    frequency = 10;
  }
}

bool Controller::healthyMessage(const vortex_msgs::JoystickMotionCommand& msg)
{
  if (abs(msg.forward) > 1)
  {
    ROS_WARN("controller: Forward motion command out of range");
    return false;
  }
  if (abs(msg.right) > 1)
  {
    ROS_WARN("controller: Right motion command out of range");
    return false;
  }
  if (abs(msg.down) > 1)
  {
    ROS_WARN("controller: Down motion command out of range.");
    return false;
  }
  if (abs(msg.tilt_up) > 1)
  {
    ROS_WARN("controller: Tilt up motion command out of range");
    return false;
  }
  if (abs(msg.turn_right) > 1)
  {
    ROS_WARN("controller: Turn right motion command out of range");
    return false;
  }

  bool validControlMode = (msg.control_mode == ControlModes::OPEN_LOOP || msg.control_mode == ControlModes::POSITION_HOLD);
  if (!validControlMode)
  {
    ROS_WARN("controller: Invalid control mode.");
    return false;
  }
  return true;
}
<commit_msg>Add logic for control mode list<commit_after>#include "controller_ros.h"

#include "uranus_dp/eigen_helper.h"
#include <tf/transform_datatypes.h>
#include <eigen_conversions/eigen_msg.h>

Controller::Controller(ros::NodeHandle nh) : nh(nh)
{
  command_sub = nh.subscribe("joystick_motion_command", 10, &Controller::commandCallback, this);
  state_sub   = nh.subscribe("state_estimate", 10, &Controller::stateCallback, this);
  wrench_pub  = nh.advertise<geometry_msgs::Wrench>("rov_forces", 10);

  control_mode = ControlModes::OPEN_LOOP;
  prev_time_valid = false;

  position_state.setZero();
  orientation_state.setIdentity();
  velocity_state.setZero();
  position_setpoint.setZero();
  orientation_setpoint.setIdentity();
  wrench_setpoint.setZero();

  getParams();

  // Read controller gains from parameter server
  std::map<std::string,double> gains;
  if (!nh.getParam("/controller/gains", gains))
    ROS_ERROR("Failed to read parameter controller gains.");

  // Read center of gravity and buoyancy vectors
  std::vector<double> r_G_vec, r_B_vec;
  if (!nh.getParam("/physical/center_of_mass", r_G_vec))
    ROS_ERROR("Failed to read robot center of mass parameter.");
  if (!nh.getParam("/physical/center_of_buoyancy", r_B_vec))
    ROS_ERROR("Failed to read robot center of buoyancy parameter.");
  Eigen::Vector3d r_G(r_G_vec.data());
  Eigen::Vector3d r_B(r_B_vec.data());

  // Read and calculate ROV weight and buoyancy
  double mass, displacement, acceleration_of_gravity, density_of_water;
  if (!nh.getParam("/physical/mass_kg", mass))
    ROS_ERROR("Failed to read parameter mass.");
  if (!nh.getParam("/physical/displacement_m3", displacement))
    ROS_ERROR("Failed to read parameter displacement.");
  if (!nh.getParam("/gravity/acceleration", acceleration_of_gravity))
    ROS_ERROR("Failed to read parameter acceleration of gravity");
  if (!nh.getParam("/water/density", density_of_water))
    ROS_ERROR("Failed to read parameter density of water");
  double W = mass * acceleration_of_gravity;
  double B = density_of_water * displacement * acceleration_of_gravity;

  position_hold_controller = new QuaternionPdController(gains["a"], gains["b"], gains["c"], W, B, r_G, r_B);

  // Set up a dynamic reconfigure server
  dynamic_reconfigure::Server<uranus_dp::ControllerConfig>::CallbackType cb;
  cb = boost::bind(&Controller::configCallback, this, _1, _2);
  dr_srv.setCallback(cb);

  ROS_INFO("Controller: Initialized with dynamic reconfigure.");
}

void Controller::commandCallback(const vortex_msgs::JoystickMotionCommand& msg)
{
  if (!healthyMessage(msg))
  {
    ROS_WARN("Controller: Propulsion command message out of range, ignoring...");
    return;
  }

  ControlMode new_control_mode;
  {
    int i;
    for (i = 0; i < msg.control_mode.size(); ++i)
      if (msg.control_mode[i])
        break;
    new_control_mode = static_cast<ControlMode>(i);
  }

  if (new_control_mode != control_mode)
  {
    control_mode = new_control_mode;
    switch (control_mode)
    {
      case ControlModes::OPEN_LOOP:
      ROS_INFO("Controller: Changing mode to OPEN LOOP.");
      break;

      case ControlModes::POSITION_HOLD:
      ROS_INFO("Controller: Changing mode to POSITION HOLD.");
      position_setpoint    = position_state;
      orientation_setpoint = orientation_state;
      prev_time_valid = false;
      break;
    }
  }

  updateSetpoints(msg);
}

void Controller::stateCallback(const nav_msgs::Odometry &msg)
{
  tf::pointMsgToEigen(msg.pose.pose.position, position_state);
  tf::quaternionMsgToEigen(msg.pose.pose.orientation, orientation_state);
}

void Controller::configCallback(uranus_dp::ControllerConfig &config, uint32_t level)
{
  ROS_INFO_STREAM("Setting quat pd gains:   a = " << config.a << ",   b = " << config.b << ",   c = " << config.c);
  position_hold_controller->setGains(config.a, config.b, config.c);
}

void Controller::spin()
{
  ros::Rate rate(frequency);
  while (ros::ok())
  {
    Eigen::Vector6d tau;
    switch (control_mode)
    {
      case ControlModes::POSITION_HOLD:
      tau = position_hold_controller->compute(position_state,
                                              orientation_state,
                                              velocity_state,
                                              position_setpoint,
                                              orientation_setpoint);
      break;

      case ControlModes::OPEN_LOOP:
      tau = wrench_setpoint;
      break;
    }

    geometry_msgs::Wrench msg;
    tf::wrenchEigenToMsg(tau, msg);
    wrench_pub.publish(msg);

    ros::spinOnce();
    rate.sleep();
  }
}

void Controller::updateSetpoints(const vortex_msgs::JoystickMotionCommand& msg)
{
  // Update wrench setpoints
  for (int i = 0; i < 6; ++i) // TODO: Fix magic number
    wrench_setpoint(i) = wrench_command_scaling[i] * wrench_command_max[i] * msg.motion[i];

  // Update pose setpoints
  if (!prev_time_valid)
  {
    prev_time = msg.header.stamp;
    prev_time_valid = true;
    return;
  }

  // Calculate time difference
  ros::Time curr_time = msg.header.stamp;
  double dt = (curr_time - prev_time).toSec();
  prev_time = curr_time;

  if (dt == 0)
  {
    // ROS_WARN("Zero time difference between propulsion command messages.");
    return;
  }

  // Increment position setpoints
  for (int i = 0; i < 3; ++i) // TODO: Fix magic number
    position_setpoint(i) += pose_command_rate[i] * dt * msg.motion[i];

  // Calc euler setpoints
  Eigen::Vector3d orientation_setpoint_euler;
  orientation_setpoint_euler = orientation_setpoint.toRotationMatrix().eulerAngles(0,1,2);
  // Increment euler setpoints
  for (int i = 0; i < 3; ++i) // TODO: Fix magic number
    orientation_setpoint_euler(i) += pose_command_rate[3+i] * dt * msg.motion[3+i];
  // Calc incremented quat setpoints
  Eigen::Matrix3d R;
  R = Eigen::AngleAxisd(orientation_setpoint_euler(0), Eigen::Vector3d::UnitX())
    * Eigen::AngleAxisd(orientation_setpoint_euler(1), Eigen::Vector3d::UnitY())
    * Eigen::AngleAxisd(orientation_setpoint_euler(2), Eigen::Vector3d::UnitZ());
  Eigen::Quaterniond q(R);
  orientation_setpoint = q;
}

void Controller::getParams()
{
  // Read control command parameters
  if (!nh.getParam("/propulsion/command/wrench/max", wrench_command_max))
    ROS_FATAL("Failed to read parameter max wrench command.");
  if (!nh.getParam("/propulsion/command/wrench/scaling", wrench_command_scaling))
    ROS_FATAL("Failed to read parameter scaling wrench command.");
  if (!nh.getParam("/propulsion/command/pose/rate", pose_command_rate))
    ROS_FATAL("Failed to read parameter pose command rate.");

  if (!nh.getParam("/controller/frequency", frequency))
  {
    ROS_WARN("Failed to read parameter controller frequency, defaulting to 10 Hz.");
    frequency = 10;
  }
}

bool Controller::healthyMessage(const vortex_msgs::JoystickMotionCommand& msg)
{
  // Check that motion commands are in range
  for (int i = 0; i < msg.motion.size(); ++i)
  {
    if (msg.motion[i] > 1 || msg.motion[i] < -1)
    {
      ROS_WARN("Controller: Motion command out of range.");
      return false;
    }
  }

  // Check that exactly one control mode is requested
  int num_requested_modes = 0;
  for (int i = 0; i < msg.control_mode.size(); ++i)
    if (msg.control_mode[i])
      num_requested_modes++;
  if (num_requested_modes != 1)
  {
    ROS_WARN("controller: Invalid control mode.");
    return false;
  }

  return true;
}
<|endoftext|>