#include "msg.h" #include "../utils/utils.h" #include #include #include #include namespace Aseba { //! Static class that fills a table of known messages types in its constructor class MessageTypesInitializer { public: //! Constructor, register all known messages types MessageTypesInitializer() { registerMessageType(ASEBA_MESSAGE_PRESENCE); registerMessageType(ASEBA_MESSAGE_DESCRIPTION); registerMessageType(ASEBA_MESSAGE_VARIABLES); registerMessageType(ASEBA_MESSAGE_EXECUTION_STATE); registerMessageType(ASEBA_MESSAGE_ARRAY_ACCESS_OUT_OF_BOUND); registerMessageType(ASEBA_MESSAGE_DIVISION_BY_ZERO); registerMessageType(ASEBA_MESSAGE_ATTACH_DEBUGGER); registerMessageType(ASEBA_MESSAGE_DETACH_DEBUGGER); registerMessageType(ASEBA_MESSAGE_SET_BYTECODE); registerMessageType(ASEBA_MESSAGE_SETUP_EVENT); registerMessageType(ASEBA_MESSAGE_BACKGROUND_RUN); registerMessageType(ASEBA_MESSAGE_STOP); registerMessageType(ASEBA_MESSAGE_STEP); registerMessageType(ASEBA_MESSAGE_BREAKPOINT_SET); registerMessageType(ASEBA_MESSAGE_BREAKPOINT_CLEAR); registerMessageType(ASEBA_MESSAGE_BREAKPOINT_CLEAR_ALL); registerMessageType(ASEBA_MESSAGE_GET_VARIABLES); } //! Register a message type by storing a pointer to its constructor template void registerMessageType(uint16 type) { messagesTypes[type] = &Creator_{;
}

//! Create an instance of a registered message type
Message *createMessage(uint16 type)
{
if (messagesTypes.find(type) == messagesTypes.end())
{
std::cerr << "Message of type " << std::hex << std::showbase << std::setw(4) << type << " is unknown, aborting.\n\nKnown messages are:\n";
dumpKnownMessagesTypes(std::cerr);
std::cerr << std::endl;
assert(false);
}
return messagesTypes[type]();
}

//! Print the list of registered messages types to stream
void dumpKnownMessagesTypes(std::ostream &stream)
{
for (std::map::iterator it = messagesTypes.begin(); it != messagesTypes.end(); ++it)
stream << "\t" << std::hex << std::showbase << std::setw(4) << it->first << "\n";
}

protected:
//! Pointer to constructor of class Message
typedef Message* (*CreatorFunc)();
std::map messagesTypes; //!< table of known messages types

//! Create a new message of type Sub
template
static Message* Creator()
{
return new Sub();
}
} messageTypesInitializer; //!< static instance, used only to have its constructor called on startup

//

Message::Message()
{
readPos = 0;
}

Message::~Message()
{

}

void Message::serialize(int fd)
{
data.resize(0);
serializeSpecific();
assert(data.size() + 2 < 65536);
uint16 len = static_cast(data.size());
write(fd, &len, 2);
write(fd, &type, 2);
write(fd, &data[0], data.size());
}

Message *Message::receive(int fd)
{
// read header
uint16 len, type;
read(fd, &len, 2);
read(fd, &type, 2);

// create message
Message *message = messageTypesInitializer.createMessage(type);

// preapare message
message->type = type;
message->data.resize(len);
read(fd, &message->data[0], len);
message->readPos = 0;

// deserialize it
message->deserializeSpecific();

assert(message->readPos == message->data.size());

return message;
}

void Message::dump(std::ostream &stream)
{
stream << std::hex << std::showbase << std::setw(4) << type << " " << *this << " : ";
dumpSpecific(stream);
}

template
void Message::add(const T& val)
{
size_t pos = data.size();
data.resize(pos + sizeof(T));
const uint8 *ptr = reinterpret_cast(&val);
std::copy(ptr, ptr + sizeof(T), &data[pos]);
}

