text:
#include <exec/types.h>

#include <proto/exec.h>

#include "a314.h"
#include "device.h"
#include "debug.h"

#define SysBase (*(struct ExecBase **)4)

#define MEM_SIZE	65536
#define MEM_START	(512 + 8)

struct MyMemChunk
{
	struct MyMemChunk *next;
	USHORT address;
	USHORT length;
};

void init_memory_allocator(struct A314Device *dev)
{
	struct MyMemChunk *mc = (struct MyMemChunk *)AllocMem(sizeof(struct MyMemChunk), 0);
	mc->next = NULL;
	mc->address = MEM_START;
	mc->length = MEM_SIZE - MEM_START;
	dev->first_chunk = mc;
}

ULONG a314base_translate_address(__reg("a6") struct A314Device *dev, __reg("a0") void *address)
{
	dbg_trace("Enter: a314base_translate_address");

	// It is an error if the application calls this routine.
	return INVALID_A314_ADDRESS;
}

ULONG a314base_alloc_mem(__reg("a6") struct A314Device *dev, __reg("d0") ULONG length)
{
	dbg_trace("Enter: a314base_alloc_mem, length=$l", length);

	length = (length + 3) & ~3;

	Forbid();

	ULONG address = INVALID_A314_ADDRESS;
	struct MyMemChunk *prev = NULL;
	struct MyMemChunk *mc = dev->first_chunk;
	while (mc)
	{
		if (mc->length >= length)
		{
			address = mc->address;
			if (mc->length == length)
			{
				if (prev)
					prev->next = mc->next;
				else
					dev->first_chunk = mc->next;
				FreeMem(mc, sizeof(struct MyMemChunk));
			}
			else
			{
				mc->address += length;
				mc->length -= length;
			}
			break;
		}
		prev = mc;
		mc = mc->next;
	}

	Permit();

	dbg_trace("Leave: a314base_alloc_mem, address=$l", address);
	return address;
}

void a314base_free_mem(__reg("a6") struct A314Device *dev, __reg("d0") ULONG address, __reg("d1") ULONG length)
{
	dbg_trace("Enter: a314base_free_mem, address=$l, length=$l", address, length);

	length = (length + 3) & ~3;

	Forbid();

	struct MyMemChunk *prev = NULL;
	struct MyMemChunk *succ = dev->first_chunk;

	while (succ)
	{
		if (succ->address > address)
			break;
		prev = succ;
		succ = succ->next;
	}

	if (prev && prev->address + prev->length == address)
	{
		prev->length += length;

		if (succ && prev->address + prev->length == succ->address)
		{
			prev->length += succ->length;
			prev->next = succ->next;
			FreeMem(succ, sizeof(struct MyMemChunk));
		}
	}
	else if (succ && address + length == succ->address)
	{
		succ->address = address;
		succ->length += length;
	}
	else
	{
		struct MyMemChunk *mc = (struct MyMemChunk *)AllocMem(sizeof(struct MyMemChunk), 0);
		mc->next = succ;
		mc->address = address;
		mc->length = length;
		if (prev)
			prev->next = mc;
		else
			dev->first_chunk = mc;
	}

	Permit();
}


text:
#ifndef DEVICES_A314_H
#define DEVICES_A314_H

#include <exec/io.h>

#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)

#define INVALID_A314_ADDRESS		((ULONG)-1)

struct A314_IORequest
{
	struct IORequest a314_Request;
	ULONG		a314_Socket;
	STRPTR		a314_Buffer;
	WORD		a314_Length;
};

#endif

repo_language: C

mime_type: text/xml


text:
#include <exec/types.h>

// Addresses to variables in CMEM.
#define CMEM_CFG_ADDRESS        11
#define R_EVENTS_ADDRESS	12
#define R_ENABLE_ADDRESS	13
#define A_EVENTS_ADDRESS	14
#define A_ENABLE_ADDRESS	15

// 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_BASE_ADDRESS	4

// Events that are communicated from Raspberry to Amiga.
#define A_EVENT_R2A_TAIL	1
#define A_EVENT_A2R_HEAD	2

extern void write_cp_nibble(int index, UBYTE value);
extern UBYTE read_cp_nibble(int index);

extern void write_cmem_safe(int index, UBYTE value);
extern UBYTE read_cmem_safe(int index);

extern void write_base_address(ULONG ba);

extern ULONG read_fw_flags();


text:
#ifndef PROTO_A314_H
#define PROTO_A314_H

#include <exec/types.h>

extern struct Library *A314Base;

ULONG __TranslateAddressA314(__reg("a6") void *, __reg("a0") void *)="\tjsr\t-42(a6)";
#define TranslateAddressA314(address) __TranslateAddressA314(A314Base, address)

ULONG __AllocMemA314(__reg("a6") void *, __reg("d0") ULONG)="\tjsr\t-48(a6)";
#define AllocMemA314(length) __AllocMemA314(A314Base, length)

void __FreeMemA314(__reg("a6") void *, __reg("d0") ULONG, __reg("d1") ULONG)="\tjsr\t-54(a6)";
#define FreeMemA314(address, length) __FreeMemA314(A314Base, address, length)

void __WriteMemA314(__reg("a6") void *, __reg("d0") ULONG, __reg("a0") void *, __reg("d1") ULONG)="\tjsr\t-60(a6)";
#define WriteMemA314(address, src, length) __WriteMemA314(A314Base, address, src, length)

void __ReadMemA314(__reg("a6") void *, __reg("a0") void *, __reg("d0") ULONG, __reg("d1") ULONG)="\tjsr\t-66(a6)";
#define ReadMemA314(dst, address, length) __ReadMemA314(A314Base, dst, address, length)

#endif


text:
    XDEF    _debug_process_seglist
    XREF    _debug_process_run

RTC_MATCHWORD:	equ	$4afc
RTF_AUTOINIT:	equ	(1<<7)
RTF_COLDSTART:	equ	(1<<0)
NT_DEVICE:	equ	3
VERSION:	equ	1
PRIORITY:	equ	-3

		section	code,code

		moveq	#-1,d0
		rts

romtag:
		dc.w	RTC_MATCHWORD
		dc.l	romtag
		dc.l	endcode
		dc.b	RTF_AUTOINIT | RTF_COLDSTART
		dc.b	VERSION
		dc.b	NT_DEVICE
		dc.b	PRIORITY
		dc.l	_device_name
		dc.l	_id_string
		dc.l	_auto_init_tables
endcode:

        cnop    0,4

        dc.l    16
_debug_process_seglist:
        dc.l    0
        jmp     _debug_process_run


text:
	XDEF	_IntServer
	CODE

CLOCK_PORT	equ	$d80001

SIGB_INT	equ	14
SIGF_INT	equ	(1<<SIGB_INT)

		; a1 points to driver task
_IntServer:	lea.l	CLOCK_PORT+4,a5

		move.b	(a5),d0		; read cp irq
		bne.b	should_signal

		moveq	#0,d0
		rts

should_signal:
		move.b	#1,(a5)		; clear cp irq

		move.l	$4.w,a6
		move.l	#SIGF_INT,d0
		; a1 = pointer to driver task
		jsr	-324(a6)	; Signal()

		moveq	#1,d0
		rts


text:
#include <exec/types.h>
#include <exec/memory.h>
#include <exec/tasks.h>
#include <hardware/intbits.h>

#include <proto/exec.h>

#include <string.h>

#include "a314.h"
#include "device.h"
#include "protocol.h"
#include "startup.h"
#include "debug.h"
#include "pi_if.h"
#include "fix_mem_region.h"
#include "cmem.h"

#define SysBase (*(struct ExecBase **)4)

extern void IntServer();

static void add_interrupt_handlers(struct A314Device *dev)
{
	memset(&dev->vertb_interrupt, 0, sizeof(struct Interrupt));
	dev->vertb_interrupt.is_Node.ln_Type = NT_INTERRUPT;
	dev->vertb_interrupt.is_Node.ln_Pri = -60;
	dev->vertb_interrupt.is_Node.ln_Name = device_name;
	dev->vertb_interrupt.is_Data = (APTR)&dev->task;
	dev->vertb_interrupt.is_Code = IntServer;

	AddIntServer(INTB_VERTB, &dev->vertb_interrupt);

	memset(&dev->ports_interrupt, 0, sizeof(struct Interrupt));
	dev->ports_interrupt.is_Node.ln_Type = NT_INTERRUPT;
	dev->ports_interrupt.is_Node.ln_Pri = 0;
	dev->ports_interrupt.is_Node.ln_Name = device_name;
	dev->ports_interrupt.is_Data = (APTR)&dev->task;
	dev->ports_interrupt.is_Code = IntServer;

	AddIntServer(INTB_PORTS, &dev->ports_interrupt);
}

static void detect_and_write_address_swap()
{
	// Only looking at VPOSR Agnus identification at this point.
	// Could add more dynamic identification of address mapping.

	UWORD vposr = *(UWORD *)0xdff004;
	UWORD agnus = (vposr & 0x7f00) >> 8;

	UBYTE swap = (agnus == 0x00 || agnus == 0x10) ? 0x1 : 0x0;

	write_cmem_safe(CMEM_CFG_ADDRESS, swap);
}

int probe_pi_interface(struct A314Device *dev)
{
	dev->fw_flags = read_fw_flags();

	if (!fix_memory(dev))
		return FALSE;

	detect_and_write_address_swap();

	dev->ca = (struct ComArea *)AllocMem(sizeof(struct ComArea), MEMF_A314 | MEMF_CLEAR);
	if (dev->ca == NULL)
	{
		dbg_error("Unable to allocate A314 memory for com area\n");
		return FALSE;
	}

	return TRUE;
}

void setup_pi_interface(struct A314Device *dev)
{
	write_cmem_safe(A_ENABLE_ADDRESS, 0);
	read_cmem_safe(A_EVENTS_ADDRESS);

	write_base_address(a314base_translate_address(dev, dev->ca));

	write_cmem_safe(R_EVENTS_ADDRESS, R_EVENT_BASE_ADDRESS);

	add_interrupt_handlers(dev);

	write_cmem_safe(A_ENABLE_ADDRESS, A_EVENT_R2A_TAIL);
}

void read_from_r2a(struct A314Device *dev, UBYTE *dst, UBYTE offset, int length)
{
	UBYTE *r2a_buffer = dev->ca->r2a_buffer;
	for (int i = 0; i < length; i++)
		*dst++ = r2a_buffer[offset++];
}

