+#include "gdbserverthread.h"
+
+#include <iostream>
+
+GdbServerThread::GdbServerThread(qintptr socketDescriptor, QObject* parent, DebugInterface* debugInterface)
+ : QThread(parent),
+ mSocketDescriptor(socketDescriptor),
+ mDebugInterface(debugInterface) {}
+
+void GdbServerThread::run() {
+ QTcpSocket socket;
+
+ if (!socket.setSocketDescriptor(mSocketDescriptor)) {
+ emit error(socket.error());
+ return;
+ }
+
+ QByteArray response;
+ while (true) {
+ socket.waitForReadyRead();
+ bool receivedFullMessage = processData(socket.readAll(), response);
+ if (receivedFullMessage) {
+ sendAck(socket);
+ sendResponse(socket, response);
+ }
+ }
+}
+
+bool GdbServerThread::processData(QByteArray data, QByteArray& response) {
+ while (!data.isEmpty()) {
+ const char c = data.front();
+
+ switch (c) {
+ case '+':
+ if (mData.isEmpty()) {
+ mData.clear();
+ } else {
+ mData += c;
+ }
+ break;
+ case '-':
+ if (mData.isEmpty()) {
+ printf("Received nack packet!\n");
+ } else {
+ mData += c;
+ }
+ break;
+ case '$':
+ mData.clear();
+ mData += c;
+ break;
+ case '#':
+ mData += c;
+ // read checksum
+ mData += data[1];
+ mData += data[2];
+ response = handleMessage(mData);
+ return true;
+ default:
+ mData += c;
+ break;
+ }
+ data.remove(0, 1);
+ }
+
+ return false;
+}
+
+GdbServerThread::GdbMessage GdbServerThread::parseMessage(const QByteArray& message) {
+ GdbMessage parsedMessage;
+
+ // GDB commands are in the format (cs meaning two checksum digits):
+ // $<command>[addr,len]:[payload]#cs
+ //
+ // So e.g. $M0,1:FF#cs means
+ // Command = M (write memory at location)
+ // Arg1 (address) = 0x0
+ // Arg2 (length) = 0x1 bytes
+ // Payload = 0xFF
+
+ std::string command;
+ std::vector<std::string> arguments;
+ std::string payload;
+
+ GdbMessageSegment currentSegment = GdbMessageSegment::Unknown;
+
+ for (char c : message) {
+ switch (c) {
+ case '$':
+ currentSegment = GdbMessageSegment::Command;
+ break;
+ case ',':
+ if (currentSegment == GdbMessageSegment::Arguments) {
+ arguments.emplace_back();
+ }
+ break;
+ case ':':
+ currentSegment = GdbMessageSegment::Payload;
+ break;
+ case '#':
+ currentSegment = GdbMessageSegment::End;
+ break;
+ default:
+ switch (currentSegment) {
+ case GdbMessageSegment::Command:
+ if (c >= 0x30 && c < 0x3A) {
+ currentSegment = GdbMessageSegment::Arguments;
+ arguments.emplace_back(1, c);
+ } else {
+ command += c;
+ }
+ break;
+ case GdbMessageSegment::Arguments:
+ arguments.back() += c;
+ break;
+ case GdbMessageSegment::Payload:
+ payload += c;
+ break;
+ case GdbMessageSegment::End:
+ // Do nothing
+ break;
+ default:
+ // Should not reach here
+ printf("Error processing message\n");
+ }
+ break;
+ }
+ }
+
+ if (command == "qSupported") {
+ parsedMessage.type = GdbMessageType::QuerySupported;
+ } else if (command == "?") {
+ parsedMessage.type = GdbMessageType::QueryHaltReason;
+ } else if (command == "qAttached") {
+ parsedMessage.type = GdbMessageType::QueryAttached;
+ } else if (command == "g") {
+ parsedMessage.type = GdbMessageType::QueryGeneralRegisters;
+ } else if (command == "p") {
+ parsedMessage.type = GdbMessageType::QueryRegister;
+ } else if (command == "m") {
+ parsedMessage.type = GdbMessageType::QueryMemory;
+ } else {
+ parsedMessage.type = GdbMessageType::Unknown;
+ return parsedMessage;
+ }
+
+ for (auto arg : arguments) {
+ parsedMessage.arguments.push_back(std::stoi(arg, nullptr, 16));
+ }
+
+ return parsedMessage;
+}
+
+QByteArray GdbServerThread::handleMessage(const QByteArray& message) {
+ std::cout << "Received: " << message.toStdString() << std::endl;
+
+ GdbMessage parsedMessage = parseMessage(message);
+
+ switch (parsedMessage.type) {
+ case GdbMessageType::QuerySupported:
+ return "hwbreak+";
+ case GdbMessageType::QueryHaltReason:
+ return "S05";
+ case GdbMessageType::QueryAttached:
+ return "1";
+ case GdbMessageType::QueryGeneralRegisters: {
+ QByteArray response;
+ for (int i = 0; i < 16; ++i) {
+ response += dataToString(mDebugInterface->readRegister(i));
+ }
+ return response;
+ }
+ case GdbMessageType::QueryRegister:
+ return dataToString(mDebugInterface->readRegister(parsedMessage.arguments.front()));
+ case GdbMessageType::QueryMemory: {
+ uint32_t address = parsedMessage.arguments[0];
+ uint32_t length = parsedMessage.arguments[1];
+ QByteArray response;
+ while (length) {
+ if (length >= 4) {
+ response += dataToString(mDebugInterface->readMemory32(address));
+ length -= 4;
+ address += 4;
+ } else {
+ response += dataToString(mDebugInterface->readMemory8(address));
+ length--;
+ address++;
+ }
+ }
+ return response;
+ }
+ }
+
+ return {};
+}
+
+unsigned int GdbServerThread::generateChecksum(const QByteArray& message) {
+ unsigned int sum = 0;
+ for (const char& c : message) {
+ sum += c;
+ }
+ sum &= 0xff;
+ return sum;
+}
+
+void GdbServerThread::sendAck(QTcpSocket& socket) {
+ socket.write("+");
+}
+
+void GdbServerThread::sendResponse(QTcpSocket& socket, QByteArray& response) {
+ QString checksum = QString("#%1").arg(generateChecksum(response), 2, 16, QChar('0'));
+ response.prepend('$');
+ response.append(checksum.toUtf8());
+ std::cout << "Responding: " << response.toStdString() << std::endl;
+ socket.write(response);
+ socket.flush();
+}
+
+QByteArray GdbServerThread::dataToString(uint32_t value) {
+ QByteArray output;
+ output.resize(8);
+ std::sprintf(output.data(),
+ "%02x%02x%02x%02x",
+ value & 0xFF,
+ value >> 8 & 0xFF,
+ value >> 16 & 0xFF,
+ value >> 24 & 0xFF);
+ return output;
+}
+
+QByteArray GdbServerThread::dataToString(uint8_t value) {
+ QByteArray output;
+ output.resize(2);
+ std::sprintf(output.data(),
+ "%02x",
+ value & 0xFF);
+ return output;
+}
projects / WindEmu.git / commitdiff