From 96f185f30ddf1c87af791b5a4bc67fad40b77e33 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Niklas=20Ekstr=C3=B6m?= Date: Sat, 28 Nov 2020 16:29:40 +0100 Subject: [PATCH] Add A314 emulation --- .gitignore | 1 + Makefile | 9 +- a314/a314.cc | 1405 ++++++++++++++++++++++++++++++ a314/a314.h | 31 + a314/a314device/a314.h | 43 + a314/a314device/a314driver.c | 623 +++++++++++++ a314/a314device/build.bat | 1 + a314/a314device/debug.h | 3 + a314/a314device/device.c | 121 +++ a314/a314device/device.h | 2 + a314/a314device/fix_mem_region.c | 131 +++ a314/a314device/fix_mem_region.h | 4 + a314/a314device/int_server.asm | 25 + a314/a314device/proto_a314.h | 9 + a314/a314device/protocol.h | 44 + a314/a314device/romtag.asm | 23 + a314/a314device/sockets.c | 115 +++ a314/a314device/sockets.h | 57 ++ a314/a314device/startup.c | 108 +++ a314/a314device/startup.h | 14 + emulator.c | 53 ++ 21 files changed, 2819 insertions(+), 3 deletions(-) create mode 100644 .gitignore create mode 100644 a314/a314.cc create mode 100644 a314/a314.h create mode 100644 a314/a314device/a314.h create mode 100644 a314/a314device/a314driver.c create mode 100644 a314/a314device/build.bat create mode 100644 a314/a314device/debug.h create mode 100644 a314/a314device/device.c create mode 100644 a314/a314device/device.h create mode 100644 a314/a314device/fix_mem_region.c create mode 100644 a314/a314device/fix_mem_region.h create mode 100644 a314/a314device/int_server.asm create mode 100644 a314/a314device/proto_a314.h create mode 100644 a314/a314device/protocol.h create mode 100644 a314/a314device/romtag.asm create mode 100644 a314/a314device/sockets.c create mode 100644 a314/a314device/sockets.h create mode 100644 a314/a314device/startup.c create mode 100644 a314/a314device/startup.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..722d5e7 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode diff --git a/Makefile b/Makefile index 1f8e2a2..270e6ed 100644 --- a/Makefile +++ b/Makefile @@ -12,9 +12,10 @@ EXE = EXEPATH = ./ .CFILES = $(MAINFILES) $(MUSASHIFILES) $(MUSASHIGENCFILES) -.OFILES = $(.CFILES:%.c=%.o) +.OFILES = $(.CFILES:%.c=%.o) a314.o CC = gcc +CPP = g++ WARNINGS = -Wall -Wextra -pedantic CFLAGS = $(WARNINGS) -march=armv7 -O3 LFLAGS = $(WARNINGS) @@ -29,12 +30,14 @@ all: $(TARGET) clean: rm -f $(DELETEFILES) - $(TARGET): $(MUSASHIGENHFILES) $(.OFILES) Makefile - $(CC) -o $@ $(.OFILES) -O3 -pthread $(LFLAGS) -lm + $(CPP) -o $@ $(.OFILES) -O3 -pthread $(LFLAGS) -lm $(MUSASHIGENCFILES) $(MUSASHIGENHFILES): $(MUSASHIGENERATOR)$(EXE) $(EXEPATH)$(MUSASHIGENERATOR)$(EXE) $(MUSASHIGENERATOR)$(EXE): $(MUSASHIGENERATOR).c $(CC) -o $(MUSASHIGENERATOR)$(EXE) $(MUSASHIGENERATOR).c + +a314.o: a314/a314.cc a314/a314.h m68k.h + $(CPP) a314/a314.cc -O3 -c diff --git a/a314/a314.cc b/a314/a314.cc new file mode 100644 index 0000000..4d5dfe6 --- /dev/null +++ b/a314/a314.cc @@ -0,0 +1,1405 @@ +/* + * Copyright 2020 Niklas Ekström + * Based on a314d daemon for A314. + */ + +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "a314.h" +#include "../m68k.h" + +#define LOGGER_TRACE 1 +#define LOGGER_DEBUG 2 +#define LOGGER_INFO 3 +#define LOGGER_WARN 4 +#define LOGGER_ERROR 5 + +#define LOGGER_SHOW LOGGER_INFO + +#define logger_trace(...) do { if (LOGGER_TRACE >= LOGGER_SHOW) fprintf(stdout, __VA_ARGS__); } while (0) +#define logger_debug(...) do { if (LOGGER_DEBUG >= LOGGER_SHOW) fprintf(stdout, __VA_ARGS__); } while (0) +#define logger_info(...) do { if (LOGGER_INFO >= LOGGER_SHOW) fprintf(stdout, __VA_ARGS__); } while (0) +#define logger_warn(...) do { if (LOGGER_WARN >= LOGGER_SHOW) fprintf(stdout, __VA_ARGS__); } while (0) +#define logger_error(...) do { if (LOGGER_ERROR >= LOGGER_SHOW) fprintf(stderr, __VA_ARGS__); } while (0) + +// Events that are communicated via IRQ from Amiga to Raspberry. +#define R_EVENT_A2R_TAIL 1 +#define R_EVENT_R2A_HEAD 2 +#define R_EVENT_STARTED 4 + +// Events that are communicated from Raspberry to Amiga. +#define A_EVENT_R2A_TAIL 1 +#define A_EVENT_A2R_HEAD 2 + +// Offset relative to communication area for queue pointers. +#define A2R_TAIL_OFFSET 0 +#define R2A_HEAD_OFFSET 1 +#define R2A_TAIL_OFFSET 2 +#define A2R_HEAD_OFFSET 3 + +// Packets that are communicated across physical channels (A2R and R2A). +#define PKT_CONNECT 4 +#define PKT_CONNECT_RESPONSE 5 +#define PKT_DATA 6 +#define PKT_EOS 7 +#define PKT_RESET 8 + +// Valid responses for PKT_CONNECT_RESPONSE. +#define CONNECT_OK 0 +#define CONNECT_UNKNOWN_SERVICE 3 + +// Messages that are communicated between driver and client. +#define MSG_REGISTER_REQ 1 +#define MSG_REGISTER_RES 2 +#define MSG_DEREGISTER_REQ 3 +#define MSG_DEREGISTER_RES 4 +#define MSG_READ_MEM_REQ 5 +#define MSG_READ_MEM_RES 6 +#define MSG_WRITE_MEM_REQ 7 +#define MSG_WRITE_MEM_RES 8 +#define MSG_CONNECT 9 +#define MSG_CONNECT_RESPONSE 10 +#define MSG_DATA 11 +#define MSG_EOS 12 +#define MSG_RESET 13 + +#define MSG_SUCCESS 1 +#define MSG_FAIL 0 + +static sigset_t original_sigset; + +static pthread_t thread_id; +static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; + +static int server_socket = -1; + +static int epfd = -1; +static int irq_fds[2]; + +extern "C" unsigned char fast_ram_array[]; +extern "C" void write16(unsigned int address, unsigned int value); + +// Register bank in 0xE90000 memory. +struct ComArea +{ + uint8_t a_events; + uint8_t a_enable; + uint8_t r_events; + uint8_t r_enable; // Unused. + + uint8_t a2r_tail; + uint8_t r2a_head; + uint8_t r2a_tail; + uint8_t a2r_head; + + uint8_t a2r_buffer[256]; + uint8_t r2a_buffer[256]; +}; + +static ComArea ca; + +static bool a314_device_started = false; + +static uint8_t channel_status[4]; +static uint8_t channel_status_updated = 0; + +static uint8_t recv_buf[256]; +static uint8_t send_buf[256]; + +struct LogicalChannel; +struct ClientConnection; + +#pragma pack(push, 1) +struct MessageHeader +{ + uint32_t length; + uint32_t stream_id; + uint8_t type; +}; //} __attribute__((packed)); +#pragma pack(pop) + +struct MessageBuffer +{ + int pos; + std::vector data; +}; + +struct RegisteredService +{ + std::string name; + ClientConnection *cc; +}; + +struct PacketBuffer +{ + int type; + std::vector data; +}; + +struct ClientConnection +{ + int fd; + + int next_stream_id; + + int bytes_read; + MessageHeader header; + std::vector payload; + + std::list message_queue; + + std::list associations; +}; + +struct LogicalChannel +{ + int channel_id; + + ClientConnection *association; + int stream_id; + + bool got_eos_from_ami; + bool got_eos_from_client; + + std::list packet_queue; +}; + +static void remove_association(LogicalChannel *ch); +static void clear_packet_queue(LogicalChannel *ch); +static void create_and_enqueue_packet(LogicalChannel *ch, uint8_t type, uint8_t *data, uint8_t length); + +static std::list connections; +static std::list services; +static std::list channels; +static std::list send_queue; + +struct OnDemandStart +{ + std::string service_name; + std::string program; + std::vector arguments; +}; + +std::vector on_demand_services; + +static void load_config_file(const char *filename) +{ + FILE *f = fopen(filename, "rt"); + if (f == nullptr) + return; + + char line[256]; + std::vector parts; + + while (fgets(line, 256, f) != nullptr) + { + char org_line[256]; + strcpy(org_line, line); + + bool in_quotes = false; + + int start = 0; + for (int i = 0; i < 256; i++) + { + if (line[i] == 0) + { + if (start < i) + parts.push_back(&line[start]); + break; + } + else if (line[i] == '"') + { + line[i] = 0; + if (in_quotes) + parts.push_back(&line[start]); + in_quotes = !in_quotes; + start = i + 1; + } + else if (isspace(line[i]) && !in_quotes) + { + line[i] = 0; + if (start < i) + parts.push_back(&line[start]); + start = i + 1; + } + } + + if (parts.size() >= 2) + { + on_demand_services.emplace_back(); + auto &e = on_demand_services.back(); + e.service_name = parts[0]; + e.program = parts[1]; + for (int i = 1; i < parts.size(); i++) + e.arguments.push_back(std::string(parts[i])); + } + else if (parts.size() != 0) + logger_warn("Invalid number of columns in configuration file line: %s\n", org_line); + + parts.clear(); + } + + fclose(f); + + if (on_demand_services.empty()) + logger_warn("No registered services\n"); +} + +static int init_server_socket() +{ + server_socket = socket(AF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0); + if (server_socket == -1) + { + logger_error("Failed to create server socket\n"); + return -1; + } + + struct sockaddr_in address; + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(7110); + + int res = bind(server_socket, (struct sockaddr *)&address, sizeof(address)); + if (res < 0) + { + logger_error("Bind to localhost:7110 failed\n"); + return -1; + } + + listen(server_socket, 16); + + return 0; +} + +static void shutdown_server_socket() +{ + if (server_socket != -1) + close(server_socket); + server_socket = -1; +} + +void create_and_send_msg(ClientConnection *cc, int type, int stream_id, uint8_t *data, int length) +{ + MessageBuffer mb; + mb.pos = 0; + mb.data.resize(sizeof(MessageHeader) + length); + + MessageHeader *mh = (MessageHeader *)&mb.data[0]; + mh->length = length; + mh->stream_id = stream_id; + mh->type = type; + if (length && data) + memcpy(&mb.data[sizeof(MessageHeader)], data, length); + + if (!cc->message_queue.empty()) + { + cc->message_queue.push_back(std::move(mb)); + return; + } + + while (1) + { + int left = mb.data.size() - mb.pos; + uint8_t *src = &mb.data[mb.pos]; + ssize_t r = write(cc->fd, src, left); + if (r == -1) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + { + cc->message_queue.push_back(std::move(mb)); + return; + } + else if (errno == ECONNRESET) + { + // Do not close connection here; it will get done at some other place. + return; + } + else + { + logger_error("Write failed unexpectedly with errno = %d\n", errno); + exit(-1); + } + } + + mb.pos += r; + if (r == left) + { + return; + } + } +} + +static void handle_msg_register_req(ClientConnection *cc) +{ + uint8_t result = MSG_FAIL; + + std::string service_name((char *)&cc->payload[0], cc->payload.size()); + + auto it = services.begin(); + for (; it != services.end(); it++) + if (it->name == service_name) + break; + + if (it == services.end()) + { + services.emplace_back(); + + RegisteredService &srv = services.back(); + srv.cc = cc; + srv.name = std::move(service_name); + + result = MSG_SUCCESS; + } + + create_and_send_msg(cc, MSG_REGISTER_RES, 0, &result, 1); +} + +static void handle_msg_deregister_req(ClientConnection *cc) +{ + uint8_t result = MSG_FAIL; + + std::string service_name((char *)&cc->payload[0], cc->payload.size()); + + for (auto it = services.begin(); it != services.end(); it++) + { + if (it->name == service_name && it->cc == cc) + { + services.erase(it); + result = MSG_SUCCESS; + break; + } + } + + create_and_send_msg(cc, MSG_DEREGISTER_RES, 0, &result, 1); +} + +static void handle_msg_read_mem_req(ClientConnection *cc) +{ + uint32_t address = *(uint32_t *)&(cc->payload[0]); + uint32_t length = *(uint32_t *)&(cc->payload[4]); + + create_and_send_msg(cc, MSG_READ_MEM_RES, 0, &fast_ram_array[address], length); +} + +static void handle_msg_write_mem_req(ClientConnection *cc) +{ + uint32_t address = *(uint32_t *)&(cc->payload[0]); + uint32_t length = cc->payload.size() - 4; + + memcpy(&fast_ram_array[address], &(cc->payload[4]), length); + + create_and_send_msg(cc, MSG_WRITE_MEM_RES, 0, nullptr, 0); +} + +static LogicalChannel *get_associated_channel_by_stream_id(ClientConnection *cc, int stream_id) +{ + for (auto ch : cc->associations) + { + if (ch->stream_id == stream_id) + return ch; + } + return nullptr; +} + +static void handle_msg_connect(ClientConnection *cc) +{ + // We currently don't handle that a client tries to connect to a service on the Amiga. +} + +static void handle_msg_connect_response(ClientConnection *cc) +{ + LogicalChannel *ch = get_associated_channel_by_stream_id(cc, cc->header.stream_id); + if (!ch) + return; + + create_and_enqueue_packet(ch, PKT_CONNECT_RESPONSE, &cc->payload[0], cc->payload.size()); + + if (cc->payload[0] != CONNECT_OK) + remove_association(ch); +} + +static void handle_msg_data(ClientConnection *cc) +{ + LogicalChannel *ch = get_associated_channel_by_stream_id(cc, cc->header.stream_id); + if (!ch) + return; + + create_and_enqueue_packet(ch, PKT_DATA, &cc->payload[0], cc->header.length); +} + +static void handle_msg_eos(ClientConnection *cc) +{ + LogicalChannel *ch = get_associated_channel_by_stream_id(cc, cc->header.stream_id); + if (!ch || ch->got_eos_from_client) + return; + + ch->got_eos_from_client = true; + + create_and_enqueue_packet(ch, PKT_EOS, nullptr, 0); + + if (ch->got_eos_from_ami) + remove_association(ch); +} + +static void handle_msg_reset(ClientConnection *cc) +{ + LogicalChannel *ch = get_associated_channel_by_stream_id(cc, cc->header.stream_id); + if (!ch) + return; + + remove_association(ch); + + clear_packet_queue(ch); + create_and_enqueue_packet(ch, PKT_RESET, nullptr, 0); +} + +static void handle_received_message(ClientConnection *cc) +{ + switch (cc->header.type) + { + case MSG_REGISTER_REQ: + handle_msg_register_req(cc); + break; + case MSG_DEREGISTER_REQ: + handle_msg_deregister_req(cc); + break; + case MSG_READ_MEM_REQ: + handle_msg_read_mem_req(cc); + break; + case MSG_WRITE_MEM_REQ: + handle_msg_write_mem_req(cc); + break; + case MSG_CONNECT: + handle_msg_connect(cc); + break; + case MSG_CONNECT_RESPONSE: + handle_msg_connect_response(cc); + break; + case MSG_DATA: + handle_msg_data(cc); + break; + case MSG_EOS: + handle_msg_eos(cc); + break; + case MSG_RESET: + handle_msg_reset(cc); + break; + default: + // This is bad, probably should disconnect from client. + logger_warn("Received a message of unknown type from client\n"); + break; + } +} + +static void close_and_remove_connection(ClientConnection *cc) +{ + shutdown(cc->fd, SHUT_WR); + close(cc->fd); + + { + auto it = services.begin(); + while (it != services.end()) + { + if (it->cc == cc) + it = services.erase(it); + else + it++; + } + } + + { + auto it = cc->associations.begin(); + while (it != cc->associations.end()) + { + auto ch = *it; + + clear_packet_queue(ch); + create_and_enqueue_packet(ch, PKT_RESET, nullptr, 0); + + ch->association = nullptr; + ch->stream_id = 0; + + it = cc->associations.erase(it); + } + } + + for (auto it = connections.begin(); it != connections.end(); it++) + { + if (&(*it) == cc) + { + connections.erase(it); + break; + } + } +} + +static void remove_association(LogicalChannel *ch) +{ + auto &ass = ch->association->associations; + ass.erase(std::find(ass.begin(), ass.end(), ch)); + + ch->association = nullptr; + ch->stream_id = 0; +} + +static void clear_packet_queue(LogicalChannel *ch) +{ + if (!ch->packet_queue.empty()) + { + ch->packet_queue.clear(); + send_queue.erase(std::find(send_queue.begin(), send_queue.end(), ch)); + } +} + +static void create_and_enqueue_packet(LogicalChannel *ch, uint8_t type, uint8_t *data, uint8_t length) +{ + if (ch->packet_queue.empty()) + send_queue.push_back(ch); + + ch->packet_queue.emplace_back(); + + PacketBuffer &pb = ch->packet_queue.back(); + pb.type = type; + pb.data.resize(length); + if (data && length) + memcpy(&pb.data[0], data, length); +} + +static void handle_pkt_connect(int channel_id, uint8_t *data, int plen) +{ + for (auto &ch : channels) + { + if (ch.channel_id == channel_id) + { + // We should handle this in some constructive way. + // This signals that should reset all logical channels. + logger_error("Received a CONNECT packet on a channel that was believed to be previously allocated\n"); + exit(-1); + } + } + + channels.emplace_back(); + + auto &ch = channels.back(); + + ch.channel_id = channel_id; + ch.association = nullptr; + ch.stream_id = 0; + ch.got_eos_from_ami = false; + ch.got_eos_from_client = false; + + std::string service_name((char *)data, plen); + + for (auto &srv : services) + { + if (srv.name == service_name) + { + ClientConnection *cc = srv.cc; + + ch.association = cc; + ch.stream_id = cc->next_stream_id; + + cc->next_stream_id += 2; + cc->associations.push_back(&ch); + + create_and_send_msg(ch.association, MSG_CONNECT, ch.stream_id, data, plen); + return; + } + } + + for (auto &on_demand : on_demand_services) + { + if (on_demand.service_name == service_name) + { + int fds[2]; + int status = socketpair(AF_UNIX, SOCK_STREAM, 0, fds); + if (status != 0) + { + logger_error("Unexpectedly not able to create socket pair.\n"); + exit(-1); + } + + pid_t child = fork(); + if (child == -1) + { + logger_error("Unexpectedly was not able to fork.\n"); + exit(-1); + } + else if (child == 0) + { + close(fds[0]); + int fd = fds[1]; + + // FIXE: The user should be configurable. + setgid(1000); + setuid(1000); + putenv("HOME=/home/pi"); + + std::vector args(on_demand.arguments); + args.push_back("-ondemand"); + args.push_back(std::to_string(fd)); + std::vector args_arr; + for (auto &arg : args) + args_arr.push_back(arg.c_str()); + args_arr.push_back(nullptr); + + execvp(on_demand.program.c_str(), (char* const*) &args_arr[0]); + } + else + { + close(fds[1]); + int fd = fds[0]; + + int status = fcntl(fd, F_SETFD, fcntl(fd, F_GETFD, 0) | FD_CLOEXEC); + if (status == -1) + { + logger_error("Unexpectedly unable to set close-on-exec flag on client socket descriptor; errno = %d\n", errno); + exit(-1); + } + + status = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK); + if (status == -1) + { + logger_error("Unexpectedly unable to set client socket to non blocking; errno = %d\n", errno); + exit(-1); + } + + int flag = 1; + setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(int)); + + connections.emplace_back(); + + ClientConnection &cc = connections.back(); + cc.fd = fd; + cc.next_stream_id = 1; + cc.bytes_read = 0; + + struct epoll_event ev; + ev.events = EPOLLIN | EPOLLOUT | EPOLLET; + ev.data.fd = fd; + if (epoll_ctl(epfd, EPOLL_CTL_ADD, fd, &ev) != 0) + { + logger_error("epoll_ctl() failed unexpectedly with errno = %d\n", errno); + exit(-1); + } + + services.emplace_back(); + + RegisteredService &srv = services.back(); + srv.cc = &cc; + srv.name = std::move(service_name); + + ch.association = &cc; + ch.stream_id = cc.next_stream_id; + + cc.next_stream_id += 2; + cc.associations.push_back(&ch); + + create_and_send_msg(ch.association, MSG_CONNECT, ch.stream_id, data, plen); + return; + } + } + } + + uint8_t response = CONNECT_UNKNOWN_SERVICE; + create_and_enqueue_packet(&ch, PKT_CONNECT_RESPONSE, &response, 1); +} + +static void handle_pkt_data(int channel_id, uint8_t *data, int plen) +{ + for (auto &ch : channels) + { + if (ch.channel_id == channel_id) + { + if (ch.association != nullptr && !ch.got_eos_from_ami) + create_and_send_msg(ch.association, MSG_DATA, ch.stream_id, data, plen); + + break; + } + } +} + +static void handle_pkt_eos(int channel_id) +{ + for (auto &ch : channels) + { + if (ch.channel_id == channel_id) + { + if (ch.association != nullptr && !ch.got_eos_from_ami) + { + ch.got_eos_from_ami = true; + + create_and_send_msg(ch.association, MSG_EOS, ch.stream_id, nullptr, 0); + + if (ch.got_eos_from_client) + remove_association(&ch); + } + break; + } + } +} + +static void handle_pkt_reset(int channel_id) +{ + for (auto &ch : channels) + { + if (ch.channel_id == channel_id) + { + clear_packet_queue(&ch); + + if (ch.association != nullptr) + { + create_and_send_msg(ch.association, MSG_RESET, ch.stream_id, nullptr, 0); + remove_association(&ch); + } + + break; + } + } +} + +static void remove_channel_if_not_associated_and_empty_pq(int channel_id) +{ + for (auto it = channels.begin(); it != channels.end(); it++) + { + if (it->channel_id == channel_id) + { + if (it->association == nullptr && it->packet_queue.empty()) + channels.erase(it); + + break; + } + } +} + +static void handle_received_pkt(int ptype, int channel_id, uint8_t *data, int plen) +{ + if (ptype == PKT_CONNECT) + handle_pkt_connect(channel_id, data, plen); + else if (ptype == PKT_DATA) + handle_pkt_data(channel_id, data, plen); + else if (ptype == PKT_EOS) + handle_pkt_eos(channel_id); + else if (ptype == PKT_RESET) + handle_pkt_reset(channel_id); + + remove_channel_if_not_associated_and_empty_pq(channel_id); +} + +static bool receive_from_a2r() +{ + int head = channel_status[A2R_HEAD_OFFSET]; + int tail = channel_status[A2R_TAIL_OFFSET]; + int len = (tail - head) & 255; + if (len == 0) + return false; + + if (head < tail) + { + memcpy(recv_buf, &ca.a2r_buffer[head], len); + } + else + { + memcpy(recv_buf, &ca.a2r_buffer[head], 256 - head); + + if (tail != 0) + { + memcpy(&recv_buf[len - tail], &ca.a2r_buffer[0], tail); + } + } + + uint8_t *p = recv_buf; + while (p < recv_buf + len) + { + uint8_t plen = *p++; + uint8_t ptype = *p++; + uint8_t channel_id = *p++; + handle_received_pkt(ptype, channel_id, p, plen); + p += plen; + } + + channel_status[A2R_HEAD_OFFSET] = channel_status[A2R_TAIL_OFFSET]; + channel_status_updated |= A_EVENT_A2R_HEAD; + return true; +} + +static bool flush_send_queue() +{ + int tail = channel_status[R2A_TAIL_OFFSET]; + int head = channel_status[R2A_HEAD_OFFSET]; + int len = (tail - head) & 255; + int left = 255 - len; + + int pos = 0; + + while (!send_queue.empty()) + { + LogicalChannel *ch = send_queue.front(); + PacketBuffer &pb = ch->packet_queue.front(); + + int ptype = pb.type; + int plen = 3 + pb.data.size(); + + if (left < plen) + break; + + send_buf[pos++] = pb.data.size(); + send_buf[pos++] = ptype; + send_buf[pos++] = ch->channel_id; + memcpy(&send_buf[pos], &pb.data[0], pb.data.size()); + pos += pb.data.size(); + + ch->packet_queue.pop_front(); + + send_queue.pop_front(); + + if (!ch->packet_queue.empty()) + send_queue.push_back(ch); + else + remove_channel_if_not_associated_and_empty_pq(ch->channel_id); + + left -= plen; + } + + int to_write = pos; + if (!to_write) + return false; + + uint8_t *p = send_buf; + int at_end = 256 - tail; + if (at_end < to_write) + { + memcpy(&ca.r2a_buffer[tail], p, at_end); + p += at_end; + to_write -= at_end; + tail = 0; + } + + memcpy(&ca.r2a_buffer[tail], p, to_write); + tail = (tail + to_write) & 255; + + channel_status[R2A_TAIL_OFFSET] = tail; + channel_status_updated |= A_EVENT_R2A_TAIL; + return true; +} + +static void read_channel_status() +{ + channel_status[A2R_TAIL_OFFSET] = ca.a2r_tail; + channel_status[R2A_HEAD_OFFSET] = ca.r2a_head; + channel_status[R2A_TAIL_OFFSET] = ca.r2a_tail; + channel_status[A2R_HEAD_OFFSET] = ca.a2r_head; + channel_status_updated = 0; +} + +static void write_channel_status() +{ + if (channel_status_updated != 0) + { + ca.r2a_tail = channel_status[R2A_TAIL_OFFSET]; + ca.a2r_head = channel_status[A2R_HEAD_OFFSET]; + + pthread_mutex_lock(&mutex); + ca.a_events |= channel_status_updated; + pthread_mutex_unlock(&mutex); + + channel_status_updated = 0; + } +} + +static void close_all_logical_channels() +{ + send_queue.clear(); + + auto it = channels.begin(); + while (it != channels.end()) + { + LogicalChannel &ch = *it; + + if (ch.association != nullptr) + { + create_and_send_msg(ch.association, MSG_RESET, ch.stream_id, nullptr, 0); + remove_association(&ch); + } + + it = channels.erase(it); + } +} + +static void handle_a314_irq(uint8_t events) +{ + if (events == 0) + return; + + if (events & R_EVENT_STARTED) + { + if (!channels.empty()) + logger_info("Received STARTED event while logical channels are open -- closing channels\n"); + + close_all_logical_channels(); + a314_device_started = true; + } + + if (!a314_device_started) + return; + + read_channel_status(); + + bool any_rcvd = receive_from_a2r(); + bool any_sent = flush_send_queue(); + + if (any_rcvd || any_sent) + write_channel_status(); +} + +static void handle_client_connection_event(ClientConnection *cc, struct epoll_event *ev) +{ + if (ev->events & EPOLLERR) + { + logger_warn("Received EPOLLERR for client connection\n"); + close_and_remove_connection(cc); + return; + } + + if (ev->events & EPOLLIN) + { + while (1) + { + int left; + uint8_t *dst; + + if (cc->payload.empty()) + { + left = sizeof(MessageHeader) - cc->bytes_read; + dst = (uint8_t *)&(cc->header) + cc->bytes_read; + } + else + { + left = cc->header.length - cc->bytes_read; + dst = &cc->payload[cc->bytes_read]; + } + + ssize_t r = read(cc->fd, dst, left); + if (r == -1) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + break; + + logger_error("Read failed unexpectedly with errno = %d\n", errno); + exit(-1); + } + + if (r == 0) + { + logger_info("Received End-of-File on client connection\n"); + close_and_remove_connection(cc); + return; + } + else + { + cc->bytes_read += r; + left -= r; + if (!left) + { + if (cc->payload.empty()) + { + if (cc->header.length == 0) + { + logger_trace("header: length=%d, stream_id=%d, type=%d\n", cc->header.length, cc->header.stream_id, cc->header.type); + handle_received_message(cc); + } + else + { + cc->payload.resize(cc->header.length); + } + } + else + { + logger_trace("header: length=%d, stream_id=%d, type=%d\n", cc->header.length, cc->header.stream_id, cc->header.type); + handle_received_message(cc); + cc->payload.clear(); + } + cc->bytes_read = 0; + } + } + } + } + + if (ev->events & EPOLLOUT) + { + while (!cc->message_queue.empty()) + { + MessageBuffer &mb = cc->message_queue.front(); + + int left = mb.data.size() - mb.pos; + uint8_t *src = &mb.data[mb.pos]; + ssize_t r = write(cc->fd, src, left); + if (r == -1) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + break; + else if (errno == ECONNRESET) + { + close_and_remove_connection(cc); + return; + } + else + { + logger_error("Write failed unexpectedly with errno = %d\n", errno); + exit(-1); + } + } + + mb.pos += r; + if (r == left) + cc->message_queue.pop_front(); + } + } +} + +static void handle_server_socket_ready() +{ + struct sockaddr_in address; + int alen = sizeof(struct sockaddr_in); + + int fd = accept(server_socket, (struct sockaddr *)&address, (socklen_t *)&alen); + if (fd < 0) + { + logger_error("Accept failed unexpectedly with errno = %d\n", errno); + exit(-1); + } + + int status = fcntl(fd, F_SETFD, fcntl(fd, F_GETFD, 0) | FD_CLOEXEC); + if (status == -1) + { + logger_error("Unexpectedly unable to set close-on-exec flag on client socket descriptor; errno = %d\n", errno); + exit(-1); + } + + status = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK); + if (status == -1) + { + logger_error("Unexpectedly unable to set client socket to non blocking; errno = %d\n", errno); + exit(-1); + } + + int flag = 1; + setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(int)); + + connections.emplace_back(); + + ClientConnection &cc = connections.back(); + cc.fd = fd; + cc.next_stream_id = 1; + cc.bytes_read = 0; + + struct epoll_event ev; + ev.events = EPOLLIN | EPOLLOUT | EPOLLET; + ev.data.fd = fd; + if (epoll_ctl(epfd, EPOLL_CTL_ADD, fd, &ev) != 0) + { + logger_error("epoll_ctl() failed unexpectedly with errno = %d\n", errno); + exit(-1); + } +} + +static void main_loop() +{ + bool shutting_down = false; + bool done = false; + + while (!done) + { + struct epoll_event ev; + int timeout = shutting_down ? 10000 : -1; + int n = epoll_pwait(epfd, &ev, 1, timeout, &original_sigset); + if (n == -1) + { + if (errno == EINTR) + { + logger_info("Received SIGTERM\n"); + + shutdown_server_socket(); + + while (!connections.empty()) + close_and_remove_connection(&connections.front()); + + if (flush_send_queue()) + write_channel_status(); + + if (!channels.empty()) + shutting_down = true; + else + done = true; + } + else + { + logger_error("epoll_pwait failed with unexpected errno = %d\n", errno); + exit(-1); + } + } + else if (n == 0) + { + if (shutting_down) + done = true; + else + { + logger_error("epoll_pwait returned 0 which is unexpected since no timeout was set\n"); + exit(-1); + } + } + else + { + if (ev.data.fd == irq_fds[1]) + { + uint8_t events; + if (read(irq_fds[1], &events, 1) != 1) + { + logger_error("Read from interrupt socket pair, and unexpectedly didn't return 1 byte\n"); + exit(-1); + } + + handle_a314_irq(events); + } + else if (ev.data.fd == server_socket) + { + logger_trace("Epoll event: server socket is ready, events = %d\n", ev.events); + handle_server_socket_ready(); + } + else + { + logger_trace("Epoll event: client socket is ready, events = %d\n", ev.events); + + auto it = connections.begin(); + for (; it != connections.end(); it++) + { + if (it->fd == ev.data.fd) + break; + } + + if (it == connections.end()) + { + logger_error("Got notified about an event on a client connection that supposedly isn't currently open\n"); + exit(-1); + } + + ClientConnection *cc = &(*it); + handle_client_connection_event(cc, &ev); + + if (flush_send_queue()) + write_channel_status(); + } + } + } +} + +static void sigterm_handler(int signo) +{ +} + +static void init_sigterm() +{ + /* + sigset_t ss; + sigemptyset(&ss); + sigaddset(&ss, SIGTERM); + sigprocmask(SIG_BLOCK, &ss, &original_sigset); + + struct sigaction sa; + sa.sa_handler = sigterm_handler; + sigemptyset(&sa.sa_mask); + sa.sa_flags = 0; + sigaction(SIGTERM, &sa, NULL); + */ +} + +static int init_driver() +{ + init_sigterm(); + + if (init_server_socket() != 0) + return -1; + + int err = socketpair(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK | SOCK_CLOEXEC, 0, irq_fds); + if (err != 0) + { + logger_error("Unable to create socket pair, errno = %d\n", errno); + return -1; + } + + epfd = epoll_create1(EPOLL_CLOEXEC); + if (epfd == -1) + return -1; + + struct epoll_event ev; + ev.events = EPOLLIN; + ev.data.fd = irq_fds[1]; + if (epoll_ctl(epfd, EPOLL_CTL_ADD, irq_fds[1], &ev) != 0) + return -1; + + ev.events = EPOLLIN; + ev.data.fd = server_socket; + if (epoll_ctl(epfd, EPOLL_CTL_ADD, server_socket, &ev) != 0) + return -1; + + return 0; +} + +static void shutdown_driver() +{ + if (epfd != -1) + close(epfd); + + shutdown_server_socket(); +} + +static void *thread_start(void *arg) +{ + main_loop(); + shutdown_driver(); + return NULL; +} + +static void write_r_events(uint8_t events) +{ + if (write(irq_fds[0], &events, 1) != 1) + logger_error("Write to interrupt socket pair did not return 1\n"); +} + +#ifdef __cplusplus +extern "C" { +#endif + +int a314_init() +{ + std::string conf_filename("/etc/opt/a314/a314d.conf"); + + load_config_file(conf_filename.c_str()); + + int err = init_driver(); + if (err < 0) + { + shutdown_driver(); + return -1; + } + + err = pthread_create(&thread_id, NULL, thread_start, NULL); + if (err < 0) + { + logger_error("pthread_create failed with err = %d\n", err); + return -2; + } + + return 0; +} + +void a314_process_events() +{ + if (ca.a_events & ca.a_enable) + { + write16(0xdff09c, 0x8008); + m68k_set_irq(2); + } +} + +unsigned int a314_read_memory_8(unsigned int address) +{ + if (address >= sizeof(ca)) + return 0; + + uint8_t val; + if (address == offsetof(ComArea, a_events)) + { + pthread_mutex_lock(&mutex); + val = ca.a_events; + ca.a_events = 0; + pthread_mutex_unlock(&mutex); + } + else + { + uint8_t *p = (uint8_t *)&ca; + val = p[address]; + } + + return val; +} + +unsigned int a314_read_memory_16(unsigned int address) +{ + // Not implemented. + return 0; +} + +unsigned int a314_read_memory_32(unsigned int address) +{ + // Not implemented. + return 0; +} + +void a314_write_memory_8(unsigned int address, unsigned int value) +{ + if (address >= sizeof(ca)) + return; + + switch (address) + { + case offsetof(ComArea, a_events): + // a_events is not writable. + break; + + case offsetof(ComArea, r_events): + if (value != 0) + write_r_events((uint8_t)value); + break; + + default: + { + uint8_t *p = (uint8_t *)&ca; + p[address] = (uint8_t)value; + break; + } + } +} + +void a314_write_memory_16(unsigned int address, unsigned int value) +{ + // Not implemented. +} + +void a314_write_memory_32(unsigned int address, unsigned int value) +{ + // Not implemented. +} + +#ifdef __cplusplus +} +#endif diff --git a/a314/a314.h b/a314/a314.h new file mode 100644 index 0000000..14fca5b --- /dev/null +++ b/a314/a314.h @@ -0,0 +1,31 @@ +// A314 emulation. + +#ifndef A314_H +#define A314_H + +#ifdef __cplusplus +extern "C" { +#endif + +#define A314_ENABLED 1 + +// TODO: Base address should be obtained dynamically through Auto-Config. +#define A314_COM_AREA_BASE 0xE90000 +#define A314_COM_AREA_SIZE (64*1024) + +int a314_init(); +void a314_process_events(); + +unsigned int a314_read_memory_8(unsigned int address); +unsigned int a314_read_memory_16(unsigned int address); +unsigned int a314_read_memory_32(unsigned int address); + +void a314_write_memory_8(unsigned int address, unsigned int value); +void a314_write_memory_16(unsigned int address, unsigned int value); +void a314_write_memory_32(unsigned int address, unsigned int value); + +#ifdef __cplusplus +} +#endif + +#endif /* A314_H */ diff --git a/a314/a314device/a314.h b/a314/a314device/a314.h new file mode 100644 index 0000000..0c10f25 --- /dev/null +++ b/a314/a314device/a314.h @@ -0,0 +1,43 @@ +#ifndef DEVICES_A314_H +#define DEVICES_A314_H + +#include + +#define A314_NAME "a314.device" + +#define A314_CONNECT (CMD_NONSTD+0) +#define A314_READ (CMD_NONSTD+1) +#define A314_WRITE (CMD_NONSTD+2) +#define A314_EOS (CMD_NONSTD+3) +#define A314_RESET (CMD_NONSTD+4) + +#define A314_CONNECT_OK 0 +#define A314_CONNECT_SOCKET_IN_USE 1 +#define A314_CONNECT_RESET 2 +#define A314_CONNECT_UNKNOWN_SERVICE 3 + +#define A314_READ_OK 0 +#define A314_READ_EOS 1 +#define A314_READ_RESET 2 + +#define A314_WRITE_OK 0 +#define A314_WRITE_EOS_SENT 1 +#define A314_WRITE_RESET 2 + +#define A314_EOS_OK 0 +#define A314_EOS_EOS_SENT 1 +#define A314_EOS_RESET 2 + +#define A314_RESET_OK 0 + +#define MEMF_A314 (1<<7) + +struct A314_IORequest +{ + struct IORequest a314_Request; + ULONG a314_Socket; + STRPTR a314_Buffer; + WORD a314_Length; +}; + +#endif diff --git a/a314/a314device/a314driver.c b/a314/a314device/a314driver.c new file mode 100644 index 0000000..6db2dc6 --- /dev/null +++ b/a314/a314device/a314driver.c @@ -0,0 +1,623 @@ +/* + * Copyright (c) 2018 Niklas Ekström + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +#include "a314.h" +#include "debug.h" +#include "device.h" +#include "protocol.h" +#include "sockets.h" +#include "fix_mem_region.h" +#include "startup.h" + +int used_in_r2a() +{ + return (ca->r2a_tail - ca->r2a_head) & 255; +} + +int used_in_a2r() +{ + return (ca->a2r_tail - ca->a2r_head) & 255; +} + +BOOL room_in_a2r(int len) +{ + return used_in_a2r() + 3 + len <= 255; +} + +void append_a2r_packet(UBYTE type, UBYTE stream_id, UBYTE length, UBYTE *data) +{ + UBYTE index = ca->a2r_tail; + ca->a2r_buffer[index++] = length; + ca->a2r_buffer[index++] = type; + ca->a2r_buffer[index++] = stream_id; + for (int i = 0; i < (int)length; i++) + ca->a2r_buffer[index++] = *data++; + ca->a2r_tail = index; +} + +void close_socket(struct Socket *s, BOOL should_send_reset) +{ + debug_printf("Called close socket\n"); + + if (s->pending_connect != NULL) + { + struct A314_IORequest *ior = s->pending_connect; + ior->a314_Request.io_Error = A314_CONNECT_RESET; + ReplyMsg((struct Message *)ior); + + s->pending_connect = NULL; + } + + if (s->pending_read != NULL) + { + struct A314_IORequest *ior = s->pending_read; + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_READ_RESET; + ReplyMsg((struct Message *)ior); + + s->pending_read = NULL; + } + + if (s->pending_write != NULL) + { + struct A314_IORequest *ior = s->pending_write; + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_WRITE_RESET; // A314_EOS_RESET == A314_WRITE_RESET + ReplyMsg((struct Message *)ior); + + s->pending_write = NULL; + } + + if (s->rq_head != NULL) + { + struct QueuedData *qd = s->rq_head; + while (qd != NULL) + { + struct QueuedData *next = qd->next; + FreeMem(qd, sizeof(struct QueuedData) + qd->length); + qd = next; + } + s->rq_head = NULL; + s->rq_tail = NULL; + } + + remove_from_send_queue(s); + + // No operations can be pending when SOCKET_CLOSED is set. + // However, may not be able to delete socket yet, because is waiting to send PKT_RESET. + s->flags |= SOCKET_CLOSED; + + BOOL should_delete_socket = TRUE; + + if (should_send_reset) + { + if (send_queue_head == NULL && room_in_a2r(0)) + { + append_a2r_packet(PKT_RESET, s->stream_id, 0, NULL); + } + else + { + s->flags |= SOCKET_SHOULD_SEND_RESET; + add_to_send_queue(s, 0); + should_delete_socket = FALSE; + } + } + + if (should_delete_socket) + delete_socket(s); +} + +// When a message is received on R2A it is written to this buffer, +// to avoid dealing with the issue that R2A is a circular buffer. +// This is somewhat inefficient, so may want to change that to read from R2A directly. +UBYTE received_packet[256]; + +static void handle_pkt_connect_response(UBYTE length, struct Socket *s) +{ + debug_printf("Received a CONNECT RESPONSE packet from rpi\n"); + + if (s->pending_connect == NULL) + { + debug_printf("SERIOUS ERROR: received a CONNECT RESPONSE even though no connect was pending\n"); + // Should reset stream? + } + else if (length != 1) + { + debug_printf("SERIOUS ERROR: received a CONNECT RESPONSE whose length was not 1\n"); + // Should reset stream? + } + else + { + UBYTE result = received_packet[0]; + if (result == 0) + { + struct A314_IORequest *ior = s->pending_connect; + ior->a314_Request.io_Error = A314_CONNECT_OK; + ReplyMsg((struct Message *)ior); + + s->pending_connect = NULL; + } + else + { + struct A314_IORequest *ior = s->pending_connect; + ior->a314_Request.io_Error = A314_CONNECT_UNKNOWN_SERVICE; + ReplyMsg((struct Message *)ior); + + s->pending_connect = NULL; + + close_socket(s, FALSE); + } + } +} + +static void handle_pkt_data(UBYTE length, struct Socket *s) +{ + debug_printf("Received a DATA packet from rpi\n"); + + if (s->pending_read != NULL) + { + struct A314_IORequest *ior = s->pending_read; + + if (ior->a314_Length < length) + close_socket(s, TRUE); + else + { + memcpy(ior->a314_Buffer, received_packet, length); + ior->a314_Length = length; + ior->a314_Request.io_Error = A314_READ_OK; + ReplyMsg((struct Message *)ior); + + s->pending_read = NULL; + } + } + else + { + struct QueuedData *qd = (struct QueuedData *)AllocMem(sizeof(struct QueuedData) + length, 0); + qd->next = NULL, + qd->length = length; + memcpy(qd->data, received_packet, length); + + if (s->rq_head == NULL) + s->rq_head = qd; + else + s->rq_tail->next = qd; + s->rq_tail = qd; + } +} + +static void handle_pkt_eos(struct Socket *s) +{ + debug_printf("Received a EOS packet from rpi\n"); + + s->flags |= SOCKET_RCVD_EOS_FROM_RPI; + + if (s->pending_read != NULL) + { + struct A314_IORequest *ior = s->pending_read; + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_READ_EOS; + ReplyMsg((struct Message *)ior); + + s->pending_read = NULL; + + s->flags |= SOCKET_SENT_EOS_TO_APP; + + if (s->flags & SOCKET_SENT_EOS_TO_RPI) + close_socket(s, FALSE); + } +} + +static void handle_r2a_packet(UBYTE type, UBYTE stream_id, UBYTE length) +{ + struct Socket *s = find_socket_by_stream_id(stream_id); + + if (s != NULL && type == PKT_RESET) + { + debug_printf("Received a RESET packet from rpi\n"); + close_socket(s, FALSE); + return; + } + + if (s == NULL || (s->flags & SOCKET_CLOSED)) + { + // Ignore this packet. The only packet that can do anything useful on a closed + // channel is CONNECT, which is not handled at this time. + return; + } + + if (type == PKT_CONNECT_RESPONSE) + { + handle_pkt_connect_response(length, s); + } + else if (type == PKT_DATA) + { + handle_pkt_data(length, s); + } + else if (type == PKT_EOS) + { + handle_pkt_eos(s); + } +} + +void handle_packets_received_r2a() +{ + while (used_in_r2a() != 0) + { + UBYTE index = ca->r2a_head; + + UBYTE len = ca->r2a_buffer[index++]; + UBYTE type = ca->r2a_buffer[index++]; + UBYTE stream_id = ca->r2a_buffer[index++]; + + for (int i = 0; i < len; i++) + received_packet[i] = ca->r2a_buffer[index++]; + + ca->r2a_head = index; + + handle_r2a_packet(type, stream_id, len); + } +} + +void handle_room_in_a2r() +{ + while (send_queue_head != NULL) + { + struct Socket *s = send_queue_head; + + if (!room_in_a2r(s->send_queue_required_length)) + break; + + remove_from_send_queue(s); + + if (s->pending_connect != NULL) + { + struct A314_IORequest *ior = s->pending_connect; + int len = ior->a314_Length; + append_a2r_packet(PKT_CONNECT, s->stream_id, (UBYTE)len, ior->a314_Buffer); + } + else if (s->pending_write != NULL) + { + struct A314_IORequest *ior = s->pending_write; + int len = ior->a314_Length; + + if (ior->a314_Request.io_Command == A314_WRITE) + { + append_a2r_packet(PKT_DATA, s->stream_id, (UBYTE)len, ior->a314_Buffer); + + ior->a314_Request.io_Error = A314_WRITE_OK; + ReplyMsg((struct Message *)ior); + + s->pending_write = NULL; + } + else // A314_EOS + { + append_a2r_packet(PKT_EOS, s->stream_id, 0, NULL); + + ior->a314_Request.io_Error = A314_EOS_OK; + ReplyMsg((struct Message *)ior); + + s->pending_write = NULL; + + s->flags |= SOCKET_SENT_EOS_TO_RPI; + + if (s->flags & SOCKET_SENT_EOS_TO_APP) + close_socket(s, FALSE); + } + } + else if (s->flags & SOCKET_SHOULD_SEND_RESET) + { + append_a2r_packet(PKT_RESET, s->stream_id, 0, NULL); + delete_socket(s); + } + else + { + debug_printf("SERIOUS ERROR: Was in send queue but has nothing to send\n"); + } + } +} + +static void handle_app_connect(struct A314_IORequest *ior, struct Socket *s) +{ + debug_printf("Received a CONNECT request from application\n"); + + if (s != NULL) + { + ior->a314_Request.io_Error = A314_CONNECT_SOCKET_IN_USE; + ReplyMsg((struct Message *)ior); + } + else if (ior->a314_Length + 3 > 255) + { + ior->a314_Request.io_Error = A314_CONNECT_RESET; + ReplyMsg((struct Message *)ior); + } + else + { + s = create_socket(ior->a314_Request.io_Message.mn_ReplyPort->mp_SigTask, ior->a314_Socket); + + s->pending_connect = ior; + s->flags = 0; + + int len = ior->a314_Length; + if (send_queue_head == NULL && room_in_a2r(len)) + { + append_a2r_packet(PKT_CONNECT, s->stream_id, (UBYTE)len, ior->a314_Buffer); + } + else + { + add_to_send_queue(s, len); + } + } +} + +static void handle_app_read(struct A314_IORequest *ior, struct Socket *s) +{ + debug_printf("Received a READ request from application\n"); + + if (s == NULL || (s->flags & SOCKET_CLOSED)) + { + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_READ_RESET; + ReplyMsg((struct Message *)ior); + } + else + { + if (s->pending_connect != NULL || s->pending_read != NULL) + { + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_READ_RESET; + ReplyMsg((struct Message *)ior); + + close_socket(s, TRUE); + } + else if (s->rq_head != NULL) + { + struct QueuedData *qd = s->rq_head; + int len = qd->length; + + if (ior->a314_Length < len) + { + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_READ_RESET; + ReplyMsg((struct Message *)ior); + + close_socket(s, TRUE); + } + else + { + s->rq_head = qd->next; + if (s->rq_head == NULL) + s->rq_tail = NULL; + + memcpy(ior->a314_Buffer, qd->data, len); + FreeMem(qd, sizeof(struct QueuedData) + len); + + ior->a314_Length = len; + ior->a314_Request.io_Error = A314_READ_OK; + ReplyMsg((struct Message *)ior); + } + } + else if (s->flags & SOCKET_RCVD_EOS_FROM_RPI) + { + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_READ_EOS; + ReplyMsg((struct Message *)ior); + + s->flags |= SOCKET_SENT_EOS_TO_APP; + + if (s->flags & SOCKET_SENT_EOS_TO_RPI) + close_socket(s, FALSE); + } + else + s->pending_read = ior; + } +} + +static void handle_app_write(struct A314_IORequest *ior, struct Socket *s) +{ + debug_printf("Received a WRITE request from application\n"); + + if (s == NULL || (s->flags & SOCKET_CLOSED)) + { + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_WRITE_RESET; + ReplyMsg((struct Message *)ior); + } + else + { + int len = ior->a314_Length; + if (s->pending_connect != NULL || s->pending_write != NULL || (s->flags & SOCKET_RCVD_EOS_FROM_APP) || len + 3 > 255) + { + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_WRITE_RESET; + ReplyMsg((struct Message *)ior); + + close_socket(s, TRUE); + } + else + { + if (send_queue_head == NULL && room_in_a2r(len)) + { + append_a2r_packet(PKT_DATA, s->stream_id, (UBYTE)len, ior->a314_Buffer); + + ior->a314_Request.io_Error = A314_WRITE_OK; + ReplyMsg((struct Message *)ior); + } + else + { + s->pending_write = ior; + add_to_send_queue(s, len); + } + } + } +} + +static void handle_app_eos(struct A314_IORequest *ior, struct Socket *s) +{ + debug_printf("Received an EOS request from application\n"); + + if (s == NULL || (s->flags & SOCKET_CLOSED)) + { + ior->a314_Request.io_Error = A314_EOS_RESET; + ReplyMsg((struct Message *)ior); + } + else + { + if (s->pending_connect != NULL || s->pending_write != NULL || (s->flags & SOCKET_RCVD_EOS_FROM_APP)) + { + ior->a314_Length = 0; + ior->a314_Request.io_Error = A314_EOS_RESET; + ReplyMsg((struct Message *)ior); + + close_socket(s, TRUE); + } + else + { + s->flags |= SOCKET_RCVD_EOS_FROM_APP; + + if (send_queue_head == NULL && room_in_a2r(0)) + { + append_a2r_packet(PKT_EOS, s->stream_id, 0, NULL); + + ior->a314_Request.io_Error = A314_EOS_OK; + ReplyMsg((struct Message *)ior); + + s->flags |= SOCKET_SENT_EOS_TO_RPI; + + if (s->flags & SOCKET_SENT_EOS_TO_APP) + close_socket(s, FALSE); + } + else + { + s->pending_write = ior; + add_to_send_queue(s, 0); + } + } + } +} + +static void handle_app_reset(struct A314_IORequest *ior, struct Socket *s) +{ + debug_printf("Received a RESET request from application\n"); + + if (s == NULL || (s->flags & SOCKET_CLOSED)) + { + ior->a314_Request.io_Error = A314_RESET_OK; + ReplyMsg((struct Message *)ior); + } + else + { + ior->a314_Request.io_Error = A314_RESET_OK; + ReplyMsg((struct Message *)ior); + + close_socket(s, TRUE); + } +} + +static void handle_app_request(struct A314_IORequest *ior) +{ + struct Socket *s = find_socket(ior->a314_Request.io_Message.mn_ReplyPort->mp_SigTask, ior->a314_Socket); + + switch (ior->a314_Request.io_Command) + { + case A314_CONNECT: + handle_app_connect(ior, s); + break; + case A314_READ: + handle_app_read(ior, s); + break; + case A314_WRITE: + handle_app_write(ior, s); + break; + case A314_EOS: + handle_app_eos(ior, s); + break; + case A314_RESET: + handle_app_reset(ior, s); + break; + default: + ior->a314_Request.io_Error = IOERR_NOCMD; + ReplyMsg((struct Message *)ior); + break; + } +} + +void task_main() +{ + while (TRUE) + { + debug_printf("Waiting for signal\n"); + + ULONG signal = Wait(SIGF_MSGPORT | SIGF_INT); + + UBYTE prev_a2r_tail = ca->a2r_tail; + UBYTE prev_r2a_head = ca->r2a_head; + + if (signal & SIGF_MSGPORT) + { + ca->a_enable = 0; + + struct Message *msg; + while (msg = GetMsg(&task_mp)) + handle_app_request((struct A314_IORequest *)msg); + } + + UBYTE a_enable = 0; + while (a_enable == 0) + { + handle_packets_received_r2a(); + handle_room_in_a2r(); + + UBYTE r_events = 0; + if (ca->a2r_tail != prev_a2r_tail) + r_events |= R_EVENT_A2R_TAIL; + if (ca->r2a_head != prev_r2a_head) + r_events |= R_EVENT_R2A_HEAD; + + UBYTE discard_value = ca->a_events; + + if (ca->r2a_head == ca->r2a_tail) + { + if (send_queue_head == NULL) + a_enable = A_EVENT_R2A_TAIL; + else if (!room_in_a2r(send_queue_head->send_queue_required_length)) + a_enable = A_EVENT_R2A_TAIL | A_EVENT_A2R_HEAD; + + if (a_enable != 0) + { + ca->a_enable = a_enable; + if (r_events != 0) + ca->r_events = r_events; + } + } + } + } + + // There is currently no way to unload a314.device. + + //debug_printf("Shutting down\n"); + + //RemIntServer(INTB_PORTS, &ports_interrupt); + //RemIntServer(INTB_VERTB, &vertb_interrupt); + //FreeMem(ca, sizeof(struct ComArea)); + + // Stack and task structure should be reclaimed. +} diff --git a/a314/a314device/build.bat b/a314/a314device/build.bat new file mode 100644 index 0000000..d2b8ccb --- /dev/null +++ b/a314/a314device/build.bat @@ -0,0 +1 @@ +vc romtag.asm a314driver.c device.c startup.c fix_mem_region.c sockets.c int_server.asm -O3 -nostdlib -o a314.device diff --git a/a314/a314device/debug.h b/a314/a314device/debug.h new file mode 100644 index 0000000..55a465b --- /dev/null +++ b/a314/a314device/debug.h @@ -0,0 +1,3 @@ +#define DEBUG 0 +//#define debug_printf(...) do { if (DEBUG) fprintf(stdout, __VA_ARGS__); } while (0) +#define debug_printf(...) diff --git a/a314/a314device/device.c b/a314/a314device/device.c new file mode 100644 index 0000000..7863c96 --- /dev/null +++ b/a314/a314device/device.c @@ -0,0 +1,121 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "device.h" +#include "a314.h" +#include "startup.h" +#include "fix_mem_region.h" + +char device_name[] = A314_NAME; +char id_string[] = A314_NAME " 1.1 (28 Nov 2020)"; + +struct ExecBase *SysBase; +BPTR saved_seg_list; +BOOL running = FALSE; + +static struct Library *init_device(__reg("a6") struct ExecBase *sys_base, __reg("a0") BPTR seg_list, __reg("d0") struct Library *dev) +{ + SysBase = *(struct ExecBase **)4; + saved_seg_list = seg_list; + + // We are being called from InitResident() in initializers.asm. + // MakeLibrary() was executed before we got here. + + dev->lib_Node.ln_Type = NT_DEVICE; + dev->lib_Node.ln_Name = device_name; + dev->lib_Flags = LIBF_SUMUSED | LIBF_CHANGED; + dev->lib_Version = 1; + dev->lib_Revision = 0; + dev->lib_IdString = (APTR)id_string; + + // AddDevice() is executed after we return. + return dev; +} + +static BPTR expunge(__reg("a6") struct Library *dev) +{ + // There is currently no support for unloading a314.device. + + if (TRUE) //dev->lib_OpenCnt != 0) + { + dev->lib_Flags |= LIBF_DELEXP; + return 0; + } + + /* + BPTR seg_list = saved_seg_list; + Remove(&dev->lib_Node); + FreeMem((char *)dev - dev->lib_NegSize, dev->lib_NegSize + dev->lib_PosSize); + return seg_list; + */ +} + +static void open(__reg("a6") struct Library *dev, __reg("a1") struct A314_IORequest *ior, __reg("d0") ULONG unitnum, __reg("d1") ULONG flags) +{ + dev->lib_OpenCnt++; + + if (dev->lib_OpenCnt == 1 && !running) + { + if (!task_start()) + { + ior->a314_Request.io_Error = IOERR_OPENFAIL; + ior->a314_Request.io_Message.mn_Node.ln_Type = NT_REPLYMSG; + dev->lib_OpenCnt--; + return; + } + running = TRUE; + } + + ior->a314_Request.io_Error = 0; + ior->a314_Request.io_Message.mn_Node.ln_Type = NT_REPLYMSG; +} + +static BPTR close(__reg("a6") struct Library *dev, __reg("a1") struct A314_IORequest *ior) +{ + ior->a314_Request.io_Device = NULL; + ior->a314_Request.io_Unit = NULL; + + dev->lib_OpenCnt--; + + if (dev->lib_OpenCnt == 0 && (dev->lib_Flags & LIBF_DELEXP)) + return expunge(dev); + + return 0; +} + +static void begin_io(__reg("a6") struct Library *dev, __reg("a1") struct A314_IORequest *ior) +{ + PutMsg(&task_mp, (struct Message *)ior); + ior->a314_Request.io_Flags &= ~IOF_QUICK; +} + +static ULONG abort_io(__reg("a6") struct Library *dev, __reg("a1") struct A314_IORequest *ior) +{ + // There is currently no support for aborting an IORequest. + return IOERR_NOCMD; +} + +static ULONG device_vectors[] = +{ + (ULONG)open, + (ULONG)close, + (ULONG)expunge, + 0, + (ULONG)begin_io, + (ULONG)abort_io, + (ULONG)translate_address_a314, + -1, +}; + +ULONG auto_init_tables[] = +{ + sizeof(struct Library), + (ULONG)device_vectors, + 0, + (ULONG)init_device, +}; diff --git a/a314/a314device/device.h b/a314/a314device/device.h new file mode 100644 index 0000000..4aff373 --- /dev/null +++ b/a314/a314device/device.h @@ -0,0 +1,2 @@ +extern char device_name[]; +extern char id_string[]; diff --git a/a314/a314device/fix_mem_region.c b/a314/a314device/fix_mem_region.c new file mode 100644 index 0000000..9d9587c --- /dev/null +++ b/a314/a314device/fix_mem_region.c @@ -0,0 +1,131 @@ +#include +#include +#include +#include +#include + +#include "a314.h" +#include "fix_mem_region.h" +#include "protocol.h" + +struct MemChunkList +{ + struct MemChunk *first; + struct MemChunk *last; + ULONG free; +}; + +void add_chunk(struct MemChunkList *l, struct MemChunk *mc) +{ + if (l->first == NULL) + l->first = mc; + else + l->last->mc_Next = mc; + l->last = mc; + l->free += mc->mc_Bytes; +} + +struct MemHeader *split_region(struct MemHeader *lower, ULONG split_at) +{ + struct MemHeader *upper = (struct MemHeader *)AllocMem(sizeof(struct MemHeader), MEMF_PUBLIC | MEMF_CLEAR); + + struct MemChunkList ll = {NULL, NULL, 0}; + struct MemChunkList ul = {NULL, NULL, 0}; + + struct MemChunk *mc = lower->mh_First; + + while (mc != NULL) + { + struct MemChunk *next_chunk = mc->mc_Next; + mc->mc_Next = NULL; + + ULONG start = (ULONG)mc; + ULONG end = start + mc->mc_Bytes; + + if (end <= split_at) + add_chunk(&ll, mc); + else if (split_at <= start) + add_chunk(&ul, mc); + else + { + mc->mc_Bytes = split_at - start; + add_chunk(&ll, mc); + + struct MemChunk *new_chunk = (struct MemChunk *)split_at; + new_chunk->mc_Next = NULL; + new_chunk->mc_Bytes = end - split_at; + add_chunk(&ul, new_chunk); + } + mc = next_chunk; + } + + upper->mh_Node.ln_Type = NT_MEMORY; + upper->mh_Node.ln_Pri = lower->mh_Node.ln_Pri; + upper->mh_Node.ln_Name = lower->mh_Node.ln_Name; // Use a custom name? + upper->mh_Attributes = lower->mh_Attributes; + + lower->mh_First = ll.first; + upper->mh_First = ul.first; + + upper->mh_Lower = (APTR)split_at; + upper->mh_Upper = lower->mh_Upper; + lower->mh_Upper = (APTR)split_at; + + lower->mh_Free = ll.free; + upper->mh_Free = ul.free; + + return upper; +} + +BOOL overlap(struct MemHeader *mh, ULONG lower, ULONG upper) +{ + return lower < (ULONG)(mh->mh_Upper) && (ULONG)(mh->mh_Lower) < upper; +} + +void mark_region_a314(ULONG address, ULONG size) +{ + struct List *memlist = &(SysBase->MemList); + + for (struct Node *node = memlist->lh_Head; node->ln_Succ != NULL; node = node->ln_Succ) + { + struct MemHeader *mh = (struct MemHeader *)node; + if (overlap(mh, address, address + size)) + { + if ((ULONG)mh->mh_Lower < address) + { + mh->mh_Attributes &= ~MEMF_A314; + mh = split_region(mh, address); + } + else + Remove((struct Node *)mh); + + if (address + size < (ULONG)mh->mh_Upper) + { + struct MemHeader *new_mh = split_region(mh, address + size); + new_mh->mh_Attributes &= ~MEMF_A314; + Enqueue(memlist, (struct Node *)new_mh); + } + + mh->mh_Node.ln_Pri = -20; + mh->mh_Attributes |= MEMF_A314; + Enqueue(memlist, (struct Node *)mh); + return; + } + } +} + +BOOL fix_memory() +{ + Forbid(); + mark_region_a314(PISTORM_BASE, PISTORM_SIZE); + Permit(); + return TRUE; +} + +ULONG translate_address_a314(__reg("a0") void *address) +{ + ULONG offset = (ULONG)address - PISTORM_BASE; + if (offset < PISTORM_SIZE) + return offset; + return -1; +} diff --git a/a314/a314device/fix_mem_region.h b/a314/a314device/fix_mem_region.h new file mode 100644 index 0000000..02a46fe --- /dev/null +++ b/a314/a314device/fix_mem_region.h @@ -0,0 +1,4 @@ +#include + +extern ULONG translate_address_a314(__reg("a0") void *address); +extern BOOL fix_memory(); diff --git a/a314/a314device/int_server.asm b/a314/a314device/int_server.asm new file mode 100644 index 0000000..2a7b866 --- /dev/null +++ b/a314/a314device/int_server.asm @@ -0,0 +1,25 @@ + XDEF _IntServer + CODE + +COM_AREA equ $e90000 + +SIGB_INT equ 14 +SIGF_INT equ (1 << SIGB_INT) + + ; a1 points to driver task +_IntServer: lea.l COM_AREA,a5 + + move.b 0(a5),d0 ; A_EVENTS + and.b 1(a5),d0 ; A_ENABLE + beq.s should_not_signal + + move.b #0,1(a5) + + move.l $4.w,a6 + move.l #SIGF_INT,d0 + ; a1 = pointer to driver task + jsr -324(a6) ; Signal() + +should_not_signal: + moveq #0,d0 + rts diff --git a/a314/a314device/proto_a314.h b/a314/a314device/proto_a314.h new file mode 100644 index 0000000..982da4f --- /dev/null +++ b/a314/a314device/proto_a314.h @@ -0,0 +1,9 @@ +#ifndef PROTO_A314_H +#define PROTO_A314_H + +extern struct Library *A314Base; + +ULONG __TranslateAddressA314(__reg("a6") void *, __reg("a0") void *)="\tjsr\t-42(a6)"; +#define TranslateAddressA314(address) __TranslateAddressA314(A314Base, address) + +#endif diff --git a/a314/a314device/protocol.h b/a314/a314device/protocol.h new file mode 100644 index 0000000..6ff50a2 --- /dev/null +++ b/a314/a314device/protocol.h @@ -0,0 +1,44 @@ +#include + +// Packet types that are sent across the physical channel. +#define PKT_DRIVER_STARTED 1 +#define PKT_DRIVER_SHUTTING_DOWN 2 +#define PKT_SETTINGS 3 +#define PKT_CONNECT 4 +#define PKT_CONNECT_RESPONSE 5 +#define PKT_DATA 6 +#define PKT_EOS 7 +#define PKT_RESET 8 + +// Events that are communicated via IRQ from Amiga to Raspberry. +#define R_EVENT_A2R_TAIL 1 +#define R_EVENT_R2A_HEAD 2 +#define R_EVENT_STARTED 4 + +// Events that are communicated from Raspberry to Amiga. +#define A_EVENT_R2A_TAIL 1 +#define A_EVENT_A2R_HEAD 2 + +#define COM_AREA_BASE 0xe90000 + +#define PISTORM_BASE 0xc00000 +#define PISTORM_SIZE (3*512*1024) + +// The communication area, used to create the physical channel. +struct ComArea +{ + volatile UBYTE a_events; + volatile UBYTE a_enable; + volatile UBYTE r_events; + volatile UBYTE r_enable; + + volatile UBYTE a2r_tail; + volatile UBYTE r2a_head; + volatile UBYTE r2a_tail; + volatile UBYTE a2r_head; + + UBYTE a2r_buffer[256]; + UBYTE r2a_buffer[256]; +}; + +extern struct ComArea *ca; diff --git a/a314/a314device/romtag.asm b/a314/a314device/romtag.asm new file mode 100644 index 0000000..ef4a867 --- /dev/null +++ b/a314/a314device/romtag.asm @@ -0,0 +1,23 @@ +RTC_MATCHWORD: equ $4afc +RTF_AUTOINIT: equ (1<<7) +NT_DEVICE: equ 3 +VERSION: equ 1 +PRIORITY: equ 0 + + section code,code + + moveq #-1,d0 + rts + +romtag: + dc.w RTC_MATCHWORD + dc.l romtag + dc.l endcode + dc.b RTF_AUTOINIT + dc.b VERSION + dc.b NT_DEVICE + dc.b PRIORITY + dc.l _device_name + dc.l _id_string + dc.l _auto_init_tables +endcode: diff --git a/a314/a314device/sockets.c b/a314/a314device/sockets.c new file mode 100644 index 0000000..8a329c5 --- /dev/null +++ b/a314/a314device/sockets.c @@ -0,0 +1,115 @@ +#include + +#include "sockets.h" + +struct List active_sockets; + +struct Socket *send_queue_head = NULL; +struct Socket *send_queue_tail = NULL; + +static UBYTE next_stream_id = 1; + +extern void NewList(struct List *l); + +void init_sockets() +{ + NewList(&active_sockets); +} + +struct Socket *find_socket(void *sig_task, ULONG socket) +{ + for (struct Node *node = active_sockets.lh_Head; node->ln_Succ != NULL; node = node->ln_Succ) + { + struct Socket *s = (struct Socket *)node; + if (s->sig_task == sig_task && s->socket == socket) + return s; + } + return NULL; +} + +struct Socket *find_socket_by_stream_id(UBYTE stream_id) +{ + for (struct Node *node = active_sockets.lh_Head; node->ln_Succ != NULL; node = node->ln_Succ) + { + struct Socket *s = (struct Socket *)node; + if (s->stream_id == stream_id) + return s; + } + return NULL; +} + +static UBYTE allocate_stream_id() +{ + // Bug: If all stream ids are allocated then this loop won't terminate. + + while (1) + { + UBYTE stream_id = next_stream_id; + next_stream_id += 2; + if (find_socket_by_stream_id(stream_id) == NULL) + return stream_id; + } +} + +static void free_stream_id(UBYTE stream_id) +{ + // Currently do nothing. + // Could speed up allocate_stream_id using a bitmap? +} + +struct Socket *create_socket(struct Task *task, ULONG id) +{ + struct Socket *s = (struct Socket *)AllocMem(sizeof(struct Socket), MEMF_CLEAR); + s->sig_task = task; + s->socket = id; + s->stream_id = allocate_stream_id(); + AddTail(&active_sockets, (struct Node *)s); + return s; +} + +void delete_socket(struct Socket *s) +{ + Remove((struct Node *)s); + free_stream_id(s->stream_id); + FreeMem(s, sizeof(struct Socket)); +} + +void add_to_send_queue(struct Socket *s, UWORD required_length) +{ + s->send_queue_required_length = required_length; + s->next_in_send_queue = NULL; + + if (send_queue_head == NULL) + send_queue_head = s; + else + send_queue_tail->next_in_send_queue = s; + send_queue_tail = s; + + s->flags |= SOCKET_IN_SEND_QUEUE; +} + +void remove_from_send_queue(struct Socket *s) +{ + if (s->flags & SOCKET_IN_SEND_QUEUE) + { + if (send_queue_head == s) + { + send_queue_head = s->next_in_send_queue; + if (send_queue_head == NULL) + send_queue_tail = NULL; + } + else + { + struct Socket *curr = send_queue_head; + while (curr->next_in_send_queue != s) + curr = curr->next_in_send_queue; + + curr->next_in_send_queue = s->next_in_send_queue; + if (send_queue_tail == s) + send_queue_tail = curr; + } + + s->next_in_send_queue = NULL; + s->flags &= ~SOCKET_IN_SEND_QUEUE; + } +} diff --git a/a314/a314device/sockets.h b/a314/a314device/sockets.h new file mode 100644 index 0000000..ba439aa --- /dev/null +++ b/a314/a314device/sockets.h @@ -0,0 +1,57 @@ +#include +#include + +// Used to store received data until application asks for it using a A314_READ. +struct QueuedData +{ + struct QueuedData *next; + UWORD length; + UBYTE data[]; +}; + +// Socket flags, these are bit masks, can have many of them. +#define SOCKET_RCVD_EOS_FROM_APP 0x0004 +#define SOCKET_RCVD_EOS_FROM_RPI 0x0008 +#define SOCKET_SENT_EOS_TO_APP 0x0010 +#define SOCKET_SENT_EOS_TO_RPI 0x0020 +#define SOCKET_CLOSED 0x0040 +#define SOCKET_SHOULD_SEND_RESET 0x0080 +#define SOCKET_IN_SEND_QUEUE 0x0100 + +struct Socket +{ + struct MinNode node; + + void *sig_task; + ULONG socket; + + UBYTE stream_id; + UBYTE pad1; + + UWORD flags; + + struct A314_IORequest *pending_connect; + struct A314_IORequest *pending_read; + struct A314_IORequest *pending_write; + + struct Socket *next_in_send_queue; + UWORD send_queue_required_length; + + // Data that is received on the stream, but the application didn't read yet. + struct QueuedData *rq_head; + struct QueuedData *rq_tail; +}; + +extern struct Socket *send_queue_head; +extern struct Socket *send_queue_tail; + +extern void init_sockets(); + +extern struct Socket *create_socket(struct Task *task, ULONG id); +extern void delete_socket(struct Socket *s); + +extern struct Socket *find_socket(void *sig_task, ULONG socket); +extern struct Socket *find_socket_by_stream_id(UBYTE stream_id); + +extern void add_to_send_queue(struct Socket *s, UWORD required_length); +extern void remove_from_send_queue(struct Socket *s); diff --git a/a314/a314device/startup.c b/a314/a314device/startup.c new file mode 100644 index 0000000..cce7a13 --- /dev/null +++ b/a314/a314device/startup.c @@ -0,0 +1,108 @@ +#include +#include +#include +#include + +#include + +#include "a314.h" +#include "device.h" +#include "protocol.h" +#include "startup.h" +#include "fix_mem_region.h" +#include "debug.h" + +#define TASK_PRIORITY 80 +#define TASK_STACK_SIZE 1024 + +struct MsgPort task_mp; +struct Task *task; +struct ComArea *ca; + +struct Interrupt ports_interrupt; + +extern void task_main(); +extern void init_sockets(); +extern void IntServer(); + +void NewList(struct List *l) +{ + l->lh_Head = (struct Node *)&(l->lh_Tail); + l->lh_Tail = NULL; + l->lh_TailPred = (struct Node *)&(l->lh_Head); +} + +static struct Task *create_task(char *name, long priority, char *initialPC, unsigned long stacksize) +{ + char *stack = AllocMem(stacksize, MEMF_CLEAR); + if (stack == NULL) + return NULL; + + struct Task *tc = AllocMem(sizeof(struct Task), MEMF_CLEAR | MEMF_PUBLIC); + if (tc == NULL) + { + FreeMem(stack, stacksize); + return NULL; + } + + tc->tc_Node.ln_Type = NT_TASK; + tc->tc_Node.ln_Pri = priority; + tc->tc_Node.ln_Name = name; + tc->tc_SPLower = (APTR)stack; + tc->tc_SPUpper = (APTR)(stack + stacksize); + tc->tc_SPReg = (APTR)(stack + stacksize); + + AddTask(tc, initialPC, 0); + return tc; +} + +static void init_message_port() +{ + task_mp.mp_Node.ln_Name = device_name; + task_mp.mp_Node.ln_Pri = 0; + task_mp.mp_Node.ln_Type = NT_MSGPORT; + task_mp.mp_Flags = PA_SIGNAL; + task_mp.mp_SigBit = SIGB_MSGPORT; + task_mp.mp_SigTask = task; + NewList(&(task_mp.mp_MsgList)); +} + +static void add_interrupt_handler() +{ + ports_interrupt.is_Node.ln_Type = NT_INTERRUPT; + ports_interrupt.is_Node.ln_Pri = 0; + ports_interrupt.is_Node.ln_Name = device_name; + ports_interrupt.is_Data = (APTR)task; + ports_interrupt.is_Code = IntServer; + + AddIntServer(INTB_PORTS, &ports_interrupt); +} + +BOOL task_start() +{ + if (!fix_memory()) + return FALSE; + + ca = (struct ComArea *)COM_AREA_BASE; + + task = create_task(device_name, TASK_PRIORITY, (void *)task_main, TASK_STACK_SIZE); + if (task == NULL) + { + debug_printf("Unable to create task\n"); + return FALSE; + } + + init_message_port(); + init_sockets(); + + ca->a_enable = 0; + unsigned char discard_value = ca->a_events; + + ca->r_events = R_EVENT_STARTED; + + add_interrupt_handler(); + + ca->a_enable = A_EVENT_R2A_TAIL; + + return TRUE; +} diff --git a/a314/a314device/startup.h b/a314/a314device/startup.h new file mode 100644 index 0000000..933598f --- /dev/null +++ b/a314/a314device/startup.h @@ -0,0 +1,14 @@ +#include +#include +#include + +#define SIGB_INT 14 +#define SIGB_MSGPORT 15 + +#define SIGF_INT (1 << SIGB_INT) +#define SIGF_MSGPORT (1 << SIGB_MSGPORT) + +extern struct Task *task; +extern struct MsgPort task_mp; + +extern BOOL task_start(); diff --git a/emulator.c b/emulator.c index e497b59..9b2b497 100644 --- a/emulator.c +++ b/emulator.c @@ -19,6 +19,7 @@ Copyright 2020 Claude Schwartz #include #include "Gayle.h" +#include "a314/a314.h" #include "ide.h" #include "m68k.h" #include "main.h" @@ -177,6 +178,14 @@ int main(int argc, char *argv[]) { } } +#if A314_ENABLED + int err = a314_init(); + if (err < 0) { + printf("Unable to initialize A314 emulation\n"); + return -1; + } +#endif + sched_setscheduler(0, SCHED_FIFO, &priority); mlockall(MCL_CURRENT); // lock in memory to keep us from paging out @@ -320,6 +329,10 @@ int main(int argc, char *argv[]) { usleep(1); */ +#if A314_ENABLED + a314_process_events(); +#endif + if (GET_GPIO(1) == 0) { srdata = read_reg(); m68k_set_irq((srdata >> 13) & 0xff); @@ -421,6 +434,12 @@ unsigned int m68k_read_memory_8(unsigned int address) { } } +#if A314_ENABLED + if (address >= A314_COM_AREA_BASE && address < A314_COM_AREA_BASE + A314_COM_AREA_SIZE) { + return a314_read_memory_8(address - A314_COM_AREA_BASE); + } +#endif + address &= 0xFFFFFF; // if (address < 0xffffff) { return read8((uint32_t)address); @@ -446,6 +465,12 @@ unsigned int m68k_read_memory_16(unsigned int address) { } } +#if A314_ENABLED + if (address >= A314_COM_AREA_BASE && address < A314_COM_AREA_BASE + A314_COM_AREA_SIZE) { + return a314_read_memory_16(address - A314_COM_AREA_BASE); + } +#endif + // if (address < 0xffffff) { address &= 0xFFFFFF; return (unsigned int)read16((uint32_t)address); @@ -471,6 +496,12 @@ unsigned int m68k_read_memory_32(unsigned int address) { } } +#if A314_ENABLED + if (address >= A314_COM_AREA_BASE && address < A314_COM_AREA_BASE + A314_COM_AREA_SIZE) { + return a314_read_memory_32(address - A314_COM_AREA_BASE); + } +#endif + // if (address < 0xffffff) { address &= 0xFFFFFF; uint16_t a = read16(address); @@ -497,6 +528,14 @@ void m68k_write_memory_8(unsigned int address, unsigned int value) { return; } } + +#if A314_ENABLED + if (address >= A314_COM_AREA_BASE && address < A314_COM_AREA_BASE + A314_COM_AREA_SIZE) { + a314_write_memory_8(address - A314_COM_AREA_BASE, value); + return; + } +#endif + /* if (address == 0xbfe001) { ovl = (value & (1 << 0)); @@ -525,6 +564,13 @@ void m68k_write_memory_16(unsigned int address, unsigned int value) { } } +#if A314_ENABLED + if (address >= A314_COM_AREA_BASE && address < A314_COM_AREA_BASE + A314_COM_AREA_SIZE) { + a314_write_memory_16(address - A314_COM_AREA_BASE, value); + return; + } +#endif + // if (address < 0xffffff) { address &= 0xFFFFFF; write16((uint32_t)address, value); @@ -545,6 +591,13 @@ void m68k_write_memory_32(unsigned int address, unsigned int value) { } } +#if A314_ENABLED + if (address >= A314_COM_AREA_BASE && address < A314_COM_AREA_BASE + A314_COM_AREA_SIZE) { + a314_write_memory_32(address - A314_COM_AREA_BASE, value); + return; + } +#endif + // if (address < 0xffffff) { address &= 0xFFFFFF; write16(address, value >> 16); -- 2.39.2