void write_to_a2r(struct A314Device *dev, UBYTE type, UBYTE stream_id, UBYTE length, UBYTE *data)
{
	struct ComArea *ca = dev->ca;
	UBYTE index = ca->cap.a2r_tail;
	UBYTE *a2r_buffer = ca->a2r_buffer;
	a2r_buffer[index++] = length;
	a2r_buffer[index++] = type;
	a2r_buffer[index++] = stream_id;
	for (int i = 0; i < (int)length; i++)
		a2r_buffer[index++] = *data++;
	ca->cap.a2r_tail = index;
}


text:
// Interrupt bits.
#define IRQ_R2A_TAIL		0x01
#define IRQ_A2R_HEAD		0x02
#define IRQ_A2R_TAIL		0x04
#define IRQ_R2A_HEAD		0x08
#define IRQ_BASE_ADDRESS	0x10


text:
#include <proto/exec.h>

#include "sockets.h"
#include "debug.h"

#define SysBase (*(struct ExecBase **)4)

extern void NewList(struct List *l);

void init_sockets(struct A314Device *dev)
{
	NewList(&dev->active_sockets);
	dev->send_queue_head = NULL;
	dev->send_queue_tail = NULL;
	dev->next_stream_id = 1;
}

struct Socket *find_socket(struct A314Device *dev, void *sig_task, ULONG socket)
{
	dbg_trace("Enter: find_socket, socket=$l", socket);

	for (struct Node *node = dev->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(struct A314Device *dev, UBYTE stream_id)
{
	dbg_trace("Enter: find_socket_by_stream_id, stream_id=$b", stream_id);

	for (struct Node *node = dev->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(struct A314Device *dev)
{
	dbg_trace("Enter: allocate_stream_id");

	for (int i = 0; i < 256; i++)
	{
		UBYTE stream_id = dev->next_stream_id;
		dev->next_stream_id += 2;
		if (find_socket_by_stream_id(dev, stream_id) == NULL)
		{
			dbg_trace("Leave: allocate_stream_id, stream_id=$b", stream_id);
			return stream_id;
		}
	}

	// BUG: What to do here? This is quite bad.
	// Need to propagate this error condition to the calling application.
	dbg_error("There were no free stream ids, and there is no code to handle this case");
	return dev->next_stream_id;
}

static void free_stream_id(struct A314Device *dev, UBYTE stream_id)
{
	dbg_trace("Enter: free_stream_id, stream_id=$b", stream_id);
	// Currently do nothing.
	// Could speed up allocate_stream_id using a bitmap?
}

struct Socket *create_socket(struct A314Device *dev, struct Task *task, ULONG id)
{
	dbg_trace("Enter: create_socket, socket_id=$l", 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(dev);
	AddTail(&dev->active_sockets, (struct Node *)s);
	return s;
}

void delete_socket(struct A314Device *dev, struct Socket *s)
{
	dbg_trace("Enter: delete_socket, socket_id=$l", s->socket);

	Remove((struct Node *)s);
	free_stream_id(dev, s->stream_id);
	FreeMem(s, sizeof(struct Socket));
}

void add_to_send_queue(struct A314Device *dev, struct Socket *s, UWORD required_length)
{
	dbg_trace("Enter: add_to_send_queue, socket_id=$l, required_length=$w", s->socket, required_length);

	s->send_queue_required_length = required_length;
	s->next_in_send_queue = NULL;

	if (dev->send_queue_head == NULL)
		dev->send_queue_head = s;
	else
		dev->send_queue_tail->next_in_send_queue = s;
	dev->send_queue_tail = s;

	s->flags |= SOCKET_IN_SEND_QUEUE;
}

void remove_from_send_queue(struct A314Device *dev, struct Socket *s)
{
	dbg_trace("Enter: remove_from_send_queue, socket_id=$l", s->socket);

	if (s->flags & SOCKET_IN_SEND_QUEUE)
	{
		if (dev->send_queue_head == s)
		{
			dev->send_queue_head = s->next_in_send_queue;
			if (dev->send_queue_head == NULL)
				dev->send_queue_tail = NULL;
		}
		else
		{
			struct Socket *curr = dev->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 (dev->send_queue_tail == s)
				dev->send_queue_tail = curr;
		}

		s->next_in_send_queue = NULL;
		s->flags &= ~SOCKET_IN_SEND_QUEUE;
	}
}


text:
#ifndef __A314_PROTOCOL_H
#define __A314_PROTOCOL_H

#include <exec/types.h>

// 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

#pragma pack(push, 1)
struct PktHdr
{
	UBYTE length;
	UBYTE type;
	UBYTE stream_id;
};
#pragma pack(pop)

// TODO: These constants should be the same for TD/FE and CP.
// Must change on both sides at the same time.
#if defined(MODEL_TD)
#define CAP_BASE 0
#define A2R_BASE 4
#define R2A_BASE 260

struct ComAreaPtrs
{
	volatile UBYTE a2r_tail;
	volatile UBYTE r2a_head;
	volatile UBYTE r2a_tail;
	volatile UBYTE a2r_head;
};

// The communication area, used to create the physical channel.
struct ComArea
{
	struct ComAreaPtrs cap;
	UBYTE a2r_buffer[256];
	UBYTE r2a_buffer[256];
};
#elif defined(MODEL_FE)
#define HANDSHAKE_BASE 0
#define CAP_BASE 4
#define A2R_BASE 8
#define R2A_BASE 264

struct ComAreaPtrs
{
	volatile UBYTE a2r_tail;
	volatile UBYTE r2a_head;
	volatile UBYTE r2a_tail;
	volatile UBYTE a2r_head;
};

struct ComArea
{
	volatile UWORD lock_word;
	volatile UWORD interrupt;
	struct ComAreaPtrs cap;
	UBYTE a2r_buffer[256];
	UBYTE r2a_buffer[256];
};
#elif defined(MODEL_CP)
#define A2R_BASE 0
#define R2A_BASE 256
#define CAP_BASE 512

struct ComAreaPtrs
{
	volatile UBYTE r2a_tail;		// Written by Pi.
	volatile UBYTE a2r_head;		// Written by Pi.
	volatile UBYTE a2r_tail;		// Written by Amiga.
	volatile UBYTE r2a_head;		// Written by Amiga.
};
#endif

#endif /* __A314_PROTOCOL_H */


text:
#include <exec/types.h>
#include <exec/execbase.h>
#include <exec/memory.h>
#include <graphics/gfxbase.h>
#include <proto/exec.h>
#include <proto/graphics.h>

#include <string.h>

#include "a314.h"
#include "fix_mem_region.h"
#include "device.h"
#include "pi_if.h"

#if defined(MODEL_TD)
#include "cmem.h"

extern ULONG check_a314_mapping(__reg("a0") void *address);
#endif

#define SysBase (*(struct ExecBase **)4)

#define FW_FLAG_AUTODETECT	1
#define FW_FLAG_A600		2

#define QUARTER_MB (256*1024)
#define HALF_MB (512*1024)
#define ONE_MB (1024*1024)
#define TWO_MB (2*1024*1024)

#define FE_BANK_ADDRESS 0x40000

struct MemChunkList
{
	struct MemChunk *first;
	struct MemChunk *last;
	ULONG free;
};

static 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;
}

static 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;
}

static BOOL overlap(struct MemHeader *mh, ULONG lower, ULONG upper)
{
	return lower < (ULONG)(mh->mh_Upper) && (ULONG)(mh->mh_Lower) < upper;
}

BOOL check_overlap_region(ULONG lower, ULONG upper)
{
	Forbid();

	for (struct Node *node = SysBase->MemList.lh_Head; node->ln_Succ != NULL; node = node->ln_Succ)
	{
		struct MemHeader *mh = (struct MemHeader *)node;
		if (overlap(mh, lower, upper))
		{
			Permit();
			return TRUE;
		}
	}

	Permit();
	return FALSE;
}

static 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;
		}
	}
}

#if defined(MODEL_TD)
static void detect_block_mapping(ULONG present_blocks, ULONG *bank_address)
{
	struct GfxBase *GfxBase = (struct GfxBase *)OpenLibrary("graphics.library", 0);
	if (!GfxBase)
		return;

	Disable();
	WaitBlit();

	UBYTE prev_regd = read_cp_nibble(13);
	write_cp_nibble(13, prev_regd | 8);

	for (ULONG block = 0; block < 32; block++)
	{
		if (present_blocks & (1UL << block))
		{
			ULONG address = block << 19;
			ULONG sram_address = check_a314_mapping((void *)address);

			if ((sram_address & 1) == 0)
			{
				BOOL match = FALSE;
				for (ULONG bank = 0; bank < 4; bank++)
				{
					if (sram_address == bank * HALF_MB)
					{
						if (bank_address[bank] != -1)
						{
							// This is not the first address that maps to this A314 block.
							// I currently don't understand how this could happen.
							// This should be logged, so that it can be investigated further.
						}

						bank_address[bank] = address;
						match = TRUE;
						break;
					}
				}

				if (!match)
				{
					// I currently don't understand how this could happen.
					// This should be logged, so that it can be investigated further.
				}
			}
		}
	}

	write_cp_nibble(13, prev_regd);
	Enable();

	CloseLibrary((struct Library *)GfxBase);
}

static void detect_block_mapping_heuristically(ULONG present_blocks, ULONG *bank_address)
{
	if ((present_blocks & 0xc) == 0xc)
	{
		bank_address[0] = ONE_MB;
		bank_address[1] = ONE_MB + HALF_MB;
	}
	else if (present_blocks & 0x01000000)
		bank_address[0] = 0xc00000;
	else if (present_blocks & 0x00000002)
		bank_address[0] = HALF_MB;
}

static ULONG get_present_blocks()
{
	ULONG present_blocks = 0;

	for (struct Node *node = SysBase->MemList.lh_Head; node->ln_Succ != NULL; node = node->ln_Succ)
	{
		struct MemHeader *mh = (struct MemHeader *)node;
		ULONG block = (ULONG)mh->mh_Lower >> 19;
		ULONG address = block << 19;
		while (address < (ULONG)mh->mh_Upper)
		{
			present_blocks |= 1UL << block;
			block++;
			address = block << 19;
		}
	}

	return present_blocks;
}