template<>
void Message::add(const std::string& val)
{
assert(val.length() <= 255);
add(static_cast(val.length()));
for (size_t i = 0; i < val.length(); i++)
add(val[i]);
}

template
T Message::get()
{
assert(readPos + sizeof(T) <= data.size());
size_t pos = readPos;
readPos += sizeof(T);
T val;
uint8 *ptr = reinterpret_cast(&val);
std::copy(&data[pos], &data[pos + sizeof(T)], ptr);
return val;
}

template<>
std::string Message::get()
{
std::string s;
size_t len = get();
s.resize(len);
for (size_t i = 0; i < len; i++)
s[i] = get();
return s;
}

//

void UserMessage::dumpSpecific(std::ostream &stream)
{
stream << "user message of size " << data.size();
}

//

void BootloaderDataRead::serializeSpecific()
{
for (size_t i = 0 ; i < sizeof(data); i++)
add(data[i]);
}

void BootloaderDataRead::deserializeSpecific()
{
for (size_t i = 0 ; i < sizeof(data); i++)
data[i] = get();
}

void BootloaderDataRead::dumpSpecific(std::ostream &stream)
{
for (size_t i = 0 ; i < sizeof(data); i++)
stream << std::hex << (unsigned)data[i] << " ";
}

//

void BootloaderAck::serializeSpecific()
{
add(errorCode);
if (errorCode == 2)
add(errorAddress);
}

void BootloaderAck::deserializeSpecific()
{
errorCode = get();
if (errorCode == 2)
errorAddress = get();
}

void BootloaderAck::dumpSpecific(std::ostream &stream)
{
switch (errorCode)
{
case 0: stream << "success"; break;
case 1: stream << "error, invalid frame size"; break;
case 2: stream << "error, programming failed at low address " << std::hex << errorAddress; break;
case 3: stream << "error, not programming"; break;
default: stream << "unknown error"; break;
}
}

//

void Description::serializeSpecific()
{
add(bytecodeSize);
add(stackSize);
add(variablesSize);
add(nodeName);

assert(variablesNames.size() == variablesSizes.size());
add(static_cast(variablesNames.size()));
for (size_t i = 0; i < variablesNames.size(); i++)
{
add(variablesNames[i]);
add(variablesSizes[i]);
}
}

void Description::deserializeSpecific()
{
bytecodeSize = get();
stackSize = get();
variablesSize = get();
nodeName = get();

uint16 namedVariablesCount = get();
variablesNames.resize(namedVariablesCount);
variablesSizes.resize(namedVariablesCount);
for (size_t i = 0; i < variablesNames.size(); i++)
{
variablesNames[i] = get();
variablesSizes[i] = get();
}
}

void Description::dumpSpecific(std::ostream &stream)
{
stream << "Node " << nodeName << "\n";
stream << "bytecode " << bytecodeSize << ", stack " << stackSize << ", variables " << variablesSize;
for (size_t i = 0; i < variablesNames.size(); i++)
stream << "\n\t" << variablesNames[i] << " : " << variablesSizes[i];
}

//

void Variables::serializeSpecific()
{
add(start);
for (size_t i = 0; i < variables.size(); i++)
add(variables[i]);
}

void Variables::deserializeSpecific()
{
start = get();
variables.resize((data.size() - readPos) / 2);
for (size_t i = 0; i < variables.size(); i++)
variables[i] = get();
}

void Variables::dumpSpecific(std::ostream &stream)
{
stream << "start " << start << ", variables vector of size " << variables.size();
}

//

void ExecutionState::serializeSpecific()
{
add(pc);
add(flags);
}

void ExecutionState::deserializeSpecific()
{
pc = get();
flags = get();
}

void ExecutionState::dumpSpecific(std::ostream &stream)
{
stream << "pc " << pc << ", flags " << flags;
}

//

void ArrayAccessOutOfBounds::serializeSpecific()
{
add(pc);
add(index);
}