BOOL fix_memory(struct A314Device *dev)
{
	ULONG *bank_address = dev->bank_address;
	for (int i = 0; i < 4; i++)
		bank_address[i] = -1;

	dev->is_a600 = FALSE;

	Forbid();

	ULONG present_blocks = get_present_blocks();

	// Only test blocks in chip mem and slow mem ranges.
	present_blocks &= 0x0700000f; // 0b00000111000000000000000000001111

	if (dev->fw_flags & FW_FLAG_A600)
	{
		dev->is_a600 = TRUE;
		detect_block_mapping(present_blocks, bank_address);

		BOOL all_present = TRUE;
		for (int i = 0; i < 4; i++)
			if (bank_address[i] != i * HALF_MB)
				all_present = FALSE;

		if (all_present)
			mark_region_a314(0, TWO_MB);

		Permit();
		return all_present;
	}

	if (dev->fw_flags & FW_FLAG_AUTODETECT)
		detect_block_mapping(present_blocks, bank_address);
	else
		detect_block_mapping_heuristically(present_blocks, bank_address);

	if (bank_address[0] == -1 && bank_address[1] == -1)
	{
		// Didn't detect any A314 memory; the a314.device Open() should fail.
		// Should log the reason why this happened.
		Permit();
		return FALSE;
	}

	if (bank_address[0] == -1)
	{
		mark_region_a314(bank_address[1], HALF_MB);
	}
	else if (bank_address[1] == -1)
	{
		mark_region_a314(bank_address[0], HALF_MB);
	}
	else
	{
		ULONG lo = bank_address[0];
		ULONG hi = bank_address[1];
		if (lo > hi)
		{
			lo = bank_address[1];
			hi = bank_address[0];
		}

		if (lo + HALF_MB == hi)
		{
			mark_region_a314(lo, ONE_MB);
		}
		else
		{
			mark_region_a314(lo, HALF_MB);
			mark_region_a314(hi, HALF_MB);
		}
	}

	Permit();
	return TRUE;
}
#endif

#if defined(MODEL_FE)
BOOL fix_memory(struct A314Device *dev)
{
	Forbid();
	mark_region_a314(FE_BANK_ADDRESS, QUARTER_MB);
	Permit();
	return TRUE;
}
#endif

static ULONG cpu_to_a314_address(__reg("a6") struct A314Device *dev, __reg("a0") void *address)
{
#if defined(MODEL_TD)
	if (dev->is_a600)
	{
		ULONG offset = (ULONG)address;
		return offset < TWO_MB ? offset : INVALID_A314_ADDRESS;
	}

	if (dev->bank_address[0] != -1)
	{
		ULONG offset = (ULONG)address - dev->bank_address[0];
		if (offset < HALF_MB)
			return offset;
	}

	if (dev->bank_address[1] != -1)
	{
		ULONG offset = (ULONG)address - dev->bank_address[1];
		if (offset < HALF_MB)
			return HALF_MB + offset;
	}
#elif defined(MODEL_FE)
	ULONG offset = (ULONG)address - FE_BANK_ADDRESS;
	if (offset < QUARTER_MB)
		return offset;
#endif

	return INVALID_A314_ADDRESS;
}

static void *a314_to_cpu_address(__reg("a6") struct A314Device *dev, __reg("d0") ULONG address)
{
#if defined(MODEL_TD)
	ULONG bank = address >> 19;
	ULONG offset = address & ((1 << 19) - 1);
	return (void *)(dev->bank_address[bank] + offset);
#elif defined(MODEL_FE)
	return (void *)(FE_BANK_ADDRESS + address);
#endif
}

ULONG a314base_translate_address(__reg("a6") struct A314Device *dev, __reg("a0") void *address)
{
	return cpu_to_a314_address(dev, address);
}

ULONG a314base_alloc_mem(__reg("a6") struct A314Device *dev, __reg("d0") ULONG length)
{
	void *p = AllocMem(length, MEMF_A314);
	if (!p)
		return INVALID_A314_ADDRESS;
	return cpu_to_a314_address(dev, p);
}

void a314base_free_mem(__reg("a6") struct A314Device *dev, __reg("d0") ULONG address, __reg("d1") ULONG length)
{
	void *p = a314_to_cpu_address(dev, address);
	FreeMem(p, length);
}

void a314base_write_mem(__reg("a6") struct A314Device *dev, __reg("d0") ULONG address, __reg("a0") UBYTE *src, __reg("d1") ULONG length)
{
	UBYTE *dst = a314_to_cpu_address(dev, address);
	memcpy(dst, src, length);
}

void a314base_read_mem(__reg("a6") struct A314Device *dev, __reg("a0") UBYTE *dst, __reg("d0") ULONG address, __reg("d1") ULONG length)
{
	UBYTE *src = a314_to_cpu_address(dev, address);
	memcpy(dst, src, length);
}


text:
#include <exec/types.h>
#include <exec/lists.h>

#include "device.h"

// 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 void init_sockets(struct A314Device *dev);

extern struct Socket *create_socket(struct A314Device *dev, struct Task *task, ULONG id);
extern void delete_socket(struct A314Device *dev, struct Socket *s);

extern struct Socket *find_socket(struct A314Device *dev, void *sig_task, ULONG socket);
extern struct Socket *find_socket_by_stream_id(struct A314Device *dev, UBYTE stream_id);

extern void add_to_send_queue(struct A314Device *dev, struct Socket *s, UWORD required_length);
extern void remove_from_send_queue(struct A314Device *dev, struct Socket *s);


text:
#include <exec/types.h>

#include "device.h"

extern void init_memory_allocator(struct A314Device *dev);


text:
#include <exec/types.h>
#include <exec/memory.h>
#include <exec/tasks.h>

#include <proto/exec.h>

#include <string.h>

#include "a314.h"
#include "device.h"
#include "protocol.h"
#include "startup.h"
#include "debug.h"
#include "pi_if.h"

#define SysBase (*(struct ExecBase **)4)

#define TASK_PRIORITY 80
#define TASK_STACK_SIZE 1024

extern void task_main();
extern void init_sockets(struct A314Device *dev);

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 BOOL setup_task(struct A314Device *dev)
{
	char *stack = AllocMem(TASK_STACK_SIZE, MEMF_CLEAR);
	if (stack == NULL)
		return FALSE;

	struct Task *task = &dev->task;
	memset(task, 0, sizeof(struct Task));
	task->tc_Node.ln_Type = NT_TASK;
	task->tc_Node.ln_Pri = TASK_PRIORITY;
	task->tc_Node.ln_Name = device_name;
	task->tc_SPLower = (APTR)stack;
	task->tc_SPUpper = (APTR)(stack + TASK_STACK_SIZE);
	task->tc_SPReg = (APTR)(stack + TASK_STACK_SIZE);
	task->tc_UserData = (void *)dev;

	return TRUE;
}

static void init_message_port(struct A314Device *dev)
{
	struct MsgPort *mp = &dev->task_mp;
	memset(mp, 0, sizeof(struct MsgPort));
	mp->mp_Node.ln_Name = device_name;
	mp->mp_Node.ln_Pri = 0;
	mp->mp_Node.ln_Type = NT_MSGPORT;
	mp->mp_Flags = PA_SIGNAL;
	mp->mp_SigBit = SIGB_MSGPORT;
	mp->mp_SigTask = &dev->task;
	NewList(&(mp->mp_MsgList));
}

BOOL task_start(struct A314Device *dev)
{
	if (!setup_task(dev))
	{
		dbg_error("Unable to create task stack\n");
		return FALSE;
	}

	if (!probe_pi_interface(dev))
	{
		FreeMem(dev->task.tc_SPLower, TASK_STACK_SIZE);
		return FALSE;
	}

	init_message_port(dev);
	init_sockets(dev);

	setup_pi_interface(dev);

	AddTask(&dev->task, (void *)task_main, 0);

	return TRUE;
}


text:
	XDEF	_IntServer
	CODE

R_EVENTS_ADDRESS	equ	12
R_ENABLE_ADDRESS	equ	13
A_EVENTS_ADDRESS	equ	14
A_ENABLE_ADDRESS	equ	15

A_EVENT_R2A_TAIL        equ	1
A_EVENT_A2R_HEAD        equ	2

INTENA		equ	$dff09a
CLOCK_PORT	equ	$dc0000

SIGB_INT	equ	14
SIGF_INT	equ	(1<<SIGB_INT)

		; a1 points to driver task
_IntServer:	lea.l	CLOCK_PORT,a5

		move	#$4000,INTENA

		move.b	$37(a5),d1
		moveq	#8,d0
		or.b	d1,d0
		move.b	d0,$37(a5)	; Switch to CMEM
		move.b	$3b(a5),d0	; A_EVENTS
		and.b	$3f(a5),d0	; A_ENABLE
		and.b	#$f,d0
		bne.s	should_signal

		move.b	d1,$37(a5)	; Switch to RTC

		move	#$c000,INTENA
		moveq	#0,d0
		rts

should_signal:
		move.b	#0,$3f(a5)	; A_ENABLE
		move.b	d1,$37(a5)	; Switch to RTC

		move	#$c000,INTENA

		move.l	$4.w,a6
		move.l	#SIGF_INT,d0
		; a1 = pointer to driver task
		jsr	-324(a6)	; Signal()

		moveq	#0,d0
		rts


text:
#include <exec/types.h>

#include "device.h"

extern BOOL fix_memory(struct A314Device *dev);

extern BOOL check_overlap_region(ULONG lower, ULONG upper);


text:
	XDEF	_IntServer
	CODE

		; these offsets must be in sync with A314Device struct
CA_OFFSET	equ	40
TASK_OFFSET	equ	44

SIGB_INT	equ	14
SIGF_INT	equ	(1<<SIGB_INT)

		; a1 points to device struct
_IntServer:	move.l	CA_OFFSET(a1),a5	; com area pointer

		move	2(a5),d0
		and	#3,d0
		beq.s	spurious

		move	#$0300,2(a5)		; clear enable bits

		move.l	$4.w,a6
		move.l	#SIGF_INT,d0
		lea.l	TASK_OFFSET(a1),a1	; pointer to driver task
		jsr	-324(a6)		; Signal()

spurious:	moveq	#0,d0
		rts


text:
#include <exec/types.h>
#include <exec/tasks.h>
#include <exec/ports.h>

#define SIGB_INT 14
#define SIGB_MSGPORT 15

#define SIGF_INT (1 << SIGB_INT)
#define SIGF_MSGPORT (1 << SIGB_MSGPORT)

extern BOOL task_start(struct A314Device *dev);


text:
#include <exec/types.h>
#include <exec/memory.h>
#include <exec/tasks.h>
#include <hardware/intbits.h>

#include <proto/exec.h>

#include <string.h>

#include "a314.h"
#include "device.h"
#include "protocol.h"
#include "startup.h"
#include "debug.h"
#include "pi_if.h"
#include "fix_mem_region.h"
#include "handshake_fe.h"

#define SysBase (*(struct ExecBase **)4)

// TODO: Currently these values are hard coded.
// We may want to add more dynamic detection of where the
// shared memory ends up in the memory space.
#define FE_BANK_ADDRESS		0x40000
#define QUARTER_MB		(256*1024)

extern void IntServer();

static void add_interrupt_handlers(struct A314Device *dev)
{
	memset(&dev->vertb_interrupt, 0, sizeof(struct Interrupt));
	dev->vertb_interrupt.is_Node.ln_Type = NT_INTERRUPT;
	dev->vertb_interrupt.is_Node.ln_Pri = -60;
	dev->vertb_interrupt.is_Node.ln_Name = device_name;
	dev->vertb_interrupt.is_Data = (APTR)dev;
	dev->vertb_interrupt.is_Code = IntServer;

	AddIntServer(INTB_VERTB, &dev->vertb_interrupt);

	memset(&dev->ports_interrupt, 0, sizeof(struct Interrupt));
	dev->ports_interrupt.is_Node.ln_Type = NT_INTERRUPT;
	dev->ports_interrupt.is_Node.ln_Pri = 0;
	dev->ports_interrupt.is_Node.ln_Name = device_name;
	dev->ports_interrupt.is_Data = (APTR)dev;
	dev->ports_interrupt.is_Code = IntServer;

	AddIntServer(INTB_PORTS, &dev->ports_interrupt);
}

static BOOL detect_board_present(ULONG address)
{
	Disable();

	volatile UWORD *p = (volatile UWORD *)address;
	UWORD old_value = *p;

	*p = 0x413a;
	*p = 0xfeed;
	*p = 0xc0de;
	*p = 0xa314;
	*p = 0xa314;

	UWORD new_value = *p;

	*p = old_value;

	Enable();

	return new_value == 0x413a;
}

static void set_com_area_base_address(struct A314Device *dev)
{
	Disable();
	volatile UWORD *p = &(dev->ca->lock_word);
	*p = 0x413a;
	*p = 0xfeed;
	*p = 0xc0de;
	*p = 0xa314;
	*p = 0xa314;
	Enable();
}

int probe_pi_interface(struct A314Device *dev)
{
	if (!check_overlap_region(FE_BANK_ADDRESS, FE_BANK_ADDRESS + QUARTER_MB))
		return FALSE;

	if (!detect_board_present(FE_BANK_ADDRESS))
		return FALSE;

	if (!fix_memory(dev))
		return FALSE;

	dev->ca = (struct ComArea *)AllocMem(sizeof(struct ComArea), MEMF_A314 | MEMF_CLEAR);
	if (dev->ca == NULL)
	{
		dbg_error("Unable to allocate A314 memory for com area\n");
		return FALSE;
	}

	return TRUE;
}

void setup_pi_interface(struct A314Device *dev)
{
	set_com_area_base_address(dev);

	// Clear all enable and event bits, for both sides.
	dev->ca->interrupt = 0x1f1f;

	// Set BASE_ADDRESS interrupt bit.
	dev->ca->interrupt = 0x8000 | IRQ_BASE_ADDRESS;

	add_interrupt_handlers(dev);

	// Enable R2A_TAIL interrupt.
	dev->ca->interrupt = 0x8000 | (IRQ_R2A_TAIL << 8);
}

void read_from_r2a(struct A314Device *dev, UBYTE *dst, UBYTE offset, int length)
{
	UBYTE *r2a_buffer = dev->ca->r2a_buffer;
	for (int i = 0; i < length; i++)
		*dst++ = r2a_buffer[offset++];
}

void write_to_a2r(struct A314Device *dev, UBYTE type, UBYTE stream_id, UBYTE length, UBYTE *data)
{
	struct ComArea *ca = dev->ca;
	UBYTE index = ca->cap.a2r_tail;
	UBYTE *a2r_buffer = ca->a2r_buffer;
	a2r_buffer[index++] = length;
	a2r_buffer[index++] = type;
	a2r_buffer[index++] = stream_id;
	for (int i = 0; i < (int)length; i++)
		a2r_buffer[index++] = *data++;
	ca->cap.a2r_tail = index;
}


text:
#!/usr/bin/python
# -*- coding: utf-8 -*-

from distutils.core import setup, Extension

setup(  name            = "bpls2gif",
        version         = "1.0",
        description     = "Encode Amiga bitplanes as GIF file",
        author          = "Niklas Ekström",
        url             = "http://github.com/niklasekstrom/a314",
        ext_modules     = [Extension("bpls2gif", ["bpls2gif.c"])]
)


text:
/*
 * Copyright (c) 2018 Niklas Ekström
 */

#include <Python.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define COLORS 8
#define CHUNKLEN 255

typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;

struct _Node
{
	ushort children[COLORS];
};
typedef struct _Node Node;

static Node *nodes;

static void init_node(ushort code)
{
	Node *node = &nodes[code];
	for (int i = 0; i < COLORS; i++)
		node->children[i] = 0xffff;
}

static uchar *buffer;
static int capacity;
static int length;

static void append_byte(uchar b)
{
	if (length == capacity)
	{
		capacity *= 2;
		buffer = realloc(buffer, capacity);
	}

	buffer[length++] = b;
}

static int chunk_left = 0;

static void flush_byte(uchar b)
{
	if (!chunk_left)
	{
		append_byte(CHUNKLEN);
		chunk_left = CHUNKLEN;
	}

	append_byte(b);
	chunk_left--;
}

#define c2p(c) &nodes[c]
#define p2c(p) (((uint)p - (uint)nodes) / sizeof(Node))

static const uint clear_code = 256;
static const uint eoi_code = 257;
static uint next_code = 258;

static int clen;

static uint buf;
static int blen;

static Node *prefix;

static void begin_encode(void)
{
	chunk_left = 0;

	for (int i = 0; i < COLORS; i++)
		init_node(i);

	next_code = 258;
	clen = 9;

	buf = 0;
	blen = 0;

	buf |= clear_code << blen;
	blen += clen;

	prefix = NULL;
}

static void end_encode(void)
{
	buf |= p2c(prefix) << blen;
	blen += clen;

	buf |= eoi_code << blen;
	blen += clen;

	while (blen > 0)
	{
		flush_byte(buf);
		buf >>= 8;
		blen -= 8;
	}

	if (chunk_left)
	{
		int used = CHUNKLEN - chunk_left;
		buffer[length - used - 1] = used;
	}
}

static void encode(uchar *pixels, int count)
{
	uint *cp = (uint *)pixels;
	uint *end = (uint *)(pixels + (count / 2));

	uint ibuf = *cp++;
	int ilen = 8;

	if (!prefix)
	{
		uchar K = ibuf & 0xf;
		ibuf >>= 4;
		ilen--;
		prefix = &nodes[K];
	}

	while (cp < end || ilen)
	{
		if (!ilen)
		{
			ibuf = *cp++;
			ilen = 8;
		}

		uchar K = ibuf & 0xf;
		ibuf >>= 4;
		ilen--;

		if (prefix->children[K] != 0xffff)
			prefix = c2p(prefix->children[K]);
		else
		{
			buf |= p2c(prefix) << blen;
			blen += clen;

			init_node(next_code);
			prefix->children[K] = next_code;

			if (next_code == (1 << clen))
				clen++;
			next_code++;

			prefix = &nodes[K];

			if (next_code == 4095)
			{
				buf |= clear_code << blen;
				blen += clen;

				for (int i = 0; i < COLORS; i++)
					init_node(i);

				clen = 9;
				next_code = 258;
			}

			while (blen >= 8)
			{
				flush_byte(buf);
				buf >>= 8;
				blen -= 8;
			}
		}
	}
}

#define W 640
#define H 256
#define BLOCK_HEIGHT 32
#define BPL_SIZE (W * H / 8)
#define BPL_COUNT 3

static uchar pal[8*3] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

static void copy_bpls_to_pixels(uchar *pixels, uchar *bpls, int b)
{
	memset(pixels, 0, W * BLOCK_HEIGHT / 2);

	bpls += b * BLOCK_HEIGHT * W / 8;
	for (int i = 0; i < BPL_COUNT; i++)
	{
		uchar *src = bpls;
		uint *dst = (uint *)pixels;

		int shift = 28 - (7 - i);

		for (int j = 0; j < BLOCK_HEIGHT * W / 8; j++)
		{
			uchar x = *src++;
			uint bits = 0;
			for (int k = 0; k < 8; k++)
			{
				bits = (bits >> 4) | ((x & 0x80) << shift);
				x <<= 1;
			}
			*dst++ |= bits;
		}
		bpls += BPL_SIZE;
	}
}

static uchar *pixels;

static void write_gif(uchar *bpls)
{
	uchar *p = buffer;
	memcpy(p, "GIF89a", 6);
	p += 6;
	*((short *)p) = W;
	p += 2;
	*((short *)p) = H;
	p += 2;
	*p++ = 0xf7;
	*p++ = 0;
	*p++ = 0;
	memset(p, 0, 768);
	memcpy(p, pal, sizeof(pal));
	p += 768;
	*p++ = 0x2c;
	*((short *)p) = 0;
	p += 2;
	*((short *)p) = 0;
	p += 2;
	*((short *)p) = W;
	p += 2;
	*((short *)p) = H;
	p += 2;
	*p++ = 0;
	*p++ = 8;
	length = p - buffer;

	begin_encode();
	for (int b = 0; b < H / BLOCK_HEIGHT; b++)
	{
		copy_bpls_to_pixels(pixels, bpls, b);
		encode(pixels, W * BLOCK_HEIGHT);
	}
	end_encode();

	append_byte(0);
	append_byte(0x3b);
}