void ArrayAccessOutOfBounds::deserializeSpecific()
{
pc = get();
index = get();
}

void ArrayAccessOutOfBounds::dumpSpecific(std::ostream &stream)
{
stream << "pc " << pc << ", index " << index;
}

//

void DivisionByZero::serializeSpecific()
{
add(pc);
}

void DivisionByZero::deserializeSpecific()
{
pc = get();
}

void DivisionByZero::dumpSpecific(std::ostream &stream)
{
stream << "pc " << pc;
}

//

void CmdMessage::serializeSpecific()
{
add(dest);
}

void CmdMessage::deserializeSpecific()
{
dest = get();
}

void CmdMessage::dumpSpecific(std::ostream &stream)
{
stream << "debug message dest " << std::hex << (unsigned)dest;
}

//

void BootloaderReadPage::serializeSpecific()
{
add(pageNumber);
}

void BootloaderReadPage::deserializeSpecific()
{
pageNumber = get();
}

void BootloaderReadPage::dumpSpecific(std::ostream &stream)
{
stream << "page " << std::hex << pageNumber;
}

//

void BootloaderWritePage::serializeSpecific()
{
add(pageNumber);
}

void BootloaderWritePage::deserializeSpecific()
{
pageNumber = get();
}

void BootloaderWritePage::dumpSpecific(std::ostream &stream)
{
stream << "page " << std::hex << pageNumber;
}

//

void BootloaderPageDataWrite::serializeSpecific()
{
for (size_t i = 0 ; i < sizeof(data); i++)
add(data[i]);
}

void BootloaderPageDataWrite::deserializeSpecific()
{
for (size_t i = 0 ; i < sizeof(data); i++)
data[i] = get();
}

void BootloaderPageDataWrite::dumpSpecific(std::ostream &stream)
{
for (size_t i = 0 ; i < sizeof(data); i++)
stream << std::hex << (unsigned)data[i] << " ";
}

//

void SetBytecode::serializeSpecific()
{
CmdMessage::serializeSpecific();

for (size_t i = 0; i < bytecode.size(); i++)
add(bytecode[i]);
}

void SetBytecode::deserializeSpecific()
{
CmdMessage::deserializeSpecific();

bytecode.resize((data.size() - readPos) / 2);
for (size_t i = 0; i < bytecode.size(); i++)
bytecode[i] = get();
}

void SetBytecode::dumpSpecific(std::ostream &stream)
{
CmdMessage::dumpSpecific(stream);

stream << "bytecode of size " << bytecode.size();
}

//

void SetupEvent::serializeSpecific()
{
CmdMessage::serializeSpecific();

add(eventId);
for (size_t i = 0; i < arguments.size(); i++)
add(arguments[i]);
}

void SetupEvent::deserializeSpecific()
{
CmdMessage::deserializeSpecific();

eventId = get();
arguments.resize((data.size() - readPos) / 2);
for (size_t i = 0; i < arguments.size(); i++)
arguments[i] = get();
}

void SetupEvent::dumpSpecific(std::ostream &stream)
{
CmdMessage::dumpSpecific(stream);

stream << "event id " << eventId << ", arguments vector of size " << arguments.size();
}

//

void BreakpointSet::serializeSpecific()
{
CmdMessage::serializeSpecific();

add(pc);
}

void BreakpointSet::deserializeSpecific()
{
CmdMessage::deserializeSpecific();

pc = get();
}

void BreakpointSet::dumpSpecific(std::ostream &stream)
{
CmdMessage::dumpSpecific(stream);

stream << "pc " << pc;
}

//

void BreakpointClear::serializeSpecific()
{
CmdMessage::serializeSpecific();

add(pc);
}

void BreakpointClear::deserializeSpecific()
{
CmdMessage::deserializeSpecific();

pc = get();
}

void BreakpointClear::dumpSpecific(std::ostream &stream)
{
CmdMessage::dumpSpecific(stream);

stream << "pc " << pc;
}
}}