#if PY_MAJOR_VERSION > 2
#define BYTEARRAY_FORMAT "y#"
#else
#define BYTEARRAY_FORMAT "s#"
#endif

static PyObject *b2g_set_palette(PyObject *self, PyObject *args)
{
	char *buf;
	int len;

	if (!PyArg_ParseTuple(args, BYTEARRAY_FORMAT, &buf, &len))
		return NULL;

	if (len != 24) {
		PyErr_SetString(PyExc_RuntimeError, "Must be 8*3 = 24 bytes (r, g, b).");
		return NULL;
	}

	memcpy(pal, buf, 24);

	Py_INCREF(Py_None);
	return Py_None;
}

static PyObject *b2g_encode(PyObject *self, PyObject *args)
{
	char *buf;
	int len;

	if (!PyArg_ParseTuple(args, BYTEARRAY_FORMAT, &buf, &len))
		return NULL;

	if (len != 80*256*3) {
		PyErr_SetString(PyExc_RuntimeError, "Must be 3 bitplanes of 256 rows with 80 bytes per row (total of 61440 bytes).");
		return NULL;
	}

	write_gif((uchar *)buf);

	return Py_BuildValue(BYTEARRAY_FORMAT, buffer, length);
}

static char module_docstring[] =
"Encode an image given as a set of Amiga bitplanes as a GIF file.";

static char set_palette_docstring[] =
"Set palette to use for image. Input is a string of r, g, b color values.";

static char encode_docstring[] =
"Encode bitplanes as GIF, using the palette given previously.";

static PyMethodDef module_methods[] = {
	{"set_palette", b2g_set_palette, METH_VARARGS, set_palette_docstring },
	{"encode", b2g_encode, METH_VARARGS, encode_docstring},
	{NULL, NULL, 0, NULL}
};

#if PY_MAJOR_VERSION > 2
static struct PyModuleDef bpls2gif_module = {
   PyModuleDef_HEAD_INIT,
   "bpls2gif",
   module_docstring,
   -1,
   module_methods
};
#endif

#if PY_MAJOR_VERSION > 2
PyMODINIT_FUNC PyInit_bpls2gif(void)
#else
PyMODINIT_FUNC initbpls2gif(void)
#endif
{
#if PY_MAJOR_VERSION > 2
	PyObject *m = PyModule_Create(&bpls2gif_module);
	if (m == NULL)
		return NULL;
#else
	PyObject *m = Py_InitModule3("bpls2gif", module_methods, module_docstring);
	if (m == NULL)
		return;
#endif

	nodes = malloc(4096 * sizeof(Node));
	pixels = malloc(W * BLOCK_HEIGHT / 2);

	buffer = malloc(4096);
	capacity = 4096;
	length = 0;

#if PY_MAJOR_VERSION > 2
	return m;
#else
	return;
#endif
}


text:
/*
 * Copyright (c) 2018-2023 Niklas Ekström
 */

#include <arpa/inet.h>

#include <linux/spi/spidev.h>
#include <linux/types.h>

#include <netinet/in.h>
#include <netinet/tcp.h>

#include <sys/epoll.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>

#include <ctype.h>
#include <errno.h>
#include <fcntl.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>

#include <algorithm>
#include <list>
#include <string>
#include <vector>

#if !defined(MODEL_TD) && !defined(MODEL_FE) && !defined(MODEL_CP)
#error Need to define MODEL_TD, MODEL_FE or MODEL_CP
#elif defined(MODEL_TD) && defined(MODEL_FE) || defined(MODEL_TD) && defined(MODEL_CP) || defined(MODEL_FE) && defined(MODEL_CP)
#error The MODEL_XX flags cannot be combined
#endif

#define LOGGER_TRACE 0
#define logger_trace(...) do { if (LOGGER_TRACE) fprintf(stdout, __VA_ARGS__); } while (0)

#define LOGGER_DEBUG 0
#define logger_debug(...) do { if (LOGGER_DEBUG) fprintf(stdout, __VA_ARGS__); } while (0)

#define LOGGER_INFO 1
#define logger_info(...) do { if (LOGGER_INFO) fprintf(stdout, __VA_ARGS__); } while (0)

#define LOGGER_WARN 1
#define logger_warn(...) do { if (LOGGER_WARN) fprintf(stdout, __VA_ARGS__); } while (0)

#define LOGGER_ERROR 1
#define logger_error(...) do { if (LOGGER_ERROR) fprintf(stderr, __VA_ARGS__); } while (0)

// 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

#define R2A_TAIL_UPDATED        1
#define A2R_HEAD_UPDATED        2

// TODO: These constants should be the same for both TD/FE and CP.
// Need to update a314.device in order to change these.
#if defined(MODEL_TD)
// 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

// Addresses of fixed data structures in shared memory.
#define CAP_BASE                0
#define A2R_BASE                4
#define R2A_BASE                260
#elif defined(MODEL_FE)
// 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

// Addresses of fixed data structures in shared memory.
#define HANDSHAKE_BASE          0
#define CAP_BASE                4
#define A2R_BASE                8
#define R2A_BASE                264
#elif defined(MODEL_CP)
// Offset relative to communication area for queue pointers.
#define R2A_TAIL_OFFSET         0
#define A2R_HEAD_OFFSET         1
#define A2R_TAIL_OFFSET         2
#define R2A_HEAD_OFFSET         3

// Addresses of fixed data structures in shared memory.
#define A2R_BASE                0
#define R2A_BASE                256
#define CAP_BASE                512
#endif

#if defined(MODEL_TD)
#define IRQ_GPIO                "25"
#define IRQ_GPIO_EDGE           "both"
#elif defined(MODEL_FE)
#define IRQ_GPIO                "23"
#define IRQ_GPIO_EDGE           "rising"
#elif defined(MODEL_CP)
#define IRQ_GPIO                "2"
#define IRQ_GPIO_EDGE           "rising"
#endif

#if defined(MODEL_TD)
// SPI commands.
#define READ_SRAM_CMD           0
#define WRITE_SRAM_CMD          1
#define READ_CMEM_CMD           2
#define WRITE_CMEM_CMD          3
#define SPI_PROTO_VER_CMD       255

#define READ_SRAM_HDR_LEN       4

// Addresses to variables in CMEM.
#define BASE_ADDRESS_LEN        6
#define R_EVENTS_ADDRESS        12
#define R_ENABLE_ADDRESS        13
#define A_EVENTS_ADDRESS        14
#define A_ENABLE_ADDRESS        15

// 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_BASE_ADDRESS    4

// Events that are communicated from Raspberry to Amiga.
#define A_EVENT_R2A_TAIL        1
#define A_EVENT_A2R_HEAD        2
#elif defined(MODEL_FE)
#define PIN_D(x)                (4 + x)
#define PIN_WR                  20
#define PIN_REQ                 21
#define PIN_ACK                 22
#define PIN_IRQ                 23
#define PIN_A(x)                (24 + x)

#define GPIO_DIR_0_MASK         0x3ffff000
#define GPIO_DIR_0_DIN          0x00000000
#define GPIO_DIR_0_DOUT         0x09249000
#define GPIO_DIR_1_MASK         0x3fffffff
#define GPIO_DIR_1_DIN          0x00000000
#define GPIO_DIR_1_DOUT         0x09249249
#define GPIO_DIR_2_MASK         0x00ffffff
#define GPIO_DIR_2              0x00249009

#define GPIO_PULL_NONE          0
#define GPIO_PULL_DOWN          1
#define GPIO_PULL_UP            2

#define REG_SRAM_BYTE           0
#define REG_SRAM_WORD           1
#define REG_ADDR_LO             2
#define REG_ADDR_HI             3
#define REG_INT_REQ             4
#define REG_INT_ENA             5
#define REG_CA_BASE_ADDR        6

#define IRQ_SET                 0x8000
#define IRQ_CLR                 0x0000
#define IRQ_R2A_TAIL            0x0001
#define IRQ_A2R_HEAD            0x0002
#define IRQ_A2R_TAIL            0x0004
#define IRQ_R2A_HEAD            0x0008
#define IRQ_BASE_ADDRESS        0x0010
#elif defined(MODEL_CP)
#define PIN_IRQ                 2
#define PIN_CLK                 4
#define PIN_REQ                 14
#define PIN_ACK                 15
#define PIN_D(x)                (16 + x)
#define PIN_A(x)                (24 + x)
#define PIN_WR                  27

#define PINS_OUT            ((0xff << PIN_D(0)) | (3 << PIN_A(0)) | (1 << PIN_WR) | (1 << PIN_REQ))
#define PINS_OUT_NOT_WR     ((0xff << PIN_D(0)) | (3 << PIN_A(0)) | (1 << PIN_REQ))

#define GPIO_DIR_0              0x08004000
#define GPIO_DIR_1_DIN          0x00001009
#define GPIO_DIR_1_DOUT         0x09241009
#define GPIO_DIR_2_DIN          0x08209000
#define GPIO_DIR_2_DOUT         0x08209249

#define GPIO_PULL_NONE          0
#define GPIO_PULL_DOWN          1
#define GPIO_PULL_UP            2

#define REG_SRAM                0
#define REG_IRQ                 1
#define REG_ADDR_LO             2
#define REG_ADDR_HI             3

#define REG_IRQ_SET             0x80
#define REG_IRQ_CLR             0x00
#define REG_IRQ_PI              0x02
#define REG_IRQ_CP              0x01
#endif

// Global variables.
static sigset_t original_sigset;

static uint8_t tx_buf[65536];
static uint8_t rx_buf[65536];

static volatile unsigned int *gpio;

#if defined(MODEL_TD)
static uint8_t mode = SPI_CS_HIGH;
static uint8_t bits = 8;
static uint32_t speed = 67000000;

static int spi_fd = -1;
static int spi_proto_ver = 0;
#elif defined(MODEL_FE)
static unsigned int low_pins_dir;
static unsigned int current_address;
static int current_dir = 0; // 0 = input, 1 = output.
#elif defined(MODEL_CP)
static unsigned short current_address;
static int current_dir = 0; // 0 = input, 1 = output.

static bool read_a314_magic = false;
static int restart_counter;
#endif

static bool gpio_irq_exported = false;
static bool gpio_irq_edge_set = false;
static int gpio_irq_fd = -1;

static bool auto_clear_irq = false;
static time_t auto_clear_irq_after;

static int server_socket = -1;

static int epfd = -1;

static bool have_base_address = false;
static unsigned int base_address = 0;

#if defined(MODEL_TD) || defined(MODEL_FE)
#define BASE_ADDRESS base_address
#elif defined(MODEL_CP)
#define BASE_ADDRESS 0
#endif

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<uint8_t> data;
};

struct RegisteredService
{
    std::string name;
    ClientConnection *cc;
};

struct PacketBuffer
{
    int type;
    std::vector<uint8_t> data;
};

struct ClientConnection
{
    int fd;

    int next_stream_id;

    int bytes_read;
    MessageHeader header;
    std::vector<uint8_t> payload;

    std::list<MessageBuffer> message_queue;

    std::list<LogicalChannel*> associations;
};

struct LogicalChannel
{
    int channel_id;

    ClientConnection *association;
    int stream_id;

    bool got_eos_from_ami;
    bool got_eos_from_client;

    std::list<PacketBuffer> 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<ClientConnection> connections;
static std::list<RegisteredService> services;
static std::list<LogicalChannel> channels;
static std::list<LogicalChannel*> send_queue;

struct OnDemandStart
{
    std::string service_name;
    std::string program;
    std::vector<std::string> arguments;
};

std::vector<OnDemandStart> 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<char *> 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");
}

#if defined(MODEL_TD)
static int init_spi()
{
    spi_fd = open("/dev/spidev0.0", O_RDWR | O_CLOEXEC);
    if (spi_fd < 0)
        return -1;

    int ret = ioctl(spi_fd, SPI_IOC_WR_MODE, &mode);
    ret |= ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &bits);
    ret |= ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
    if (ret != 0)
        return ret;

    return 0;
}

static void shutdown_spi()
{
    if (spi_fd != -1)
        close(spi_fd);
}

static int spi_transfer(int len)
{
    struct spi_ioc_transfer tr =
    {
        .tx_buf = (uintptr_t)tx_buf,
        .rx_buf = (uintptr_t)rx_buf,
        .len = (uint32_t)len,
        .speed_hz = speed,
        .delay_usecs = 0,
        .bits_per_word = bits,
        .cs_change = 0,
    };

    return ioctl(spi_fd, SPI_IOC_MESSAGE(1), &tr);
}

static int spi_protocol_version()
{
    tx_buf[0] = (uint8_t)SPI_PROTO_VER_CMD;
    tx_buf[1] = 0;
    spi_transfer(2);
    logger_trace("SPI protocol version = %d\n", rx_buf[1]);
    return (int)rx_buf[1];
}

static void spi_read_shm_rxbuf(unsigned int address, unsigned int length)
{
    logger_trace("SPI read mem address = %d length = %d\n", address, length);

    unsigned int header;
    if (spi_proto_ver == 1)
        header = (READ_SRAM_CMD << 21) | (address & 0x1fffff);
    else
        header = (READ_SRAM_CMD << 20) | (address & 0xfffff);

    tx_buf[0] = (uint8_t)((header >> 16) & 0xff);
    tx_buf[1] = (uint8_t)((header >> 8) & 0xff);
    tx_buf[2] = (uint8_t)(header & 0xff);
	tx_buf[3] = 0;

    spi_transfer(length + 4);
}

static void spi_read_shm(unsigned char *data, unsigned int address, unsigned int length)
{
    spi_read_shm_rxbuf(address, length);
    memcpy(data, &rx_buf[READ_SRAM_HDR_LEN], length);
}

static void spi_write_shm(unsigned int address, uint8_t *buf, unsigned int length)
{
    logger_trace("SPI write mem address = %d length = %d\n", address, length);

    unsigned int header;
    if (spi_proto_ver == 1)
        header = (WRITE_SRAM_CMD << 21) | (address & 0x1fffff);
    else
        header = (WRITE_SRAM_CMD << 20) | (address & 0xfffff);

    tx_buf[0] = (uint8_t)((header >> 16) & 0xff);
    tx_buf[1] = (uint8_t)((header >> 8) & 0xff);
    tx_buf[2] = (uint8_t)(header & 0xff);

    memcpy(&tx_buf[3], buf, length);
    spi_transfer(length + 3);
}

static uint8_t spi_read_cmem(unsigned int address)
{
    if (spi_proto_ver == 1)
        tx_buf[0] = (uint8_t)((READ_CMEM_CMD << 5) | (address & 0xf));
    else
        tx_buf[0] = (uint8_t)((READ_CMEM_CMD << 4) | (address & 0xf));
    tx_buf[1] = 0;
    spi_transfer(2);
    logger_trace("SPI read cmem, address = %d, returned = %d\n", address, rx_buf[1]);
    return rx_buf[1];
}

static void spi_write_cmem(unsigned int address, unsigned int data)
{
    logger_trace("SPI write cmem, address = %d, data = %d\n", address, data);

    if (spi_proto_ver == 1)
        tx_buf[0] = (uint8_t)((WRITE_CMEM_CMD << 5) | (address & 0xf));
    else
        tx_buf[0] = (uint8_t)((WRITE_CMEM_CMD << 4) | (address & 0xf));
    tx_buf[1] = (uint8_t)(data & 0xf);
    spi_transfer(2);
}

static uint8_t spi_ack_irq()
{
    logger_trace("SPI ack_irq\n");
    return spi_read_cmem(R_EVENTS_ADDRESS);
}

static void spi_read_base_address()
{
    have_base_address = false;

    unsigned int ba1 = 0;
    for (int i = 0; i < BASE_ADDRESS_LEN; i++)
        ba1 |= spi_read_cmem(i) << (i * 4);

    if ((ba1 & 1) == 1)
    {
        unsigned int ba2 = 0;
        for (int i = 0; i < BASE_ADDRESS_LEN; i++)
            ba2 |= spi_read_cmem(i) << (i * 4);

        if (ba1 == ba2)
        {
            have_base_address = true;
            base_address = ba1 & ~1;
        }
    }
}

#define read_shm spi_read_shm
#define write_shm spi_write_shm
#endif

#if defined(MODEL_FE) || defined(MODEL_CP)
static int create_dev_gpiomem_mapping()
{
    int fd = open("/dev/gpiomem", O_RDWR | O_SYNC);
    if (fd < 0)
    {
        logger_error("Unable to open /dev/gpiomem\n");
        return -1;
    }

    void *gpio_map = mmap(NULL, 4096, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);

    close(fd);

    if (gpio_map == MAP_FAILED)
    {
        logger_error("mmap failed, errno = %d\n", errno);
        return -1;
    }

    gpio = (volatile unsigned int *)gpio_map;
    return 0;
}

static void set_gpio_pull_mode(int mask, int mode)
{
    *(gpio + 37) = mode;
    usleep(50);
    *(gpio + 38) = mask;
    usleep(50);
    *(gpio + 38) = 0;
    *(gpio + 37) = 0;
    usleep(50);
}
#endif

#if defined(MODEL_FE)
static inline void set_dir_read()
{
    if (current_dir == 1)
    {
        *(gpio + 0) = GPIO_DIR_0_DIN | low_pins_dir;
        *(gpio + 1) = GPIO_DIR_1_DIN;
        current_dir = 0;
    }
}

static inline void set_dir_write()
{
    if (current_dir == 0)
    {
        *(gpio + 0) = GPIO_DIR_0_DOUT | low_pins_dir;
        *(gpio + 1) = GPIO_DIR_1_DOUT;
        current_dir = 1;
    }
}

static unsigned int read_sram_reg(unsigned int reg)
{
    set_dir_read();

    *(gpio + 7) = (reg << PIN_A(0)) | (1 << PIN_REQ);

    // Delay so that the ACK pin is sure to be negated after previous access.
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;

    // Additional delay to hopefully do only one read when there is no contention.
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;

    unsigned int value;
    while (!((value = *(gpio + 13)) & (1 << PIN_ACK)))
        ;

    value = (value >> PIN_D(0)) & 0xffff;

    *(gpio + 10) = (1 << PIN_REQ);
    *(gpio + 10) = (15 << PIN_A(0)) | (1 << PIN_WR) | (0xffff << PIN_D(0));

    return value;
}

static void write_sram_reg(unsigned int reg, unsigned int value)
{
    set_dir_write();

    *(gpio + 7) = (reg << PIN_A(0)) | (1 << PIN_REQ) | (1 << PIN_WR) | ((value & 0xffff) << PIN_D(0));

    // Delay so that the ACK pin is sure to be negated after previous access.
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;

    // Additional delay to hopefully do only one read when there is no contention.
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;

    while (!(*(gpio + 13) & (1 << PIN_ACK)))
        ;

    *(gpio + 10) = (1 << PIN_REQ);
    *(gpio + 10) = (15 << PIN_A(0)) | (1 << PIN_WR) | (0xffff << PIN_D(0));
}

static unsigned int read_reg(unsigned int reg)
{
    set_dir_read();

    *(gpio + 7) = (reg << PIN_A(0)) | (1 << PIN_REQ);
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;
    *(gpio + 7) = 0;

    unsigned int value = *(gpio + 13);
    value = (value >> PIN_D(0)) & 0xffff;

    *(gpio + 10) = (1 << PIN_REQ);
    *(gpio + 7) = 0;
    *(gpio + 10) = (15 << PIN_A(0)) | (1 << PIN_WR) | (0xffff << PIN_D(0));

    return value;
}

static void write_reg(unsigned int reg, unsigned int value)
{
    set_dir_write();

    *(gpio + 7) = (reg << PIN_A(0)) | (1 << PIN_REQ) | (1 << PIN_WR) | ((value & 0xffff) << PIN_D(0));
    *(gpio + 7) = 0;
    *(gpio + 10) = (1 << PIN_REQ);
    *(gpio + 7) = 0;
    *(gpio + 10) = (15 << PIN_A(0)) | (1 << PIN_WR) | (0xffff << PIN_D(0));
}

static void set_address(unsigned int address)
{
    if ((address & 0xffff) != (current_address & 0xffff))
        write_reg(REG_ADDR_LO, address & 0xffff);

    if (((address >> 16) & 0x3) != ((current_address >> 16) & 0x3))
        write_reg(REG_ADDR_HI, (address >> 16) & 0x3);

    current_address = address;
}

static void gpio_read_shm(unsigned char *data, unsigned int address, unsigned int length)
{
    set_address(address);

    current_address = (address + length) & 0x3ffff;

    unsigned int v;

    if (address & 1)
    {
        v = read_sram_reg(REG_SRAM_BYTE);
        *data++ = v & 0xff;
        length--;
    }

    while (length >= 2)
    {
        v = read_sram_reg(REG_SRAM_WORD);
        *data++ = (v >> 8) & 0xff;
        *data++ = v & 0xff;
        length -= 2;
    }

    if (length)
    {
        v = read_sram_reg(REG_SRAM_BYTE);
        *data++ = (v >> 8) & 0xff;
    }
}

static void gpio_write_shm(unsigned int address, unsigned char *data, unsigned int length)
{
    set_address(address);

    current_address = (address + length) & 0x3ffff;

    unsigned int v;

    if (address & 1)
    {
        v = *data++;
        write_sram_reg(REG_SRAM_BYTE, v);
        length--;
    }

    while (length >= 2)
    {
        v = ((*data++) << 8) | (*data++);
        write_sram_reg(REG_SRAM_WORD, v);
        length -= 2;
    }

    if (length)
    {
        v = (*data++) << 8;
        write_sram_reg(REG_SRAM_BYTE, v);
    }
}

#define write_shm gpio_write_shm
#define read_shm gpio_read_shm

static int init_gpio()
{
    if (create_dev_gpiomem_mapping())
        return -1;

    // Set pin directions.
    low_pins_dir = *(gpio + 0) & 0x00000fff;
    unsigned int high_pins_dir = *(gpio + 2) & 0x3f000000;

    *(gpio + 0) = GPIO_DIR_0_DIN | low_pins_dir;
    *(gpio + 1) = GPIO_DIR_1_DIN;
    *(gpio + 2) = high_pins_dir | GPIO_DIR_2;

    *(gpio + 10) = 0x0ffffff0;

    set_gpio_pull_mode((1 << PIN_ACK) | (1 << PIN_IRQ), GPIO_PULL_DOWN);
    set_gpio_pull_mode((15 << PIN_A(0)) | (1 << PIN_REQ) | (1 << PIN_WR) | (0xffff << PIN_D(0)), GPIO_PULL_NONE);

    current_dir = 0;

    // Set address pointer.
    write_reg(REG_ADDR_LO, 0);
    write_reg(REG_ADDR_HI, 0);

    current_address = 0;

    return 0;
}

static void shutdown_gpio()
{
    // Let Linux take care of this.
}

#elif defined(MODEL_CP)
static void gpio_write_reg(unsigned int reg, unsigned int value)
{
    if (current_dir == 0)
    {
        *(gpio + 7) = (1 << PIN_WR);
        *(gpio + 1) = GPIO_DIR_1_DOUT;
        *(gpio + 2) = GPIO_DIR_2_DOUT;
        current_dir = 1;
    }

    while ((*(gpio + 13) & (1 << PIN_ACK)))
        ;

    *(gpio + 7) = ((value & 0xff) << PIN_D(0)) | (reg << PIN_A(0)) | (1 << PIN_REQ);

    while (!(*(gpio + 13) & (1 << PIN_ACK)))
        ;

    *(gpio + 10) = PINS_OUT_NOT_WR;
}

static unsigned int gpio_read_reg(unsigned int reg)
{
    if (current_dir == 1)
    {
        *(gpio + 1) = GPIO_DIR_1_DIN;
        *(gpio + 2) = GPIO_DIR_2_DIN;
        *(gpio + 10) = (1 << PIN_WR);
        current_dir = 0;
    }

    while ((*(gpio + 13) & (1 << PIN_ACK)))
        ;

    *(gpio + 7) = (reg << PIN_A(0)) | (1 << PIN_REQ);

    unsigned int value;
    while (!((value = *(gpio + 13)) & (1 << PIN_ACK)))
        ;

    value = (value >> PIN_D(0)) & 0xff;

    *(gpio + 10) = PINS_OUT_NOT_WR;

    return value;
}

static void clear_pi_irq()
{
    gpio_write_reg(REG_IRQ, REG_IRQ_CLR | REG_IRQ_PI);
}

static void set_cp_irq()
{
    gpio_write_reg(REG_IRQ, REG_IRQ_SET | REG_IRQ_CP);
}

static void clear_cp_irq()
{
    gpio_write_reg(REG_IRQ, REG_IRQ_CLR | REG_IRQ_CP);
}

static void gpio_set_address(unsigned short address)
{
    if ((address & 0xff) != (current_address & 0xff))
        gpio_write_reg(REG_ADDR_LO, address & 0xff);

    if (((address >> 8) & 0xff) != ((current_address >> 8) & 0xff))
        gpio_write_reg(REG_ADDR_HI, (address >> 8) & 0xff);

    current_address = address;
}

static void gpio_write_shm(unsigned short address, unsigned char *data, unsigned short length)
{
    gpio_set_address(address);

    for (int i = 0; i < length; i++)
        gpio_write_reg(REG_SRAM, *data++);

    current_address += length;
}

static void gpio_read_shm(unsigned char *data, unsigned short address, unsigned short length)
{
    gpio_set_address(address);

    for (int i = 0; i < length; i++)
        *data++ = gpio_read_reg(REG_SRAM);

    current_address += length;
}

#define write_shm gpio_write_shm
#define read_shm gpio_read_shm

static int init_gpio()
{
    if (create_dev_gpiomem_mapping())
        return -1;

    // Set pin directions.
    // Inputs: PIN_IRQ, PIN_ACK, PIN_D(x), unused pins.
    // Outputs: PIN_REQ, PIN_A(x), PIN_WR, pin 29 (connects to LED on Pi3).
    // Alt0: PIN_CLK.
    *(gpio + 0) = GPIO_DIR_0;
    *(gpio + 1) = GPIO_DIR_1_DIN;
    *(gpio + 2) = GPIO_DIR_2_DIN;

    set_gpio_pull_mode((1 << PIN_ACK) | (1 << PIN_IRQ), GPIO_PULL_DOWN);
    set_gpio_pull_mode(PINS_OUT | (1 << PIN_CLK), GPIO_PULL_NONE);

    current_dir = 0;

    // Set address pointer.
    gpio_write_reg(REG_ADDR_LO, 0);
    gpio_write_reg(REG_ADDR_HI, 0);

    current_address = 0;

    return 0;
}

static void shutdown_gpio()
{
    // Let Linux take care of this.
}
#endif

static int open_write_close(const char *filename, const char *text)
{
    int fd = open(filename, O_WRONLY);
    if (fd == -1)
        return -1;

    write(fd, text, strlen(text));
    close(fd);
    return 0;
}

static void sleep_100ms()
{
    struct timespec delay;
    delay.tv_sec = 0;
    delay.tv_nsec = 100000000L;
    nanosleep(&delay, NULL);
}

static void set_direction()
{
    for (int retry = 0; retry < 100; retry++)
    {
        int fd = open("/sys/class/gpio/gpio" IRQ_GPIO "/direction", O_WRONLY);
        if (fd != -1)
        {
            write(fd, "in", 2);
            close(fd);
            break;
        }
        sleep_100ms();
    }
}

static int init_gpio_irq()
{
    if (open_write_close("/sys/class/gpio/export", IRQ_GPIO) != 0)
        return -1;

    gpio_irq_exported = true;

    set_direction();

    if (open_write_close("/sys/class/gpio/gpio" IRQ_GPIO "/edge", IRQ_GPIO_EDGE) == -1)
        return -1;

    gpio_irq_edge_set = true;

    gpio_irq_fd = open("/sys/class/gpio/gpio" IRQ_GPIO "/value", O_RDONLY | O_NONBLOCK | O_CLOEXEC);
    if (gpio_irq_fd == -1)
        return -1;

    return 0;
}

static void shutdown_gpio_irq()
{
    if (gpio_irq_fd != -1)
        close(gpio_irq_fd);

    if (gpio_irq_edge_set)
        open_write_close("/sys/class/gpio/gpio" IRQ_GPIO "/edge", "none");

    if (gpio_irq_exported)
        open_write_close("/sys/class/gpio/unexport", IRQ_GPIO);
}

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;
}

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;

#if defined(MODEL_TD)
    if (init_spi() != 0)
        return -1;

    spi_proto_ver = spi_protocol_version();
#elif defined(MODEL_FE) || defined(MODEL_CP)
    if (init_gpio() != 0)
        return -1;
#endif

    if (init_gpio_irq() != 0)
        return -1;

    epfd = epoll_create1(EPOLL_CLOEXEC);
    if (epfd == -1)
        return -1;

    struct epoll_event ev;
    ev.events = EPOLLPRI | EPOLLERR;
    ev.data.fd = gpio_irq_fd;
    if (epoll_ctl(epfd, EPOLL_CTL_ADD, gpio_irq_fd, &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_gpio_irq();
#if defined(MODEL_TD)
    shutdown_spi();
#elif defined(MODEL_FE) || defined(MODEL_CP)
    shutdown_gpio();
#endif
    shutdown_server_socket();
}

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]);

#if defined(MODEL_TD)
    // This is an optimization to save a memcpy.
    spi_read_shm_rxbuf(address, length);
    create_and_send_msg(cc, MSG_READ_MEM_RES, 0, &rx_buf[READ_SRAM_HDR_LEN], length);
#else
    read_shm(rx_buf, address, length);
    create_and_send_msg(cc, MSG_READ_MEM_RES, 0, rx_buf, length);
#endif
}

static void handle_msg_write_mem_req(ClientConnection *cc)
{
    uint32_t address = *(uint32_t *)&(cc->payload[0]);
    uint32_t length = cc->payload.size() - 4;

    write_shm(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];

                std::vector<std::string> args(on_demand.arguments);
                args.push_back("-ondemand");
                args.push_back(std::to_string(fd));
                std::vector<const char *> 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 void 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;

    if (head < tail)
        read_shm(recv_buf, BASE_ADDRESS + A2R_BASE + head, len);
    else
    {
        read_shm(recv_buf, BASE_ADDRESS + A2R_BASE + head, 256 - head);

        if (tail != 0)
            read_shm(&recv_buf[len - tail], BASE_ADDRESS + A2R_BASE, 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 |= A2R_HEAD_UPDATED;
}

static void 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;

    uint8_t *p = send_buf;
    int at_end = 256 - tail;
    if (at_end < to_write)
    {
        write_shm(BASE_ADDRESS + R2A_BASE + tail, p, at_end);
        p += at_end;
        to_write -= at_end;
        tail = 0;
    }

    write_shm(BASE_ADDRESS + R2A_BASE + tail, p, to_write);
    tail = (tail + to_write) & 255;

    channel_status[R2A_TAIL_OFFSET] = tail;
    channel_status_updated |= R2A_TAIL_UPDATED;
}

static void read_channel_status()
{
    read_shm(channel_status, BASE_ADDRESS + CAP_BASE, 4);
    channel_status_updated = 0;
}

static void write_channel_status()
{
    if (channel_status_updated != 0)
    {
        write_shm(BASE_ADDRESS + CAP_BASE + R2A_TAIL_OFFSET, &channel_status[R2A_TAIL_OFFSET], 2);

#if defined(MODEL_TD)
        unsigned int events = 0;
        if (channel_status_updated & R2A_TAIL_UPDATED)
            events |= A_EVENT_R2A_TAIL;
        if (channel_status_updated & A2R_HEAD_UPDATED)
            events |= A_EVENT_A2R_HEAD;
        spi_write_cmem(A_EVENTS_ADDRESS, events);
#elif defined(MODEL_FE)
        uint32_t irq = 0;
        if (channel_status_updated & R2A_TAIL_UPDATED)
            irq |= IRQ_R2A_TAIL;
        if (channel_status_updated & A2R_HEAD_UPDATED)
            irq |= IRQ_A2R_HEAD;
        write_reg(REG_INT_REQ, IRQ_SET | irq);

        auto_clear_irq = true;
        auto_clear_irq_after = time(NULL) + 3;
#elif defined(MODEL_CP)
        set_cp_irq();

        auto_clear_irq = true;
        auto_clear_irq_after = time(NULL) + 3;
#endif
        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);
    }
}

#if defined(MODEL_TD)
static void handle_a314_irq()
{
    uint8_t events = spi_ack_irq();
    if (events == 0)
        return;

    if ((events & R_EVENT_BASE_ADDRESS) || !have_base_address)
    {
        if (have_base_address && !channels.empty())
            logger_info("Base address was updated while logical channels are open -- closing channels\n");

        close_all_logical_channels();
        spi_read_base_address();
    }

    if (!have_base_address)
        return;

    read_channel_status();

    receive_from_a2r();
    flush_send_queue();

    write_channel_status();
}
#elif defined(MODEL_FE)
static void handle_a314_irq()
{
    write_reg(REG_INT_ENA, 0);

    uint32_t irq = read_reg(REG_INT_REQ) & (IRQ_BASE_ADDRESS | IRQ_R2A_HEAD | IRQ_A2R_TAIL);

    if (irq)
        write_reg(REG_INT_REQ, IRQ_CLR | irq);

    if (irq & IRQ_BASE_ADDRESS || !have_base_address)
    {
        base_address = read_reg(REG_CA_BASE_ADDR) << 2;
        have_base_address = base_address != 0x3fffc;

        if (!channels.empty())
            logger_info("Base address was updated while logical channels are open -- closing channels\n");

        close_all_logical_channels();
    }

    if (have_base_address)
    {
        read_channel_status();

        receive_from_a2r();
        flush_send_queue();

        write_channel_status();
    }

    write_reg(REG_INT_ENA, IRQ_BASE_ADDRESS | IRQ_R2A_HEAD | IRQ_A2R_TAIL);
}
#elif defined(MODEL_CP)
static void handle_a314_irq()
{
    // TODO: Currently there's a single notification event in either direction.
    // Could possibly have two events, one to signal that R2A_TAIL is updated
    // and one for A2R_HEAD.
    // Also in that case should add enable bits so that the other side can
    // mute notifications.

    clear_pi_irq();

    if (!read_a314_magic)
    {
        uint8_t values[2];
        read_shm(values, BASE_ADDRESS + CAP_BASE + 5, 2);
        if (values[0] != 0xa3 || values[1] != 0x14)
            return;

        read_shm(values, BASE_ADDRESS + CAP_BASE + 4, 1);
        restart_counter = values[0];

        read_a314_magic = true;
    }

    read_channel_status();

    uint8_t current_restart_counter;
    read_shm(&current_restart_counter, BASE_ADDRESS + CAP_BASE + 4, 1);

    if (current_restart_counter != restart_counter)
    {
        if (!channels.empty())
            logger_info("Restart counter was updated while logical channels are open -- closing channels\n");
        else
            logger_info("Restart counter was updated\n");

        close_all_logical_channels();
        restart_counter = current_restart_counter;

        read_channel_status();
    }

    receive_from_a2r();
    flush_send_queue();

    write_channel_status();
}
#endif

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()
{
    handle_a314_irq();

    bool first_gpio_event = true;
    bool shutting_down = false;
    time_t force_shutdown_after;
    bool done = false;

#if defined(MODEL_FE) || defined(MODEL_CP)
    auto_clear_irq = true;
    auto_clear_irq_after = time(NULL) + 3;
#endif

    while (!done)
    {
        struct epoll_event ev;
        int n = epoll_pwait(epfd, &ev, 1, 1000, &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());

                flush_send_queue();
                write_channel_status();

                if (!shutting_down)
                    force_shutdown_after = time(NULL) + 10;

                shutting_down = true;
            }
            else
            {
                logger_error("epoll_pwait failed with unexpected errno = %d\n", errno);
                exit(-1);
            }
        }
        else if (n == 0)
        {
            // Timeout. Handle below.
        }
        else
        {
            if (ev.data.fd == gpio_irq_fd)
            {
                logger_trace("Epoll event: gpio is ready, events = %d\n", ev.events);

                lseek(gpio_irq_fd, 0, SEEK_SET);

                char buf;
                if (read(gpio_irq_fd, &buf, 1) != 1)
                {
                    logger_error("Read from GPIO value file, and unexpectedly didn't return 1 byte\n");
                    exit(-1);
                }

                if (first_gpio_event)
                {
                    logger_debug("Received first GPIO event, which is ignored\n");
                    first_gpio_event = false;
                }
                else
                {
                    logger_trace("GPIO interupted\n");
                    handle_a314_irq();
                }
            }
            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);

                flush_send_queue();
                write_channel_status();
            }
        }

        time_t now = time(NULL);

        if (auto_clear_irq && now > auto_clear_irq_after)
        {
#if defined(MODEL_FE)
            write_reg(REG_INT_REQ, IRQ_CLR | IRQ_A2R_HEAD | IRQ_R2A_TAIL);
#elif defined(MODEL_CP)
            clear_cp_irq();
#endif
            auto_clear_irq = false;
        }

        if (shutting_down && (channels.empty() || now > force_shutdown_after))
            done = true;
    }
}

int main(int argc, char **argv)
{
    std::string conf_filename("/etc/opt/a314/a314d.conf");

    if (argc >= 2)
        conf_filename = argv[1];

    load_config_file(conf_filename.c_str());

    if (init_driver() == 0)
        main_loop();
    shutdown_driver();
    return 0;
}


text:
#include <errno.h>
#include <fcntl.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h>

#include <bcm_host.h>

#define GPIO_ADDR 0x200000
#define GPCLK_ADDR 0x101000

#define CLK_GP0_CTL 0x070
#define CLK_GP0_DIV 0x074

#define CTL_PASSWD (0x5a << 24)
#define CTL_BUSY (1 << 7)
#define CTL_KILL (1 << 5)
#define CTL_ENAB (1 << 4)

#define CTL_SRC_PLLC 5
#define CTL_SRC_PLLD 6

volatile unsigned int *gpio;
volatile unsigned int *gpclk;

static void create_dev_mem_mapping(size_t base, size_t size)
{
    int fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (fd < 0)
    {
        printf("Unable to open /dev/mem. Run as root using sudo?\n");
        exit(-1);
    }

    void *gpio_map = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, base);

    close(fd);

    if (gpio_map == MAP_FAILED)
    {
        printf("mmap failed, errno = %d\n", errno);
        exit(-1);
    }

    gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
    gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
}

static void start_gpclk(int divisor)
{
    volatile unsigned int *ctl = gpclk + (CLK_GP0_CTL / 4);
    volatile unsigned int *div = gpclk + (CLK_GP0_DIV / 4);

    *ctl = CTL_PASSWD | CTL_KILL;
    usleep(10);
    while (*ctl & CTL_BUSY)
        ;
    usleep(100);
    *div = CTL_PASSWD | (divisor << 12);
    usleep(10);
    *ctl = CTL_PASSWD | CTL_ENAB | CTL_SRC_PLLC;
    usleep(10);
    while (!(*ctl & CTL_BUSY))
        ;
    usleep(100);

    // GPIO pin 4 takes alternate function 0 (GPCLK0).
    *(gpio + 0) = (*(gpio + 0) & ~0x00007000) | 0x00004000;
}

int main(int argc, char **argv)
{
    size_t base = bcm_host_get_peripheral_address();
    size_t size = bcm_host_get_peripheral_size();

    create_dev_mem_mapping(base, size);

    int requested_mhz = 90;

    if (argc > 1)
    {
        requested_mhz = atoi(argv[1]);
        if (requested_mhz < 10 || requested_mhz > 200)
        {
            printf("Requested clock frequency \"%d\" (MHz) is outside of range 10..200\n", requested_mhz);
            return 0;
        }
    }

    int divisor = (1200 + requested_mhz - 1) / requested_mhz;
    int actual_mhz = 1200 / divisor;
    if (actual_mhz == requested_mhz)
        printf("Starting GPCLK0 with frequency=%d, divisor=%d\n", actual_mhz, divisor);
    else
        printf("Starting GPCLK0 with frequency=%d (requested=%d), divisor=%d\n", actual_mhz, requested_mhz, divisor);

    start_gpclk(divisor);

    return 0;
}


text:
[Unit]
Description=A314 Daemon
After=network.target

[Service]
Type=simple
User=##USER##
Group=##GROUP##
ExecStart=/opt/a314/a314d

[Install]
WantedBy=multi-user.target


text:
/dts-v1/;
/plugin/;

/ {
        compatible = "brcm,bcm2835";

        fragment@0 {
                target = <&spidev0>;
                __overlay__ {
                        spi-cs-high;
                };
        };
};