added ARM and ZYNQ specific code

git-svn-id: https://svn.inf.ethz.ch/svn/lecturers/a2/trunk@8306 8c9fc860-2736-0410-a75d-ab315db34111

source/ARM.Initializer.Mod

@@ -0,0 +1,201 @@
+MODULE Initializer;
+(**
+	AUTHOR "Timothée Martiel";
+	PURPOSE "Processor initialization code for ARM kernel";
+
+	This module provides initialization for CPUs. It contains the initial procedure that begins the execution of
+	the kernel. It also provides a mechanism for starting secondary processors: all processors should start executing
+	at the beginning of the kernel image (i.e. Init): only CPU with id 0 will continue and start the kernel. The other
+	processors will be sleeping in Init, waiting to be started later.
+*)
+IMPORT SYSTEM (* Needed to avoid Runtime importing Heaps *);
+VAR
+	(**
+		Wakeup address for secondary processors: if set # 0, the CPU with id matching "secondaryProcId" will jump to
+		that address.
+	*)
+	secondaryBootProc *: ADDRESS;
+	(**
+		Wakup id for secondary processors: when executing SEV, all CPUs wakup. Only the CPU with id matching this field
+		will read and jump to "secondaryBootProc".
+	*)
+	secondaryProcId *: LONGINT;
+
+	(** Configuration string base address *)
+	configBase *: ADDRESS;
+	(** Configuration string size *)
+	configSize *: LONGINT;
+
+	(**
+		Initial kernel code: this code is placed at the beginning of the kernel image and is executed by all CPUs.
+		This code:
+			- sets up a stack
+			- disables interrupts and MMU
+			- sets the interrupt vector to 0
+			- invalidates caches
+			- put secondary processors into the sleeping loop
+			- allow CPU0 to continue kernel initialization
+	*)
+	PROCEDURE {INITIAL,NOPAF} Init;
+	CODE
+		; set IRQ vector base register to zero
+		MOV	R2, #0
+		MCR	P15, 0, R2, C12, C0, 0
+
+		; disable MMU
+		MRC	P15, 0, R2, C1, C0, 0
+		BIC		R2, R2, #1
+		MCR	P15, 0, R2, C1, C0, 0
+
+		LDR	FP, [pc, #InitialFP-$-8]				; set stack pointer
+		LDR	SP, [pc, #InitialFP-$-8]				; set frame pointer
+
+		B		getProcId
+	InitialFP: 			d32 0001B000H				; initial frame pointer address, internal memory bank
+
+		; Set SMP mode
+		MRC p15, 0, R2, C1, C0, 1
+		ORR R2, R2, #041H
+		MCR p15, 0, R2, C1, C0, 1
+		ISB
+
+		; Filter CPUs: only CPU0 must initialize the kernel, other are started later on demand
+		MOV	R2, #0
+
+	getProcId:
+		MRC	P15, 0, R3, C0, C0, 5
+		AND	R3, R3, #0FH
+		CMP	R3, #0
+		BEQ	bootProc								; CPU 0 continues to booting the kernel
+
+	secondaryProcLoop:								; Secondary CPUs (i.e. CPUs that do not start the kernel) are
+		BL		InvalidateDCache
+		WFE											; sleeping and waiting to be started by SEV.
+
+
+		LDR	R0, [PC, #sndProcIdAdr-$-8]
+		LDR	R0, [R0, #0]							; R0 := secondaryProcId
+
+		MRC	P15, 0, R1, C0, C0, 5
+		AND	R1, R1, #0FH							; R1 := CPU ID
+
+		CMP	R0, R1									; IF R1 # R0: loop again
+		BNE	secondaryProcLoop
+
+		LDR	R0, [PC, #sndBootProcAdr-$-8]
+		LDR	R1, [R0, #0]							; R1 := secondaryBootProc
+		CMP	R1, #0									; IF R1 = 0: loop again
+		BEQ	secondaryProcLoop
+		BX		R1										; Jump to secondaryBootProc
+
+	sndBootProcAdr:	d32 secondaryBootProc
+	sndProcIdAdr:		d32 secondaryProcId
+
+	bootProc:											; CPU0 continues executing the kernel image
+		; Save configuration parameters
+		LDR	R2, [PC, #CfgBase - 8 - $]
+		STR	R0, [R2, #0]
+		LDR	R2, [PC, #CfgSize - 8 - $]
+		STR	R1, [R2, #0]
+
+		BL		InvalidateDCache						; After invalidating DCache
+
+		; Setup the interrupt vector: copy it from the data section below
+		MOV	R0, #0									; R0 := dst
+		LDR	R1, [PC, #intVecAdr - $ - 8]			; R1 := src
+		MOV	R2, #fiq - RESET + 4					; R2 := LEN(src) in bytes
+
+	copyInts:
+		LDR	R4, [R1, #0]
+		STR	R4, [R0, #0]
+		ADD	R0, R0, #4
+		ADD	R1, R1, #4
+		SUBS	R2, R2, #4
+		BNE	copyInts
+
+		; Start the kernel now
+		B		startKernel
+
+	; Interrupt vector data: to install the interrupt vector, we just have to copy
+	; from RESET to fiq (included) at address 0.
+	RESET: 	LDR	PC, [PC, #reset-$-8]			; RESET
+	UNDEF: 	LDR	PC, [PC, #undef-$-8]			; UNDEF
+	SWI: 		LDR	PC, [PC, #swi-$-8]			; SWI
+	PREF: 		LDR	PC, [PC, #prefetch-$-8]		; Prefetch Abort
+	DATA: 		LDR	PC, [PC, #data-$-8]			; Data Abort
+	INVALID: 	B		INVALID						; (not assigned)
+	IRQ: 		LDR	PC, [PC, #irq-$-8]				; IRQ
+	FIQ: 		LDR	PC, [PC, #fiq-$-8]				; FIQ
+		; initial interrupt vector setup such that inifinte loop is triggered
+	reset: 		d32 Init								; Reset starts the kernel
+	undef: 		d32 04H
+	swi: 		d32 08H
+	prefetch: 	d32 0CH
+	data: 		d32 10H
+	empty:		d32 0
+	irq: 		d32 14H
+	fiq: 		d32 1CH
+
+		; Address of the interrupt vector data to copy
+	intVecAdr:	d32 RESET
+
+	CfgBase:			d32 configBase
+	CfgSize:			d32 configSize
+
+
+	startKernel:
+	END Init;
+
+	PROCEDURE {NOPAF} InvalidateDCache *;
+	CODE
+		invalidate_dcache:
+			mrc	p15, 1, r0, c0, c0, 1		; read CLIDR
+			ands	r3, r0, #7000000H
+			mov	r3, r3, lsr #23			; cache level value (naturally aligned)
+			beq	finished
+			mov	r10, #0					; start with level 0
+		loop1:
+			add	r2, r10, r10, lsr #1		; work out 3xcachelevel
+			mov	r1, r0, lsr r2			; bottom 3 bits are the Cache type for this level
+			and	r1, r1, #7				; get those 3 bits alone
+			cmp	r1, #2
+			blt		skip					; no cache or only instruction cache at this level
+			mcr	p15, 2, r10, c0, c0, 0	; write the Cache Size selection register
+			isb								 ; isb to sync the change to the CacheSizeID reg
+			mrc	p15, 1, r1, c0, c0, 0		; reads current Cache Size ID register
+			and	r2, r1, #7				; extract the line length field
+			add	r2, r2, #4				; add 4 for the line length offset (log2 16 bytes)
+			ldr	r4, [pc, #H0x3ff-$-8]
+			ands	r4, r4, r1, lsr #3		; r4 is the max number on the way size (right aligned)
+			clz	r5, r4						; r5 is the bit position of the way size increment
+			ldr	r7, [pc, #H0x7fff-$-8]
+			ands	r7, r7, r1, lsr #13		; r7 is the max number of the index size (right aligned)
+		loop2:
+			mov	r9, r4					; r9 working copy of the max way size (right aligned)
+		loop3:
+			orr		r11, r10, r9, lsl r5		; factor in the way number and cache number into r11
+			orr		r11, r11, r7, lsl r2		; factor in the index number
+			mcr	p15, 0, r11, c7, c14, 2	; clean & invalidate by set/way
+			subs	r9, r9, #1				; decrement the way number
+			bge		loop3
+			subs	r7, r7, #1				; decrement the index
+			bge		loop2
+		skip:
+			add	r10, r10, #2				; increment the cache number
+			cmp	r3, r10
+			bgt		loop1
+
+		finished:
+			mov	r10, #0					; swith back to cache level 0
+			mcr	p15, 2, r10, c0, c0, 0	; select current cache level in cssr
+			dsb								; dsb
+			isb								; isb
+
+			bx	lr
+
+		H0x3ff:		d32 03FFH
+		H0x7fff:	d32 07FFFH
+	END InvalidateDCache;
+END Initializer.
+
+FoxARMInstructionSet.Disassemble A2.Bin -a=100120H ~

source/ARM.Kernel.Mod

@@ -0,0 +1,229 @@
+MODULE Kernel;	(** AUTHOR "pjm"; PURPOSE "Implementation-independent kernel interface"; *)
+
+IMPORT SYSTEM, Trace, Machine, Heaps, Modules, Objects;
+
+CONST
+	TimerFree = 0; TimerSleeping = 1; TimerWoken = 2; TimerExpired = 3;	(* Timer state *)
+
+	Second* = Machine.Second;
+
+TYPE
+	(** Finalizer for FinalizedCollection.Add. *)
+	Finalizer* = Heaps.Finalizer;	(** PROCEDURE (obj: ANY) *)
+
+	(** Enumerator for FinalizedCollection.Enumerate. *)
+	Enumerator* = PROCEDURE {DELEGATE} (obj: ANY; VAR cont: BOOLEAN);
+
+	FinalizerNode = POINTER TO RECORD (Objects.FinalizerNode)
+		nextObj {UNTRACED} : FinalizerNode;   (* in Collection c *)
+	END;
+
+	(** Polling timer. *)
+	MilliTimer* = RECORD start, target: LONGINT END;
+
+TYPE
+	(** Delay timer. *)
+	Timer* = OBJECT
+		VAR timer: Objects.Timer; state: SHORTINT;
+
+		PROCEDURE HandleTimeout;
+		BEGIN {EXCLUSIVE}
+			IF state # TimerFree THEN state := TimerExpired END
+		END HandleTimeout;
+
+		(** Delay the calling process the specified number of milliseconds or until Wakeup is called. Only one process may sleep on a specific timer at a time. *)
+		PROCEDURE Sleep*(ms: LONGINT);
+		BEGIN {EXCLUSIVE}
+			ASSERT(state = TimerFree);	(* only one process may sleep on a timer *)
+			state := TimerSleeping;
+			Objects.SetTimeout(timer, HandleTimeout, ms);
+			AWAIT(state # TimerSleeping);
+			IF state # TimerExpired THEN Objects.CancelTimeout(timer) END;
+			state := TimerFree
+		END Sleep;
+
+		(** Wake up the process sleeping on the timer, if any. *)
+		PROCEDURE Wakeup*;
+		BEGIN {EXCLUSIVE}
+			IF state = TimerSleeping THEN state := TimerWoken END
+		END Wakeup;
+
+		(** Initializer. *)
+		PROCEDURE &Init*;
+		BEGIN
+			state := TimerFree; NEW(timer)
+		END Init;
+
+	END Timer;
+
+TYPE
+	(** A collection of objects that are finalized automatically by the garbage collector. *)
+	FinalizedCollection* = OBJECT (Objects.FinalizedCollection)
+		VAR root: FinalizerNode;	(* weak list of contents linked by nextObj *)
+
+		(** Add obj to collection. Parameter fin specifies finalizer, or NIL if not required. *)	(* may be called multiple times *)
+		PROCEDURE Add*(obj: ANY; fin: Finalizer);
+		VAR n: FinalizerNode;
+		BEGIN
+			NEW(n); n.c := SELF; n.finalizer := fin;
+			Heaps.AddFinalizer(obj, n);
+			BEGIN {EXCLUSIVE}
+				n.nextObj := root.nextObj; root.nextObj := n	(* add to collection *)
+			END
+		END Add;
+
+		(** Remove one occurrence of obj from collection. *)
+		PROCEDURE Remove*(obj: ANY);
+		VAR p, n: FinalizerNode;
+		BEGIN {EXCLUSIVE}
+			p := root; n := p.nextObj;
+			WHILE (n # NIL) & (n.objWeak # obj) DO
+				p := n; n := n.nextObj
+			END;
+			IF n # NIL THEN p.nextObj := n.nextObj END;
+			(* leave in global finalizer list *)
+		END Remove;
+
+		(** Remove all occurrences of obj from collection. *)
+		PROCEDURE RemoveAll*(obj: ANY);
+		VAR p, n: FinalizerNode;
+		BEGIN {EXCLUSIVE}
+			p := root; n := p.nextObj;
+			WHILE n # NIL DO
+				IF n.objWeak = obj THEN
+					p.nextObj := n.nextObj;
+				ELSE
+					p := n;
+				END;
+				n := n.nextObj
+			END
+		END RemoveAll;
+
+		(** Enumerate all objects in the collection (Enumerator may not call Remove, Add, Enumerate or Clear). *)
+		PROCEDURE Enumerate*(enum: Enumerator);
+		VAR fn, next: FinalizerNode; cont: BOOLEAN;
+		BEGIN {EXCLUSIVE}
+			fn := root.nextObj; cont := TRUE;
+			WHILE fn # NIL DO
+				next := fn.nextObj;	(* current (or other) object may be removed by enum call *)
+				enum(fn.objWeak, cont);
+				IF cont THEN fn := next ELSE fn := NIL END
+			END
+		END Enumerate;
+		(** Enumerate all objects in the collection not being finalized (Enumerator may not call Remove, Add, Enumerate or Clear). *)
+
+		PROCEDURE EnumerateN*( enum: Enumerator );
+		VAR fn, next: FinalizerNode; cont: BOOLEAN; obj: ANY;
+		BEGIN {EXCLUSIVE}
+			fn := root.nextObj; cont := TRUE;
+			WHILE fn # NIL DO
+				next := fn.nextObj;	(* current (or other) object may be removed by enum call *)
+				obj := NIL;
+
+				Machine.Acquire( Machine.Processors );	(* prevent GC from running *)
+
+				IF (fn.objWeak # NIL ) & (fn.objStrong = NIL ) THEN (* object is not yet on the finalizers list *)
+					obj := fn.objWeak; (* now object is locally referenced, will therefore not be GCed *)
+				END;
+
+				Machine.Release(Machine.Processors);
+
+				IF obj # NIL THEN enum( obj, cont ); END;
+				IF cont THEN fn := next ELSE fn := NIL END
+			END
+		END EnumerateN;
+
+		(** Initialize new collection. May also be called to clear an existing collection. *)
+		PROCEDURE &Clear*;
+		BEGIN {EXCLUSIVE}
+			NEW(root); root.nextObj := NIL	(* head *)
+		END Clear;
+
+	END FinalizedCollection;
+
+VAR
+	second- : LONGINT;  (** number of ticks per second (Hz) *)
+
+(** Return the number of ticks since system start. For timeouts, time measurements, etc, please use MilliTimer instead.
+	Ticks increment rate is stored in "second" variable in Hz. *)
+PROCEDURE GetTicks*(): LONGINT;
+BEGIN
+	RETURN Machine.ticks;
+END GetTicks;
+
+(** -- Garbage collection -- *)
+
+(** Activate the garbage collector immediately. *)
+PROCEDURE GC*;
+BEGIN
+	Heaps.LazySweepGC
+END GC;
+
+(** -- Timers -- *)
+
+(** Set timer to expire in approximately "ms" milliseconds. *)
+PROCEDURE SetTimer*(VAR t: MilliTimer; ms: LONGINT);
+BEGIN
+	IF Second # 1000 THEN	(* convert to ticks *)
+		ASSERT((ms >= 0) & (ms <= MAX(LONGINT) DIV Second));
+		ms := ms * Second DIV 1000
+	END;
+	IF ms < 5 THEN INC(ms) END;	(* Nyquist adjustment *)
+	t.start := Machine.ticks;
+	t.target := t.start + ms
+END SetTimer;
+
+(** Test whether a timer has expired. *)
+PROCEDURE Expired*(CONST t: MilliTimer): BOOLEAN;
+BEGIN
+	RETURN Machine.ticks - t.target >= 0
+END Expired;
+
+(** Return elapsed time on a timer in milliseconds. *)
+PROCEDURE Elapsed*(CONST t: MilliTimer): LONGINT;
+BEGIN
+	RETURN (Machine.ticks- t.start) * (1000 DIV Second)
+END Elapsed;
+
+(** Return time left on a timer in milliseconds. *)
+PROCEDURE Left*(CONST t: MilliTimer): LONGINT;
+BEGIN
+	RETURN (t.target - Machine.ticks) * (1000 DIV Second)
+END Left;
+
+BEGIN
+	Machine.start := Objects.Start;
+	Machine.Timeslice := Objects.Timeslice;
+	Machine.EnableInterrupts;
+	Machine.InitProcessors;
+	Heaps.GC := Heaps.InvokeGC; (* must be done after all processors have started *)
+	IF Machine.numProcessors > 1 THEN Machine.StartAll
+	ELSE Objects.ProcessorHLT := Machine.KernelCallHLT	(* enable waiting for interrupt when idle *)
+	END;
+	Machine.SetNumberOfProcessors(Machine.numProcessors);
+	ASSERT(1000 MOD Second = 0); (* for Elapsed *)
+	second := Second;
+	Machine.Acquire(Machine.TraceOutput);
+	Trace.String("Kernel: Initialized and started."); Trace.Ln;
+	Machine.Release(Machine.TraceOutput);
+END Kernel.
+
+(**
+Notes:
+o The FinalizedCollection object implements collections of finalized objects.
+o Objects added to a finalized collection (with Add) are removed automatically by the garbage collector when no references to them exist any more. They can also be removed explicitly with Remove.
+o All the objects currently in a collection can be enumerated by Enumerate, which takes an enumerator procedure as parameter. The enumerator can also be a method in an object, which is useful when state information is required during the enumeration. The enumerator may not call other methods of the same collection.
+o An object in a finalized collection can have an finalizer procedure associated with it, which gets called by a separate process when there are no references left to the object any more. A finalizer is usually used for some cleanup functions, e.g. releasing external resources. It is executed exactly once per object. During the next garbage collector cycle the object is finally removed.
+*)
+
+(*
+to do:
+o cancel finalizer when removing object
+o fix module free race: module containing finalizer is freed. although the finalizer list is cleared, the FinalizerCaller has already taken a reference to a finalizer, but hasn't called it yet.
+o consider: a module has a FinalizedCollection, without finalizers (NIL). when the module is freed, the objects are still in the finalization list, and will get finalized in the next garbage collection. The FinalizedCollection will survive the first collection, as the objects all have references to it through their c field. After all objects have been finalized, the FinalizedCollection itself is collected. No dangling pointers occur, except the untraced module field references from the type descriptors, which are only used for tracing purposes.
+o check cyclic dependencies between finalized objects.
+o GetTime(): LONGINT - return current time in ms
+o Delay(td: LONGINT) - wait td ms
+o AwaitTime(t: LONGINT) - wait at least until time t
+o Wakeup(obj: ANY) - wake up object that is waiting
+*)

source/ARM.Machine.Mod

@@ -0,0 +1,3633 @@
+MODULE Machine; (** AUTHOR "Timothée Martiel"; PURPOSE "Machine abstraction module for ARM"; *)
+(**
+ * Machine abstraction:
+ * - processor management: caches, FPU
+ * - interrupts and exceptions
+ * - virtual memory management
+ *)
+
+IMPORT SYSTEM, Initializer, Trace, Platform, TraceDevice, BootConfig, PrivateWatchdog;
+
+CONST
+	Version = "A2 on ARM, revision 4677 (15.10.2017)";
+	DefaultObjectFileExtension* = ".Gof";
+
+	(* Interrupts Dummy Test *)
+	DummyTest = FALSE;
+	
+	(*
+		Used for compatibility with WMPerfMon
+	*)
+	MMX* = 23; (** bits in features variable *)
+
+	(* Lock levels *)
+	TraceOutput* = 0;
+	Memory* = 1;
+	Heaps* = 2;
+	Interrupts* = 3	;
+	Modules* = 4;
+	Objects* = 5;
+	Processors* = 6;
+	KernelLog* = 7;
+
+	MaxLock = 8;
+
+	(* Unit prefixes *)
+	k = 1024;
+	M = k * k;
+	G = M * k;
+	LogM = 20; (* log2(M) *)
+	(*LogK = 10;  (* log2(K) *)*)
+
+	(* CPU number *)
+	MaxCPU* = 32(*BootConfig.CpuNb*);
+
+	(* If TRUE, enables some assertions in the code *)
+	StrongChecks = TRUE;
+
+	(* Page size, in bytes *)
+	PS = 4 * k;
+	PSlog2 = 12;		(* ASH(1, PSlog2) = PS *)
+
+	PTSize = 400H;	(* size of a coarse page table *)
+	(*PTEntries = 100H;	(* 32bit entries per page table *)*)
+
+	(* Absolute maximal process stack numbers *)
+	MaxStackNb = 1024;
+
+	(* Hard limit on cache reference size *)
+	MaxCacheRefSize = M;
+
+	(* Interrupt Vector base. Valid only after MMU is enabled *)
+	InterruptVector = 0FFFF0000H;
+
+	(* Interrupts *)
+	MinIRQ* = 0;
+	MaxIRQ* = 255;
+	IRQ0* = MinIRQ; (* Alias *)
+	(** ID of the global timer IRQ *)
+	TimerIRQ* = 27;
+	PrivateTimerIRQ* = 29;
+
+	MaxIRQHandlers = 16;
+
+	(* Exceptions codes. DO NOT CHANGE: they represent interrupt vector offsets *)
+	Reset * = 20H;		(** Reset *)
+	Undef * = 24H;	(** Undefined Exception *)
+	Swi * = 28H;		(** Software Interrupt *)
+	Prefetch * = 2CH;	(** Prefetch Abort *)
+	Data * = 30H;		(** Data Abort *)
+	Irq * = 38H;		(** Interrupt Request *)
+	Fiq * = 3CH;		(** Fast Interrupt *)
+
+	(* Initial Stack size for user stacks *)
+	(*InitUserStackSize = PS;*)
+
+	HeapMin = 50;					(* "minimum" heap size as percentage of total memory size (used for heap expansion in scope of GC ) *)
+	HeapMax = 95;				(* "maximum" heap size as percentage of total memory size (used for heap expansion in scope of GC) *)
+	ExpandRate = 1;				(* always extend heap with at least this percentage of total memory size *)
+	Threshold = 10;				(* periodic GC initiated when this percentage of total memory size bytes has "passed through" NewBlock *)
+
+	(* Stack parameters *)
+	InitStackSize = PS;
+	StackGuardSize = PS;
+
+	(* access permissions *)
+	SrwUrw = 3;
+	FullAccess = (*SrwUrw*400H+SrwUrw*100H+SrwUrw*40H+*)SrwUrw*10H; (* for small pages only *)
+	LastStackPage = SrwUrw*400H+SrwUrw*100H+SrwUrw*40H; (* for small pages only *)
+
+	(* first level page table types *)
+	flCoarse = 1;
+	flSection = 2;
+
+	(* Second Level *)
+	slFault = 0;
+	slSmall = 2;
+
+	(* cachable/bufferable mapping options *)
+	cb = 0;
+	C = 8;
+	B = 4;
+	CB = C + B + 440H;
+	(* Inner and Outer Cacheable, Write-Through, no Write Allocate *)
+	Cacheable = 100CH; (* here inner cacheable, write-back, write-allocate *)
+	(* Shareable *)
+	Shareable = 10000H;
+
+	(* NIL *)
+	NilAdr* = -1;
+
+	(* Control Register Flags *)
+	DCache = 2;
+	ICache = 12;
+
+	Second* = 1000; (* frequency of ticks increments in Hz *)
+
+	Preemption* = 31;	(** flag for BreakAll() *)
+
+	(* TEMP. Temporary implementation to get Objects to compile *)
+	SSESupport* = FALSE;
+	SSE2Support* = FALSE;
+	KernelLevel * = 1;
+	UserLevel* = 1;
+	(*CS* = 1;*)
+	VMBit* = 2;
+
+	(** Period at which the CPU timer interrupts, in micro seconds *)
+	TimerPeriod* = 1000;
+
+	(** Last reboot info *)
+	RebootPowerOn * = 0;
+	RebootHardReset * = 1;
+	RebootSoftReset * = 2;
+	RebootSystemWatchdog * = 3;
+	RebootWatchdogCPU0 * = 4;
+	RebootWatchdogCPU1 * = 5;
+	RebootDebug * = 6;
+
+	(** Needed, but not used yet by Reals.Mod *)
+	fcr * = {};
+
+	IsCooperative * = FALSE;
+
+TYPE
+	(** processor state *)
+	State* = RECORD
+		R*: ARRAY 12 OF ADDRESS;		(** registers 0-11 *)
+		BP*, SP*, LR*, PC*: ADDRESS;	(** special registers *)
+		PSR*: ADDRESS;					(** processor state register *)
+		INT*: ADDRESS;					(** IRQ number *)
+	END;
+
+	(** NEON Coprocessor state *)
+	NEONState* = RECORD
+		D*: ARRAY 32 OF HUGEINT; (* 32 64-bits registers *)
+		FPSCR*: ADDRESS;
+		FPEXC*: ADDRESS;
+	END;
+
+	(** exception state *)
+	ExceptionState* = RECORD
+		halt*: LONGINT;	 	(** halt code *)
+		instn*: LONGINT;		(** undefined instruction *)
+		pf*: ADDRESS;		(** page fault address *)
+		status*: LONGINT;	(** page fault status *)
+		locks*: SET;			(** active locks *)
+	END;
+
+	(** Interrupt Hanlder *)
+	Handler* = PROCEDURE {DELEGATE} (VAR state: State);
+	EventHandler = PROCEDURE (id: LONGINT; CONST state: State);
+
+	(** Spinlock *)
+	Lock = RECORD
+		locked: BOOLEAN;
+		(* Padding to the granularity of exclusive monitor: 8 words on Cortex-A9 *)
+		pad: ARRAY 31 OF CHAR;
+	END;
+
+	(** Processor status *)
+	Proc* = RECORD
+		locksHeld*: SET;
+		preemptCount*: LONGINT;
+		(*nestCount-: LONGINT;	(** total locks held by a processor *)*)
+		intStatus*, mmu*: BOOLEAN; (* muu: if the MMU is enabled for this processor *)
+	END;
+
+	(** Virtual/physical address pair *)
+	AddressTuple = RECORD virtual, physical: ADDRESS; END;
+
+	(** Stack descriptor:
+	low:	lowest possible stack address
+	adr:	current lowest allocated address
+	high:	highest address
+	*)
+	Stack* = RECORD
+		low: ADDRESS;
+		adr*: ADDRESS;
+		high*: ADDRESS;
+	END;
+
+	(** Heap memory block *)
+	MemoryBlock* = POINTER TO MemoryBlockDesc;
+	MemoryBlockDesc* = RECORD
+		next- {UNTRACED}: MemoryBlock;
+		startAdr-: ADDRESS; 		(* unused field for I386 *)
+		size-: SIZE; 					(* unused field for I386 *)
+		beginBlockAdr-, endBlockAdr-: ADDRESS
+	END;
+
+	Address32* = ADDRESS;
+	Range* = RECORD
+		adr*: ADDRESS; size*: SIZE;
+	END;
+
+	CacheRefs = POINTER {UNSAFE,UNTRACED} TO ARRAY MaxCacheRefSize OF SHORTINT;
+VAR
+	version -: ARRAY 64 OF CHAR;
+	
+	features-: SET;	(** processor features *)
+
+	sp, fp: ADDRESS;
+
+	(** Interrupt Mask *)
+	IRQMask: SET;
+	(** IRQ Handlers. IRQ can have multiple handlers. Dispatching to those is done in IRQGlue and IRQCaller. *)
+	irqHandler: ARRAY MaxIRQ + 1, MaxIRQHandlers OF Handler;
+
+	(* Exception handlers. Called by *Glue. *)
+	undefHandler, swiHandler, prefetchHandler, dataHandler, fiqHandler: Handler;
+
+	stateTag: LONGINT;
+	dummyIRQHandler: ARRAY MaxIRQ+1 OF RECORD h: Handler; END;
+
+	(** Low level locks *)
+	lock: ARRAY MaxLock OF Lock;
+
+	(** Processors status:
+	- locksHeld:		set of low-level locks held by the processor
+	- preemptCount:	preemption counter
+	*)
+	proc-: ARRAY MaxCPU OF Proc;
+	(** IDs of all successfully started processors. *)
+	allProcessors-: SET;
+
+	heapHigh, heapLow, stackHigh, stackLow: AddressTuple;
+
+	(* Memory *)
+	pageTable: RECORD virtual, memory: ADDRESS END;	  (* contains the virtual & memory address of the first level page table *)
+	memory: RECORD size, free: SIZE; END;
+	stackPT: ADDRESS;
+
+	(* Free stack bitmap: each set element is set to indicate free stack *)
+	freeStack: ARRAY (MaxStackNb + 31) DIV 32 OF SET;
+	freeStackIndex: SIZE;
+
+	(* Address of the highest free page *)
+	freePage: ADDRESS;
+
+	(* Memory blocks -- For the heap *)
+	memBlockHead-{UNTRACED}, memBlockTail-{UNTRACED}: MemoryBlock;
+	initialMemBlock: MemoryBlockDesc;
+
+	dCacheBase: ADDRESS;
+
+	(** GC parameters *)
+	expandMin, heapMinKB, heapMaxKB : SIZE;
+	gcThreshold: SIZE;
+
+	(** For preemptive scheduling. *)
+	Timeslice*: Handler;
+	timer: EventHandler;
+
+	(** timer ticks. Written to by GetTicks. Read-only *)
+	ticks*: LONGINT;
+	eventCount, eventMax: LONGINT;
+	event: Handler;
+
+	(** Number of processor used. *)
+	numProcessors*: LONGINT;
+	numberOfProcessors: LONGINT;
+
+	(** Scheduling start procedure for non-booting processors. *)
+	start*: PROCEDURE;
+
+	(** Upcall to get current stack -- used for stack extension *)
+	getStack*: PROCEDURE (VAR stack: Stack);
+
+	(* Memory Layout *)
+	(* The system area contains:
+	 *		- the interrupt vector, which will be mapped high
+	 *		- interrupt stacks
+	 *		- the page table
+	 * Except for the first 4kB page, it is mapped 1:1.
+	 *)
+	memSysLowStart,
+	memSysLowStop,
+	(* The heap area contains:
+	 *		- the static kernel image
+	 *		- the heap
+	 * The heap is mapped 1:1 with 1MB pages.
+	 *)
+	memHeapStart,
+	memHeapStop,
+	(* Process Stacks are allocated in this area.
+	 * Stacks are mapped with 4kB pages. They take at least 2 pages:
+	 *		- 1 for the actual stack
+	 *		- 1 kept unmapped as the stack guard.
+	 * Stacks are mapped 1:1.
+	 * The stack size can be tuned to allow the system to run more processes.
+	 *)
+	memStackStart,
+	memStackStop,
+	(* Boot configuration is placed in memory. It takes 4 kB at most *)
+	memConfigStart,
+	memConfigStop,
+	(* IO registers and peripheral control are located in this region. Mapped 1:1 with 1MB and 4kB
+	 * pages, as necessary. Mapped as Device memory. *)
+	memIOStart,
+	memIOStop,
+	(* High system memory region. Within the last MB, contains: interrupt vector and reference counts
+	 * for caching. *)
+	memSysHighStart,
+	memSysHighStop,
+
+	(* System Parameters *)
+	(* Interrupt stacks. 8kB of virtual space (4kB for stack, 4kB for guard) and 4 stacks per processor. *)
+	sysIntStackStart,
+	sysIntStackStop,
+	(* First Level Page Table: size of 16 * k to map 4GB with 1MB pages. *)
+	sysFirstLvlPtStart,
+	sysFirstLvlPtStop,
+	(*
+	 * Second Level Page Table:
+	 *		- 2 * 256 entries for the system area (first and last MB of VMem)
+	 *		- 256 entries for each MB of virtual stack space
+	 * 256 entries take 1kB memory space.
+	 *)
+	sysSecondLvlPtStart,
+	sysSecondLvlPtStop,
+	(*
+	 * Interrupt Vector. Located at 0FFFFFFF0H
+	 *)
+	sysVectorStart,
+	sysVectorStop,
+	sysCacheRefStart,
+	sysCacheRefStop,
+	(*
+	 * Number of ref counters: 1 per 1st level heap page, 1 per 2nd level stack page.
+	 * This memory region is organized as follows: the first part [0 .. SysCacheStackOfs) is used for heap pages,
+	 * the second part, [SysCacheStackOfs .. SysCacheRefSize) is used for stack pages.
+	 *)
+	sysCacheRefSize,
+	(* Offset in the ref count table for stack pages. *)
+	sysCacheStackOfs: ADDRESS;
+
+	(* Process stack system *)
+	maxUserStackSize: SIZE;
+	maxUserStacks: LONGINT;
+
+	(* TRUE iff caching should be globally enabled *)
+	enableCaching: BOOLEAN;
+
+	(* UART used for kernel output *)
+	kernelOutputUart -: LONGINT;
+
+	(** Interrupt tracing option *)
+	traceInterrupts: BOOLEAN;
+	(** Trace option for CPU state *)
+	traceCpus: BOOLEAN;
+
+	(** Use private watchdog to check scheduling timers? *)
+	enableWatchdog: BOOLEAN;
+
+	(** Array of reference counts for page caching *)
+	cacheRefs -: CacheRefs;
+
+	(** Reason for last reboot *)
+	lastReboot -: LONGINT;
+
+	(* ===== Processor Management ===== *)
+	(* Current processor's ID, between 0 and MaxProc - 1 *)
+	PROCEDURE - ID*(): LONGINT;
+	CODE
+		MRC p15, 0, R0, C0, C0, 5
+		AND R0, R0, #3H; Get the last 2 bits of R0
+	END ID;
+
+	(** Enables current processor's L1 caches *)
+	PROCEDURE EnableL1Cache;
+	CODE
+		; Enable Cache and TLB maintenance broadcast
+		mrc	p15, 0, r0, c1, c0, 1
+		orr	r0, r0, #1H
+		mcr	p15, 0, r0, c1, c0, 1
+		isb
+
+		; Enable Caching in SCTLR
+		mrc	p15, 0, r0, c1, c0, 0
+		orr r0, r0, #4H
+		mcr	p15, 0, r0, c1, c0, 0
+		isb
+	END EnableL1Cache;
+
+	(** Enable L2 cache, prefetching and other speculative execution support *)
+	PROCEDURE EnableL2Cache;
+	CODE
+			ldr	r0,[pc, #L2CCCrtl-$-8]			; Load L2CC base address base + control register
+			mov	r1, #0				; force the disable bit
+			str	r1, [r0,#0]			; disable the L2 Caches
+
+			ldr	r0, [pc, #L2CCAuxCtrl-$-8]			; Load L2CC base address base + Aux control register
+			ldr	r1,[r0,#0]				; read the register
+			ldr	r2, [pc, #L2CCAuxControl-$-8]		; set the default bits
+			orr	r1,r1,r2
+			str	r1, [r0,#0]			; store the Aux Control Register
+
+			ldr	r0,[pc, #L2CCTAGLatReg-$-8]		; Load L2CC base address base + TAG Latency address
+			ldr	r1, [pc, #L2CCTAGLatency-$-8]		; set the latencies for the TAG
+			str	r1, [r0,#0]			; store the TAG Latency register Register
+
+			ldr	r0, [pc, #L2CCDataLatReg-$-8]		; Load L2CC base address base + Data Latency address
+			ldr	r1,[pc, #L2CCDataLatency-$-8]		; set the latencies for the Data
+			str	r1, [r0,#0]			; store the Data Latency register Register
+
+			ldr	r0,[pc, #L2CCWay-$-8]			; Load L2CC base address base + way register
+			ldr	r2, [pc, #H0xffff-$-8]
+			str	r2, [r0,#0]			; force invalidate
+
+			ldr	r0, [pc, #L2CCSync-$-8]			; need to poll 0x730, PSS_L2CC_CACHE_SYNC_OFFSET
+								; Load L2CC base address base + sync register
+			; poll for completion
+
+		Sync:
+			ldr	r1, [r0,#0]
+			cmp	r1, #0
+			bne	Sync
+
+			ldr	r0,[pc, #L2CCIntRaw-$-8]			; clear pending interrupts
+			ldr	r1,[r0,#0]
+			ldr	r0,[pc, #L2CCIntClear-$-8]
+			str	r1,[r0,#0]
+			ldr	r0,[pc,#L2CCCrtl-$-8]		; Load L2CC base address base + control register
+			ldr	r1,[r0,#0]					; read the register
+			mov	r2, #1					; set the enable bit
+			orr	r1,r1,r2
+			str	r1, [r0,#0]					; enable the L2 Caches
+
+			mrc	p15,0,r0,c1,c0,0		; flow prediction enable
+			orr	r0, r0, #0x800		; #0x800
+			mcr	p15,0,r0,c1,c0,0
+			isb
+
+			mrc	p15,0,r0,c1,c0,1		; read Auxiliary Control Register
+			orr	r0, r0, #4				; enable Dside prefetch
+			orr	r0, r0, #2				; enable L2 Prefetch hint
+			mcr	p15,0,r0,c1,c0,1		; write Auxiliary Control Register
+			isb
+
+			b exit
+		; Data
+		H0xffff: 			d32 0FFFFH
+		L2CCWay:			d32 0F8F02000H + 077CH
+		L2CCSync:			d32 0F8F02000H + 0730H
+		L2CCCrtl:			d32 0F8F02000H + 0100H
+		L2CCAuxCtrl:		d32 0F8F02000H + 0104H
+		L2CCTAGLatReg: 	d32 0F8F02000H + 0108H
+		L2CCDataLatReg: 	d32 0F8F02000H + 010CH
+		L2CCIntClear:		d32 0F8F02000H + 0220H
+		L2CCIntRaw:		d32 0F8F02000H + 021CH
+		L2CCAuxControl:	d32 72360000H
+		L2CCTAGLatency:	d32 0111H
+		L2CCDataLatency:	d32 0121H
+
+		exit:
+	END EnableL2Cache;
+
+	(** Enables the Snoop Control Unit
+		for L1 coherency and LDREX/STREX global monitor
+	*)
+	PROCEDURE EnableSCU;
+	CODE
+		; set scu enable bit in scu
+		ldr	r7, [pc, #H0xf8f00000-$-8]
+		ldr	r0, [r7, #0]
+		orr	r0, r0, #1
+		str	r0, [r7,#0]
+
+		; invalidate scu
+		ldr	r7, [pc, #H0xf8f0000c-$-8]
+		ldr	r6, [pc, #H0xffff-$-8]
+		str	r6, [r7, #0]
+
+		b exit
+
+	; Data
+	H0xf8f00000: 		d32 0F8F00000H
+	H0xf8f0000c: 		d32 0F8F0000CH
+	H0xffff: 			d32 0FFFFH
+	exit:
+	END EnableSCU;
+
+	(** Init NEON / VFP Engine *)
+	PROCEDURE InitFPU;
+	CODE
+		MRC p15, 0, R0, C1, C0, 2;
+		ORR R0, R0, #0x00f00000;
+		MCR p15, 0, R0, C1, C0, 2;
+		ISB
+		MOV R0, #0x40000000;
+		VMSR FPEXC, R0;
+		
+		VMRS R0, FPSCR
+		BIC R0, R0, #0x0c00000 ; round to nearest as the default
+		; remark: if we put round to minus infinity as the default, we can spare quite some instructions in emission of ENTIER
+		VMSR FPSCR, R0;
+	END InitFPU;
+
+	(** Activate the Symmetric Multiprocessing Mode for current CPU.
+		This activates L1 cache coherency.
+	*)
+	PROCEDURE SetSmpMode;
+	CODE
+		(*
+		mrc	p15, 0, r0, c1, c0, 1		/* Read ACTLR*/
+		orr	r0, r0, #(0x01 << 6)		/* set SMP bit */
+		orr	r0, r0, #(0x01 )		/* */
+		mcr	p15, 0, r0, c1, c0, 1		/* Write ACTLR*/
+		*)
+		MRC p15, 0, R0, C1, C0, 1
+		ORR R0, R0, #047H
+		MCR p15, 0, R0, C1, C0, 1
+		ISB
+	END SetSmpMode;
+
+	(** Activate Assymmetric Multiprocessing Mode for current CPU.
+		This desactivates L1 cache coherency
+	*)
+	PROCEDURE SetAmpMode;
+	CODE
+		MRC p15, 0, R0, C1, C0, 1
+		MOV R1, #040H
+		RSB R1, R1, #0
+		ORR R0, R0, R1
+		MCR p15, 0, R0, C1, C0, 1
+		ISB
+	END SetAmpMode;
+
+	(** Enable coprocessors CP10 and CP11(= VFP and NEON engine) *)
+	PROCEDURE EnableCoprocessors;
+	CODE
+		mov	r0, r0
+		mrc	p15, 0, r1, c1, c0, 2		; read cp access control register (CACR) into r1
+		orr	r1, r1, #0xf00000		; enable full access for p10 & p11
+		mcr	p15, 0, r1, c1, c0, 2		; write back into CACR
+		isb
+	END EnableCoprocessors;
+
+	(* Initializes a processor. Has to be called once by each processor. *)
+	PROCEDURE InitProcessor*;
+	BEGIN
+		timer := DummyEvent;
+		Timeslice := DummyTimeslice;
+		SetSmpMode;
+		EnableSCU;
+		EnableL1Cache;
+		InvalidateTLB;
+		InvalidateICache;
+		Initializer.InvalidateDCache;
+		(*InvalidateDCacheRange(0, SYSTEM.VAL(ADDRESS, LastAddress));*)
+		(* SCU and L2 caches are enabled in the initialization sequence *)
+		EnableL2Cache;
+		EnableCoprocessors;
+		InitFPU;
+		allProcessors := {0}
+	END InitProcessor;
+
+	(** Shut system down. If reboot is TRUE, attempts to restart system. *)
+	PROCEDURE Shutdown*(reboot: BOOLEAN);
+	VAR i: LONGINT; procs: SET;
+	BEGIN
+		IF enableWatchdog THEN PrivateWatchdog.Stop END;
+		StopTicks;
+		Trace.String("Shutting down secondary CPUs... ");
+		procs := allProcessors;
+		FOR i := 0 TO numberOfProcessors - 1 DO
+			IF (i # ID()) & (i IN allProcessors) THEN
+				EXCL(allProcessors, i)
+			END
+		END;
+		FOR i := 0 TO numberOfProcessors - 1 DO
+			IF (i #ID()) & (i IN procs) THEN
+				REPEAT	 UNTIL i IN allProcessors
+			END
+		END;
+		Trace.StringLn("done");
+
+		IF reboot THEN
+			Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey;
+			Platform.slcr.PSS_RST_CTRL := 1
+		ELSE
+			EndInterrupts;
+			LOOP
+				CODE
+					WFE
+				END;
+			END
+		END;
+	END Shutdown;
+
+	(** Shut down secondary processors *)
+	PROCEDURE ShutdownSecondary;
+	BEGIN
+		IF enableWatchdog THEN PrivateWatchdog.Stop END;
+		INCL(allProcessors, ID());
+		LOOP
+			CODE
+				WFE
+			END
+		END
+	END ShutdownSecondary;
+
+	(** Cleans the whole DCache. Taken from Minos *)
+	PROCEDURE CleanDCache *;
+	CONST
+		L2CCBBase	= 0F8F02000H;
+		L2COfs		= L2CCBBase + 7BCH;
+		L2CSync	= L2CCBBase + 730H;
+	CODE
+		; Clean all sets of all ways of L1 cache
+		MOV	R0, #0
+	WayLoop:
+		CMP	R0, #4
+		BEQ	EndWayLoop
+		LSL		R4, R0, #30
+		MOV	R1, #0
+	SetLoop:
+		CMP	R1, #256
+		BEQ	EndSetLoop
+
+		LSL		R3, R1, #5
+		ORR	R3, R4, R3
+		MCR	P15, 0, R3, C7, C10, 2
+
+		ADD	R1, R1, #1
+		B		SetLoop
+	EndSetLoop:
+		ADD	R0, R0, #1
+		B		WayLoop
+
+	EndWayLoop:
+		DSB
+
+		; Invalidate all L2 ways
+		LDR	R0, [PC, #L2COfsAdr - $ - 8]	; R0 := reg7_inv_way address
+		MOV	R1, #0FH			; R1 := 0FH => invalidate all ways
+		STR	R1, [R0, #0]		; reg7_inv_way <- R1
+
+	Sync:
+		DSB
+		LDR	R0, [PC, #L2CSyncAdr - $ - 8]	; R0 := L2 cache sync register address
+		MOV	R1, #1
+		STR	R1, [R0, #0]		; [R0] := 1
+	SyncLoop:						; repeat
+		LDR	R1, [R0, #0]		; R1 := l2 cache syc state
+		CMP	R1, #0
+		BEQ	Exit					; until R1 = 0
+		B		SyncLoop
+
+	L2COfsAdr:		d32 L2COfs
+	L2CSyncAdr:	d32 L2CSync
+	Exit:
+	END CleanDCache;
+
+	PROCEDURE FlushDCacheRange*(adr:ADDRESS; len: SIZE);
+	CONST
+		cacheline = 32;
+		L2CCBBase				= 0F8F02000H; (*XPS_L2CC_BASEADDR*)
+		L2CCCacheSync		= L2CCBBase + 00730H;		(* Cache Sync *)(*XPS_L2CC_CACHE_SYNC_OFFSET	*)
+		L2CCCacheInvClnPAOfs= 007F0H;		(* Cache Clean by PA *)(*XPS_L2CC_CACHE_INV_CLN_PA_OFFSET*)
+		L2CCOffset				= L2CCBBase + L2CCCacheInvClnPAOfs;
+	BEGIN
+		IF ~enableCaching OR (len = 0) THEN RETURN END;
+		IF len MOD cacheline # 0 THEN INC(len, cacheline - len MOD cacheline) END;
+		IF adr MOD cacheline # 0 THEN DEC(adr, len MOD cacheline) END;
+		CODE
+			LDR	R0, [FP, #adr]						; R0 := adr
+			LDR	R1, [FP, #len]						; R1 := len
+
+			LDR	R2, [PC, #Cacheline - 8 - $]		; R2 := cacheline
+			SUB	R3, R2, #1							; R3 := cacheline - 1
+			AND	R3, R0, R3							; R3 := adr MOD cacheline
+
+			ADD	R1, R1, R0
+			SUB	R0, R0, R3							; R0 := adr - adr MOD cacheline
+			;ADD	R1, R1, R3							; R1 := len + adr MOD cacheline
+
+			MOV	R3, #0
+			MCR	P15, 2, R3,  C0,  C0, 0				; Select cache level 1
+			LDR	R4, [PC, #L2COfs - 8 - $]			; R4 := L2 cache flush address register address
+
+		Loop:
+			CMP	R0, R1								; while R0 < R1
+			BEQ	Sync
+			BHI		Sync
+
+			MCR	P15, 0, R0,  C7, C14, 1				; Clean Cache Level 1 By MVA (R0)
+			STR	R0, [R4, #0]						; Clean Cache Level 2 By PA (R0)
+			DSB
+			ADD	R0, R0, R2							; R0 := R0 + cacheline
+
+			B		Loop								; end
+
+		Sync:
+			DSB
+			LDR	R0, [PC, #L2CSync - 8 - $]		; R0 := L2 cache sync register address
+			;MOV	R1, #1
+			;STR	R1, [R0, #0]						; [R0] := 1
+		SyncLoop:										; repeat
+			LDR	R1, [R0, #0]						; R1 := l2 cache syc state
+			CMP	R1, #0
+			BEQ	Exit									; until R1 = 0
+			B		SyncLoop
+
+		Cacheline:	d32 cacheline
+		L2COfs:	d32 L2CCOffset
+		L2CSync:	d32 L2CCCacheSync
+
+		Exit:
+		END;
+	END FlushDCacheRange;
+
+	PROCEDURE FlushDCachePhysRange * (adr: ADDRESS; len: LONGINT; CONST ranges: ARRAY OF Range; numRanges: LONGINT);
+	CONST
+		cacheline = 32;
+		L2CCBBase				= 0F8F02000H; (*XPS_L2CC_BASEADDR*)
+		L2CCCacheSync		= L2CCBBase + 00730H;		(* Cache Sync *)(*XPS_L2CC_CACHE_SYNC_OFFSET	*)
+		L2CCCacheInvClnPAOfs= 007F0H;		(* Cache Clean by PA *)(*XPS_L2CC_CACHE_INV_CLN_PA_OFFSET*)
+		L2CCOffset				= L2CCBBase + L2CCCacheInvClnPAOfs;
+	VAR
+		cur, end: ADDRESS;
+		r: LONGINT;
+	BEGIN
+		IF ~enableCaching & (len # 0) THEN
+			IF len MOD cacheline # 0 THEN INC(len, cacheline - len MOD cacheline) END;
+			(* Select cache L0 Data cache in CSSR *)
+			CODE
+				mov	r0, #0
+				mcr	p15, 2, r0,  c0,  c0, 0	(*		mtcp(XREG_CP15_CACHE_SIZE_SEL, 0);*)
+			END;
+
+			(* Flush all cache lines in the memory region *)
+			FOR r := 0 TO numRanges - 1 DO
+				cur := ranges[r].adr;
+				end := cur + ranges[r].size;
+				WHILE (cur < end)  DO
+					(* Flush L1 Data cache line with virtual address *)
+					CODE
+						ldr r3, [fp, #adr]    (* load*)
+						mcr	p15, 0, r3,  c7, c14, 1;				MCR XREG_CP15_CLEAN_INVAL_DC_LINE_MVA_POC :: "r" (adr));
+					END;
+					(* Flush L2 cache line with physical address *)
+					SYSTEM.PUT(L2CCOffset, cur);
+					cur := cur + cacheline;
+					adr := adr + cacheline
+				END
+			END
+		END;
+		(* Wait for L1 and L2 flush to complete *)
+		CODE
+			DSB
+		END;
+
+		SYSTEM.PUT32(L2CCCacheSync, 1);
+		REPEAT UNTIL SYSTEM.GET32(L2CCCacheSync) = 0;
+	END FlushDCachePhysRange;
+
+	PROCEDURE InvalidateDCacheRange*(adr: ADDRESS; len: SIZE);
+	CONST
+		cacheline = 32;
+		L2CCBBase				= 0F8F02000H; (*XPS_L2CC_BASEADDR*)
+		L2CCCacheSync		= L2CCBBase + 00730H;		(* Cache Sync *)(*XPS_L2CC_CACHE_SYNC_OFFSET	*)
+		L2CCCacheInvPAOfs	= 00770H;		(* Cache Invalidate by PA *)(*XPS_L2CC_CACHE_INV_CLN_PA_OFFSET*)
+		L2CCOffset = L2CCBBase + L2CCCacheInvPAOfs;
+	BEGIN
+		IF ~enableCaching OR (len = 0) THEN RETURN END;
+		IF len MOD cacheline # 0 THEN INC(len, cacheline - len MOD cacheline) END;
+		IF adr MOD cacheline # 0 THEN DEC(adr, len MOD cacheline) END;
+		CODE
+			LDR	R0, [FP, #adr]						; R0 := adr
+			LDR	R1, [FP, #len]						; R1 := len
+
+			LDR	R2, [PC, #Cacheline - 8 - $]		; R2 := cacheline
+			SUB	R3, R2, #1							; R3 := cacheline - 1
+			AND	R3, R0, R3							; R3 := adr MOD cacheline
+
+			SUB	R0, R0, R3							; R0 := adr - adr MOD cacheline
+			ADD	R1, R1, R3							; R1 := len + adr MOD cacheline
+			MOV	R5, #0								; R5 := 0 (counter value)
+
+			MOV	R3, #0
+			MCR	P15, 2, R3,  C0,  C0, 0				; Select cache level 1
+			LDR	R4, [PC, #L2COfs - 8 - $]			; R4 := L2 cache invalidate address register address
+
+		Loop:
+			CMP	R5, R1								; while R5 < R1
+			BEQ	Sync
+			BHI		Sync
+			STR	R0, [R4, #0]						; Invalidate Cache Level 2 By PA (R0)
+			DSB
+
+			MCR	P15, 0, R0,  C7, C6, 1				; Invalidate Cache Level 1 By MVA (R0)
+			ADD	R0, R0, R2							; R0 := R0 + cacheline
+			ADD	R5, R5, R2
+
+			B		Loop								; end
+
+		Sync:
+			DSB
+			LDR	R0, [PC, #L2CSync - 8 - $]		; R0 := L2 cache sync register address
+			MOV	R1, #1
+			STR	R1, [R0, #0]						; [R0] := 1
+		SyncLoop:										; repeat
+			LDR	R1, [R0, #0]						; R1 := l2 cache syc state
+			CMP	R1, #0
+			BEQ	Exit									; until R1 = 0
+			B		SyncLoop
+
+		Cacheline:	d32 cacheline
+		L2COfs:	d32 L2CCOffset
+		L2CSync:	d32 L2CCCacheSync
+
+		Exit:
+		END;
+	END InvalidateDCacheRange;
+
+	(**
+		Disable data cache for the memory range [adr, adr + len). Repeated cache disabling is recorded. A maximum of 127 successive disabling is supported.
+		Cache disabling is allowed for heap and stack memory ranges only.
+	*)
+	PROCEDURE DisableDCacheRange * (adr: ADDRESS; len: LONGINT);
+	VAR
+		range: ARRAY 1 OF Range;
+		end: ADDRESS;
+		ofs, entry: LONGINT;
+	BEGIN
+		(* Changing cache status allowed for heap and stack only. *)
+		ASSERT(((memHeapStart <= adr) & (adr + len <= memHeapStop)) OR ((memStackStart <= adr) & (adr + len <= memStackStop)));
+		end := adr + len;
+		Acquire(Memory);
+		WHILE adr < end DO
+			entry := GetFirstLevelEntry(adr - adr MOD M);
+			CASE entry MOD 4 OF
+				 0:
+					(* Page is not allocated: generate a data abort trap *)
+					Release(Memory);
+					ASSERT(entry MOD 4 # 0);
+				|flSection:
+					(* 1 MB heap page *)
+					IF enableCaching THEN
+						(* index in cache reference count array *)
+						ofs := (adr - memHeapStart) DIV M;
+						IF cacheRefs[ofs] = 0 THEN
+							(* First disabling: disable cache *)
+							range[0].adr := adr - adr MOD M;
+							range[0].size := M;
+							SetFirstLevelEntry(adr - adr MOD M, adr - adr MOD M(*SHRL(entry, 20)*), SrwUrw, Shareable + B, flSection);
+							InvalidateTLBEntry(adr);
+							FlushDCachePhysRange(adr - adr MOD M, M, range, 1);
+							cacheRefs[ofs] := 1
+						ELSE
+							(* Increase reference count and avoid overflows. *)
+							ASSERT(cacheRefs[ofs] < 127);
+							INC(cacheRefs[ofs])
+						END
+					END;
+					INC(adr, M);
+					DEC(adr, adr MOD M);
+				|flCoarse:
+					(* Second level pt *)
+					entry := GetSecondLevelEntry(adr - adr MOD PS);
+					CASE entry MOD 4 OF
+						 0:
+							(* Page is not allocated: generate a data abort trap *)
+							Release(Memory);
+							ASSERT(entry MOD 4 # 0);
+						|slSmall:
+							(* 4 kB stack page *)
+							IF enableCaching THEN
+								(* Index in cache reference count array *)
+								ofs := (adr - memStackStart) DIV PS + sysCacheStackOfs;
+								IF cacheRefs[ofs] = 0 THEN
+									(* First disabling: disable cache *)
+									range[0].adr := adr - adr MOD PS;
+									range[0].size := PS;
+									SetSecondLevelEntry(adr - adr MOD PS, SHRL(entry, PSlog2), FullAccess + 400H + B);
+									InvalidateTLBEntry(adr);
+									FlushDCachePhysRange(adr - adr MOD PS, PS, range, 1);
+									cacheRefs[ofs] := 1
+								ELSE
+									(* Increase reference count and avoid overflows *)
+									ASSERT(cacheRefs[ofs] < 127);
+									INC(cacheRefs[ofs])
+								END
+							END;
+							INC(adr, PS);
+							DEC(adr, adr MOD PS)
+					END;
+			END;
+		END;
+		Release(Memory)
+	END DisableDCacheRange;
+
+	(**
+		Enable data cache for the memory range [adr, adr + len).
+		The memory range must have been previously disabled.
+		It is the responsibility of client software to re-enable cache for the regions that it disabled.
+	*)
+	PROCEDURE EnableDCacheRange * (adr: ADDRESS; len: LONGINT);
+	VAR
+		end: ADDRESS;
+		ofs, entry: LONGINT;
+	BEGIN
+		(* Changing cache status allowed for heap and stack only. *)
+		ASSERT(((memHeapStart <= adr) & (adr < memHeapStop)) OR ((memStackStart <= adr) & (adr < memStackStop)));
+		(*InvalidateDCacheRange(adr - (adr MOD M), len + M - (adr + len) MOD M + adr MOD M);*)
+		Acquire(Memory);
+		end := adr + len;
+		WHILE adr < end DO
+			entry := GetFirstLevelEntry(SHRL(adr, LogM));
+			CASE entry MOD 4 OF
+				 0:
+					(* page not mapped: generate trap *)
+					Release(Memory);
+					ASSERT(entry MOD 4 # 0);
+				|flSection:
+					(* 1 MB heap page *)
+					IF enableCaching THEN
+						ofs := (adr - memHeapStart) DIV M;
+						ASSERT(cacheRefs[ofs] > 0);
+						IF cacheRefs[ofs] = 1 THEN
+							SetFirstLevelEntry(SHRL(adr, LogM), SHRL(entry, LogM), SrwUrw, Cacheable + Shareable, entry MOD 4);
+							InvalidateTLBEntry(adr);
+							cacheRefs[ofs] := 0
+						ELSE
+							DEC(cacheRefs[ofs])
+						END
+					END;
+					INC(adr, M);
+					DEC(adr, adr MOD M)
+				|flCoarse:
+					(* Second-level pt entry *)
+					entry := GetSecondLevelEntry(SHRL(adr, PSlog2));
+					CASE entry MOD 4 OF
+						 0:
+							(* Page not mapped: generate trap *)
+							Release(Memory);
+							ASSERT(entry MOD 4 # 0);
+						|slSmall:
+							(* 4 kB stack page *)
+							IF enableCaching THEN
+								ofs := (adr - memStackStart) DIV PS + sysCacheStackOfs;
+								ASSERT(cacheRefs[ofs] > 0);
+								IF cacheRefs[ofs] = 1 THEN
+									SetSecondLevelEntry(SHRL(adr, PSlog2), SHRL(entry, PSlog2), FullAccess + CB);
+									InvalidateTLBEntry(SHRL(adr, PSlog2));
+									cacheRefs[ofs] := 0
+								ELSE
+									DEC(cacheRefs[ofs])
+								END
+							END;
+							INC(adr, PS);
+							DEC(adr, adr MOD PS)
+					END;
+			END;
+		END;
+		Release(Memory)
+	END EnableDCacheRange;
+
+	(* InvalidateICache - invalidates the ICache. Works only in a priviledged mode. *)
+	PROCEDURE InvalidateICache*;
+	CODE
+		MCR p15, 0, R0, c7, c5, 0	; invalidate ICache & BTB
+		ISB
+		; cpwait
+		MRC p15, 0, R0, c2, c0, 0
+		MOV R0, R0
+		SUB PC, PC, #4
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+	END InvalidateICache;
+
+	(* InvalidateTLB: data and instruction TLBs - Works only in a priviledged mode *)
+	PROCEDURE - InvalidateTLB *;
+	CODE
+		MCR p15, 0, R0, c8, c3, 0	; invalidate I+D TLB
+		ISB
+		; cpwait
+		MRC p15, 0, R0, c2, c0, 0
+		MOV R0, R0
+		SUB PC, PC, #4
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+		DSB
+	END InvalidateTLB;
+
+	(* InvalidateTLBEntry - invalidates the TLB for a given virtual address. Works only in a priviledged mode *)
+	PROCEDURE - InvalidateTLBEntry(address: LONGINT);
+	CODE
+		LDR R0, [SP, #address]
+		ADD SP, SP, #4
+		;MCR p15, 0, R0, c8, c6, 1	; invalidate address
+		MCR p15, 0, R0, c8, c3, 1	; invalidate address
+		ISB
+		; cpwait
+		MRC p15, 0, R0, c2, c0, 0
+		MOV R0, R0
+		SUB PC, PC, #4
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+		DSB
+	END InvalidateTLBEntry;
+
+	(* GetControlRegister - returns the control register of coprocessor 15 *)
+	PROCEDURE -GetControlRegister(): SET;
+	CODE
+		MRC p15, 0, R0, c1, c0, 0
+	END GetControlRegister;
+
+	(* SetControlRegister - sets the control register of coprocessor 15. Works only in a priviledged mode *)
+	PROCEDURE -SetControlRegister(cr: SET);
+	CODE
+		LDR R0, [SP, #cr]
+		ADD SP, SP, #4	; remove parameter
+		MCR p15, 0, R0, c1, c0, 0
+		ISB
+		; cpwait
+		MRC p15, 0, R0, c2, c0, 0
+		MOV R0, R0
+		SUB PC, PC, #4
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+	END SetControlRegister;
+
+	(* DrainWriteBuffer - drains the write buffer. Works only in a priviledged mode *)
+	PROCEDURE DrainWriteBuffer*;
+	CODE
+		MCR p15, 0, R0, c7, c10, 4	; drain WB
+		ISB
+		; cpwait
+		MRC p15, 0, R0, c2, c0, 0
+		MOV R0, R0
+		SUB PC, PC, #4
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+		MOV R0, R0
+	END DrainWriteBuffer;
+
+	PROCEDURE -CurrentBP*(): ADDRESS;
+	CODE
+		MOV R0, FP
+	END CurrentBP;
+
+	PROCEDURE -CurrentSP*(): ADDRESS;
+	CODE
+		MOV R0, SP
+	END CurrentSP;
+
+	PROCEDURE -CurrentPC*(): ADDRESS;
+	CODE
+		MOV R0, PC
+	END CurrentPC;
+
+	PROCEDURE -SetSP*(sp: ADDRESS);
+	CODE
+		LDR R0, [SP, #sp]
+		MOV SP, R0
+		;STR SP, [SP, #sp]
+	END SetSP;
+
+	PROCEDURE -SetBP*(bp: ADDRESS);
+	CODE
+		STR FP, [SP, #bp]
+		ADD SP, SP, #4
+	END SetBP;
+
+	PROCEDURE -GetSP*(): LONGINT;
+	CODE
+		MOV R0, SP
+	END GetSP;
+
+	PROCEDURE -GetBP*(): LONGINT;
+	CODE
+		MOV R0, FP
+	END GetBP;
+
+	PROCEDURE GetTimer*(): HUGEINT;
+	VAR t: ARRAY 2 OF LONGINT;
+	BEGIN
+		REPEAT
+			t[1] := SYSTEM.GET32(Platform.GlobalTimerCounterRegister1);
+			t[0] := SYSTEM.GET32(Platform.GlobalTimerCounterRegister0);
+		UNTIL t[1] = SYSTEM.GET32(Platform.GlobalTimerCounterRegister1);
+		RETURN SYSTEM.VAL(HUGEINT,t);
+	END GetTimer;
+
+	(* ===== Multiprocessor booting ===== *)
+	(** Initializes non-booting processors *)
+	PROCEDURE InitProcessors*;
+	VAR
+		i: LONGINT;
+		val: ARRAY 8 OF CHAR;
+	BEGIN
+		GetConfig("TraceCpu", val);
+		traceCpus := val = "1";
+		InstallHandler(HandleUPTimer, PrivateTimerIRQ);
+		InstallHandler(TimerInterruptHandler, PrivateTimerIRQ);
+		InitTicks;
+		InitWatchdog;
+		CleanDCache;
+		DrainWriteBuffer;
+		FOR i := 1 TO numProcessors - 1 DO
+			StartProcessor(i, BootMP);
+		END;
+	END InitProcessors;
+
+	PROCEDURE StartAll*;
+	BEGIN
+	END StartAll;
+
+	(** Start core id on procedure p. *)
+	PROCEDURE StartProcessor(id: LONGINT; p: PROCEDURE);
+	VAR
+		time: LONGINT;
+		started: BOOLEAN;
+	BEGIN
+		IF traceCpus THEN
+			Acquire(TraceOutput);
+			Trace.String("Starting CPU");
+			Trace.Int(id, 0);
+			Trace.String(" on address ");
+			Trace.Address(SYSTEM.VAL(ADDRESS, p));
+			Trace.Ln;
+			Release(TraceOutput)
+		END;
+		time := ticks + 5000;
+		Initializer.secondaryProcId := id;
+		Initializer.secondaryBootProc :=  SYSTEM.VAL(ADDRESS, p);
+		FlushDCacheRange(ADDRESSOF(Initializer.secondaryProcId), 4);
+		FlushDCacheRange(ADDRESSOF(Initializer.secondaryBootProc), 4);
+		CODE
+			SEV
+		END;
+
+		REPEAT
+			started := id IN allProcessors;
+		UNTIL started OR (time <= ticks);
+		IF id IN allProcessors THEN
+			IF traceCpus THEN
+				Acquire(TraceOutput);
+				Trace.String("Confirm: CPU");
+				Trace.Int(id, 0);
+				Trace.StringLn(" started");
+				Release(TraceOutput)
+			END
+		ELSE
+			Acquire(TraceOutput);
+			Trace.String("WARNING: Could not start CPU");
+			Trace.Int(id, 0);
+			Trace.Ln;
+			Release(TraceOutput)
+		END
+	END StartProcessor;
+
+	(** Init Memory for non-booting processors.
+		This enables MMU and copies the mapping of CPU0
+	*)
+	PROCEDURE InitMPMemory;
+	VAR
+		tbFlags: LONGINT;
+	BEGIN
+		IF enableCaching THEN
+			tbFlags := 7BH
+		ELSE
+			tbFlags := 0
+		END;
+		EnableMM(sysFirstLvlPtStart, tbFlags, 2000H + 1007H);
+	END InitMPMemory;
+
+	(** Init code fo a non-booting processor. No local variable allowed. *)
+	PROCEDURE {NOPAF} BootMP;
+	BEGIN
+		(* Setup stack *)
+		SYSTEM.LDPSR( 0, Platform.IRQMode + Platform.FIQDisabled + Platform.IRQDisabled );
+		SYSTEM.SETSP(sysIntStackStart + 1000H * 5);
+
+		SYSTEM.LDPSR( 0, Platform.UndefMode + Platform.FIQDisabled + Platform.IRQDisabled );
+		SYSTEM.SETSP(sysIntStackStart + 1000H * 6);
+
+		SYSTEM.LDPSR( 0, Platform.AbortMode + Platform.FIQDisabled + Platform.IRQDisabled );
+		SYSTEM.SETSP(sysIntStackStart + 1000H * 7);
+
+		SYSTEM.LDPSR( 0, Platform.SVCMode + Platform.FIQDisabled + Platform.IRQDisabled );   (* Disable interrupts, init SP, FP *)
+		SYSTEM.SETSP(sysIntStackStart + 1000H * 8);
+
+		SYSTEM.LDPSR( 0, Platform.SystemMode + Platform.FIQDisabled + Platform.IRQDisabled );   (* Disable interrupts, init SP, FP *)
+		SYSTEM.SETSP(sysIntStackStart + 1000H * 8);
+
+		Initializer.InvalidateDCache;
+		SetSmpMode;
+		EnableL1Cache;
+		EnableCoprocessors;
+		InitFPU;
+		InvalidateICache;
+		Initializer.InvalidateDCache;
+		InitMPMemory;
+		(*InvalidateDCacheRange(4096, SYSTEM.VAL(ADDRESS, LastAddress));*)
+		(*InvalidateDCacheRange(memSysHighStart, memSysHighStop - memSysHighStart);*)
+		(*CODE
+			DSB
+		END;*)
+
+		InitInterrupts;
+		EnableIRQ(PrivateTimerIRQ);
+		EnableInterrupts;
+
+		IF traceCpus THEN
+			Acquire(TraceOutput);
+			Trace.String("CPU "); Trace.Int(ID(), 0); Trace.StringLn(" started.");
+			Release(TraceOutput)
+		END;
+		Acquire(Processors);
+		INCL(allProcessors, ID());
+		Release(Processors);
+		InitTicks;
+		InitWatchdog;
+		start;
+		HALT(400)
+	END BootMP;
+
+	PROCEDURE KernelCallHLT*;
+	BEGIN
+	END KernelCallHLT;
+
+	(* function returning the number of processors that are available to Aos *)
+	PROCEDURE NumberOfProcessors*( ): LONGINT;
+	BEGIN
+		RETURN numberOfProcessors
+	END NumberOfProcessors;
+
+	(*! non portable code, for native Aos only *)
+	PROCEDURE SetNumberOfProcessors*(num: LONGINT);
+	BEGIN
+		numberOfProcessors := num;
+	END SetNumberOfProcessors;
+
+	(* ===== Context Switching ===== *)
+	(** Pushes state of the processor on the stack. Used in task-switching.
+		Does not exactly represent the layout of State. Pushed data is always used by JumpState.
+	 *)
+	PROCEDURE -PushState*(CONST state: State);
+	CODE
+		LDR R0, [SP, #0]	; R0 <- address of state
+		ADD SP, SP, #8
+
+		LDR R1, [R0, #60]	; push PC
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #56]	; push LR
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #48]	; push FP
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #44]	; push R11
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #40]	; push R10
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #36]	; push R9
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #32]	; push R8
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #28]	; push R7
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #24]	; push R6
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #20]	; push R5
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #16]	; push R4
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #12]	; push R3
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #8]	; push R2
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #4]	; push R1
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #0]	; push R0
+		STR R1, [SP, #-4]!
+		LDR R1, [R0, #64]	; push SPSR
+		STR R1, [SP, #-4]!
+	END PushState;
+
+	(** Pops a processor state from the stack and restore it. Including jumping to the PC. *)
+	PROCEDURE -JumpState*;
+	CODE
+		; Load PSR
+		LDR R0, [SP], #4
+		MSR CPSR_cxsf, R0	; set CPSR
+
+		; Load registers, including branch
+		LDMIA SP!, {R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10, R11, FP, LR}
+		LDR PC, [SP], #4
+	END JumpState;
+
+	PROCEDURE CopyState*(CONST from: State; VAR to: State);
+	BEGIN
+		to.R[0] := from.R[0];
+		to.R[1] := from.R[1];
+		to.R[2] := from.R[2];
+		to.R[3] := from.R[3];
+		to.R[4] := from.R[4];
+		to.R[5] := from.R[5];
+		to.R[6] := from.R[6];
+		to.R[7] := from.R[7];
+		to.R[8] := from.R[8];
+		to.R[9] := from.R[9];
+		to.R[10] := from.R[10];
+		to.R[11] := from.R[11];
+		to.BP := from.BP;
+		to.SP := from.SP;
+		to.LR := from.LR;
+		to.PC := from.PC;
+		to.PSR := from.PSR;
+	END CopyState;
+
+	PROCEDURE -JumpToUserLevel*(userFP: ADDRESS);
+	CODE
+		; this is an inlined procedure, so the 'userFP' parameter lies on top of the stack
+		LDR	FP, [SP, #userFP]	; pop FP (FP is not a banked register)
+		ADD	SP, SP, #4
+
+		MRS	R0, CPSR			; get current PSR
+		BIC		R0, R0, #1FH		; clear bits 4:0
+		ORR	R0, R0, #1FH		; CPU mode = System
+		MSR	CPSR_c, R0			; switch mode
+
+		MOV	SP, FP
+		LDMIA	SP! ,  {FP, LR}
+		BX		LR
+
+	END JumpToUserLevel;
+
+	PROCEDURE UpdateState*;
+	BEGIN
+	END UpdateState;
+
+	(** Save complete VFP/NEON state *)
+	PROCEDURE -FPUSaveFull*(VAR state: NEONState);
+	CODE
+		LDR R0, [SP, #state]
+		ADD SP, SP, #8
+
+		VST1 D0, R0
+		ADD R0, R0, #8
+		VST1 D1, R0
+		ADD R0, R0, #8
+		VST1 D2, R0
+		ADD R0, R0, #8
+		VST1 D3, R0
+		ADD R0, R0, #8
+		VST1 D4, R0
+		ADD R0, R0, #8
+		VST1 D5, R0
+		ADD R0, R0, #8
+		VST1 D6, R0
+		ADD R0, R0, #8
+		VST1 D7, R0
+		ADD R0, R0, #8
+		VST1 D8, R0
+		ADD R0, R0, #8
+		VST1 D9, R0
+		ADD R0, R0, #8
+		VST1 D10, R0
+		ADD R0, R0, #8
+		VST1 D11, R0
+		ADD R0, R0, #8
+		VST1 D12, R0
+		ADD R0, R0, #8
+		VST1 D13, R0
+		ADD R0, R0, #8
+		VST1 D14, R0
+		ADD R0, R0, #8
+		VST1 D15, R0
+		ADD R0, R0, #8
+		
+		VSTR D16, R0, #0
+		ADD R0, R0, #8
+		VSTR D17, R0, #0
+		ADD R0, R0, #8
+		VSTR D18, R0, #0
+		ADD R0, R0, #8
+		VSTR D19, R0, #0
+		ADD R0, R0, #8
+		VSTR D20, R0, #0
+		ADD R0, R0, #8
+		VSTR D21, R0, #0
+		ADD R0, R0, #8
+		VSTR D22, R0, #0
+		ADD R0, R0, #8
+		VSTR D23, R0, #0
+		ADD R0, R0, #8
+		VSTR D24, R0, #0
+		ADD R0, R0, #8
+		VSTR D25, R0, #0
+		ADD R0, R0, #8
+		VSTR D26, R0, #0
+		ADD R0, R0, #8
+		VSTR D27, R0, #0
+		ADD R0, R0, #8
+		VSTR D28, R0, #0
+		ADD R0, R0, #8
+		VSTR D29, R0, #0
+		ADD R0, R0, #8
+		VSTR D30, R0, #0
+		ADD R0, R0, #8
+		VSTR D31, R0, #0
+		ADD R0, R0, #8
+
+		VMRS	R1, FPSCR
+		STR	R1, [R0, #0]
+		ADD	R0, R0, #4
+		VMRS	R1, FPEXC
+		STR	R1, [R0, #0]
+	END FPUSaveFull;
+
+	(** Save minimal VFP/NEON state *)
+	PROCEDURE -FPUSaveMin*(VAR state: NEONState);
+	CODE
+		ADD SP, SP, #4
+	END FPUSaveMin;
+
+	(** Restore full VFP/NEON state *)
+	PROCEDURE -FPURestoreFull*(VAR state: NEONState);
+	CODE
+		LDR R0, [SP, #state];
+		ADD SP, SP, #8
+
+		VLD1 D0, R0
+		ADD R0, R0, #8
+		VLD1 D1, R0
+		ADD R0, R0, #8
+		VLD1 D2, R0
+		ADD R0, R0, #8
+		VLD1 D3, R0
+		ADD R0, R0, #8
+		VLD1 D4, R0
+		ADD R0, R0, #8
+		VLD1 D5, R0
+		ADD R0, R0, #8
+		VLD1 D6, R0
+		ADD R0, R0, #8
+		VLD1 D7, R0
+		ADD R0, R0, #8
+		VLD1 D8, R0
+		ADD R0, R0, #8
+		VLD1 D9, R0
+		ADD R0, R0, #8
+		VLD1 D10, R0
+		ADD R0, R0, #8
+		VLD1 D11, R0
+		ADD R0, R0, #8
+		VLD1 D12, R0
+		ADD R0, R0, #8
+		VLD1 D13, R0
+		ADD R0, R0, #8
+		VLD1 D14, R0
+		ADD R0, R0, #8
+		VLD1 D15, R0
+		ADD R0, R0, #8
+
+		VLDR D16, R0, #0
+		ADD R0, R0, #8
+		VLDR D17, R0, #0
+		ADD R0, R0, #8
+		VLDR D18, R0, #0
+		ADD R0, R0, #8
+		VLDR D19, R0, #0
+		ADD R0, R0, #8
+		VLDR D20, R0, #0
+		ADD R0, R0, #8
+		VLDR D21, R0, #0
+		ADD R0, R0, #8
+		VLDR D22, R0, #0
+		ADD R0, R0, #8
+		VLDR D23, R0, #0
+		ADD R0, R0, #8
+		VLDR D24, R0, #0
+		ADD R0, R0, #8
+		VLDR D25, R0, #0
+		ADD R0, R0, #8
+		VLDR D26, R0, #0
+		ADD R0, R0, #8
+		VLDR D27, R0, #0
+		ADD R0, R0, #8
+		VLDR D28, R0, #0
+		ADD R0, R0, #8
+		VLDR D29, R0, #0
+		ADD R0, R0, #8
+		VLDR D30, R0, #0
+		ADD R0, R0, #8
+		VLDR D31, R0, #0
+		ADD R0, R0, #8
+
+		LDR	R1, [R0, #0]
+		VMSR	FPSCR, R1
+		ADD	R0, R0, #4
+		LDR	R1, [R0, #0]
+		VMSR	FPEXC, R1
+	END FPURestoreFull;
+
+	(** Restore minimal VFP/NEON state *)
+	PROCEDURE -FPURestoreMin*(VAR state: NEONState);
+	CODE
+		ADD SP, SP, #4
+	END FPURestoreMin;
+
+	(* ===== Interrupts ===== *)
+	(* Taken from Minos/Kernel.Mos *)
+	PROCEDURE EnableInterrupts*;
+	VAR cpsr: LONGINT;
+	BEGIN
+		SYSTEM.STPSR(0, cpsr);
+		cpsr := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, cpsr) - {7(*, 8*)});
+		SYSTEM.LDPSR(0, cpsr);
+		(*SYSTEM.PUT32(Platform.ICDDCR, {EnableSecure, EnableNonSecure});*)
+	END EnableInterrupts;
+
+	(* Taken from Minos/Kernel.Mos *)
+	PROCEDURE DisableInterrupts*;
+	VAR cpsr: LONGINT;
+	BEGIN
+		SYSTEM.STPSR(0, cpsr);
+		cpsr := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, cpsr) + {7(*, 8*)});
+		SYSTEM.LDPSR( 0, cpsr);
+		(*SYSTEM.PUT32(Platform.ICDDCR, {});*)
+	END DisableInterrupts;
+
+	(** AreInterruptsEnabled - returns TRUE if interrupts are enabled at the current processor level, FALSE otherwise *)
+	PROCEDURE AreInterruptsEnabled*(): BOOLEAN;
+	CODE
+		MRS	R0, CPSR	; move CPSR to R0
+		TST	R0, #80H	; was IBit set ?
+		MOVEQ	R0, #1	; no, interrupts are enabled
+		MOVNE	R0, #0	; yep, interrupts are disabled
+	END AreInterruptsEnabled;
+
+	(* InstallDefaultInterrupts - installs default interrupt handlers *)
+	PROCEDURE InstallDefaultInterrupts;
+		VAR p: PROCEDURE; base: ADDRESS;
+		i, int: LONGINT;
+	BEGIN
+		base := 0;
+		(* Install all *Glue procedures. *)
+		p := undefGlue; SYSTEM.PUT32(base + Undef, SYSTEM.VAL(LONGINT, p));
+		p := SWIGlue; SYSTEM.PUT32(base + Swi, SYSTEM.VAL(LONGINT, p));
+		p := prefetchGlue; SYSTEM.PUT32(base + Prefetch, SYSTEM.VAL(LONGINT, p));
+		p := dataGlue; SYSTEM.PUT32(base + Data, SYSTEM.VAL(LONGINT, p));
+		p := DefaultIRQ; SYSTEM.PUT32(base + Irq, SYSTEM.VAL(LONGINT, p));
+		p := fiqGlue; SYSTEM.PUT32(base + Fiq, SYSTEM.VAL(LONGINT, p));
+
+		(* Install default exception handlers *)
+		InstallExceptionHandler(DefaultUndefined, Undef);
+		InstallExceptionHandler(DefaultSWI, Swi);
+		InstallExceptionHandler(DefaultPrefetchAbort, Prefetch);
+		InstallExceptionHandler(DefaultDataAbort, Data);
+		InstallExceptionHandler(DefaultFIQ, Fiq);
+
+		FOR int := 0 TO MaxIRQ DO
+			FOR i := 0 TO MaxIRQHandlers -1 DO
+				irqHandler[int, i] := NIL
+			END
+		END;
+	END InstallDefaultInterrupts;
+
+	(* DefaultUndef - default handler for undefined instruction exceptions *)
+	PROCEDURE DefaultUndefined (VAR state: State);
+	VAR
+		instn: LONGINT;
+	BEGIN
+		instn := SYSTEM.GET32(state.PC);
+
+		Acquire(TraceOutput);
+		Trace.Ln;
+		Trace.StringLn("Undefined Instruction Trap:");
+		Trace.String("  pc: "); Trace.Address(state.PC); Trace.Ln;
+		Trace.String("  instruction: "); Trace.Hex(instn, -8); Trace.Ln;
+		Trace.String("  CPU: "); Trace.Int(ID(), 0); Trace.Ln;
+
+		TraceState(state);
+		Trace.String("Kernel Halted");
+		Release(TraceOutput);
+		LOOP END
+	END DefaultUndefined;
+
+	(* DefaultSWI - default handler for software interrupts *)
+	PROCEDURE DefaultSWI (VAR state: State);
+	BEGIN
+		Acquire(TraceOutput);
+		Trace.Ln;
+		Trace.StringLn("Software Interrupt:");
+		Trace.String("  pc: "); Trace.Address(state.PC); Trace.Ln;
+		Trace.String("  number: "); Trace.Int(state.INT, -8); Trace.Ln;
+		Trace.String("  CPU: "); Trace.Int(ID(), 0); Trace.Ln;
+
+		TraceState(state);
+		Trace.Ln;
+		Trace.String("Kernel halted.");
+		Release(TraceOutput);
+		LOOP END
+	END DefaultSWI;
+
+	(* Instruction Prefetch abort *)
+	PROCEDURE DefaultPrefetchAbort (VAR state: State);
+	BEGIN
+		Acquire(TraceOutput);
+		Trace.String("Prefetch abort at location: "); Trace.Address(state.PC); Trace.Ln;
+		Trace.String(" CPU: "); Trace.Int(ID(), 0); Trace.Ln;
+		Trace.String("  FP: "); Trace.Address(state.BP); Trace.Ln;
+		Trace.String("SPSR: "); Trace.Hex(state.PSR, -8); Trace.Ln;
+
+		TraceState(state);
+		Trace.Ln;
+		Trace.StringLn("Kernel Halted");
+		Release(TraceOutput);
+		LOOP END;
+	END DefaultPrefetchAbort;
+
+	(* DefaultDataAbort - default handler for data abort exceptions *)
+	PROCEDURE DefaultDataAbort (VAR state: State);
+	VAR
+		faultAdr: ADDRESS;
+		faultStatus: LONGINT;
+		stack: Stack;
+	BEGIN
+		GetPageFault(faultAdr, faultStatus);
+		(*getStack(stack);
+
+		IF ~ExtendStack(stack, faultAdr) THEN*)
+			Acquire(TraceOutput);
+			IF faultAdr < 4 * k THEN
+				Trace.StringLn("NIL pointer exception");
+				Trace.String("pc:	"); Trace.Address(state.PC); Trace.Ln
+			ELSE
+				Trace.StringLn("Data Abort Trap");
+				Trace.String("pc:		"); Trace.Address(state.PC); Trace.Ln;
+				Trace.String("instn:	"); Trace.Address(SYSTEM.GET32(state.PC)); Trace.Ln;
+				Trace.String("address:	"); Trace.Address(faultAdr); Trace.Ln;
+				Trace.String("status:	"); Trace.Address(faultStatus); Trace.Ln
+			END;
+
+			TraceState(state);
+			Trace.Ln; Trace.StringLn("Kernel Halted.");
+			Release(TraceOutput);
+			LOOP END
+		(*END*)
+	END DefaultDataAbort;
+
+	(* DefaultIRQ - default handler for IRQs *)
+	PROCEDURE DefaultIRQ;
+	BEGIN
+		Acquire(TraceOutput);
+		Trace.StringLn("(IRQ)");
+		Trace.String("  CPU: "); Trace.Address(ID()); Trace.Ln;
+
+		Trace.Ln; Trace.StringLn("Kernel Halted");
+		Release(TraceOutput);
+		LOOP END
+	END DefaultIRQ;
+
+	(* DefaultFIQ - default handler for fast interrupts *)
+	(*PROCEDURE DefaultFIQ;
+	BEGIN
+		Trace.StringLn("Fast IRQ Trap");
+		Trace.String("  CPU: "); Trace.Address(ID()); Trace.Ln;
+		Trace.String("Kernel halted.");
+		LOOP END
+	END DefaultFIQ;*)
+	PROCEDURE DefaultFIQ (VAR state: State);
+	BEGIN
+		Acquire(TraceOutput);
+		Trace.StringLn("Fast IRQ Trap");
+		Trace.String("  CPU: "); Trace.Address(ID()); Trace.Ln;
+		Trace.String("Kernel halted.");
+		Release(TraceOutput);
+		LOOP END
+	END DefaultFIQ;
+
+	PROCEDURE DummyISR(VAR state: State);
+	VAR i: LONGINT; icip : SET;
+	BEGIN
+		icip := SYSTEM.VAL(SET, SYSTEM.GET32((*IC +*) Platform.ICIP));
+		FOR i:=MinIRQ TO MaxIRQ DO
+			IF i IN icip THEN
+				state.INT := i;
+				dummyIRQHandler[state.INT].h(state);
+			END;
+		END;
+	END DummyISR;
+
+	(** EnableIRQ - enables a hardware interrupt (also done automatically by InstallHandler) *)
+	PROCEDURE InEnableIRQ(num: LONGINT);
+	BEGIN
+		ASSERT((MinIRQ <= num) & (num<= MaxIRQ));
+		(*IF TRUE OR (num = 53) THEN Trace.StringLn("Enable USB IRQ") END;*)
+		SYSTEM.PUT32(Platform.ICDISER + 4 * (num DIV 32) , {num MOD 32});
+	END InEnableIRQ;
+
+	PROCEDURE EnableIRQ*(int: LONGINT);
+	BEGIN
+		Acquire(Interrupts);
+		InEnableIRQ(int);
+		Release(Interrupts)
+	END EnableIRQ;
+
+	(** DisableIRQ - disables a hardware interrupt (also done automatically by RemoveHandler) *)
+	PROCEDURE InDisableIRQ(num: LONGINT);
+	BEGIN
+		ASSERT((MinIRQ <= num) & (num <= MaxIRQ));
+		(*IF TRUE OR (num = 53) THEN Trace.StringLn("Disable USB IRQ") END;*)
+		SYSTEM.PUT32(Platform.ICDICER + 4 * (num DIV 32) , {num MOD 32});
+	END InDisableIRQ;
+
+	PROCEDURE DisableIRQ*(int: LONGINT);
+	BEGIN
+		Acquire(Interrupts);
+		InDisableIRQ(int);
+		Release(Interrupts)
+	END DisableIRQ;
+
+	PROCEDURE IsIRQEnabled(int: LONGINT): BOOLEAN;
+	VAR
+		enabled: BOOLEAN;
+		reg: SET;
+	BEGIN
+		Acquire(Interrupts);
+		SYSTEM.GET(Platform.ICDISER + 4 * (int DIV 32) , reg);
+		enabled := (int MOD 32) IN reg;
+		Release(Interrupts);
+		RETURN enabled
+	END IsIRQEnabled;
+
+	(** InstallHandler - installs a interrupt handler & enable IRQ if necessary.
+		On entry to h interrupts are disabled and may be enabled with XXXXX.  After handling the interrupt
+		the state of interrupts are restored.  The acknowledgement of a hardware interrupt is done automatically.
+		IRQs are mapped from MinIRQ to MaxIRQ. *)
+	PROCEDURE InstallHandler*(h: Handler; int: LONGINT);
+	VAR
+		i: LONGINT;
+	BEGIN
+		(*NEW(n);*)	(* outside locked region, to allow gc *)
+		i := 0;
+		WHILE irqHandler[int, i] # NIL DO
+			INC(i)
+		END;
+
+		Acquire(Interrupts);
+			(* IRQGlue may traverse list while it is being modified. *)
+		irqHandler[int, i] := h;
+
+		IF DummyTest THEN
+			irqHandler[int, i] := DummyISR;
+			dummyIRQHandler[int].h := h;
+		END;
+
+ 		IF (int >= MinIRQ) & (int <= MaxIRQ) THEN InEnableIRQ(int) END;
+
+		Release(Interrupts);
+
+		IF traceInterrupts THEN
+			Acquire(TraceOutput);
+			Trace.String("[Machine]InstallHandler: h = 0x"); Trace.Address(SYSTEM.VAL(LONGINT, h));
+			Trace.String("; int = "); Trace.Address(int);
+			Trace.String("; IRQMask = 0x"); Trace.Address(SYSTEM.VAL(LONGINT, IRQMask));
+			Trace.Ln;
+			Release(TraceOutput)
+		END
+	END InstallHandler;
+
+	PROCEDURE RemoveHandler * (h: Handler; int: LONGINT);
+	END RemoveHandler;
+
+	PROCEDURE InstallExceptionHandler * (h: Handler; e: LONGINT);
+	BEGIN
+		CASE e OF
+			 Undef: undefHandler := h
+			|Swi: swiHandler := h
+			|Prefetch: prefetchHandler := h
+			|Data: dataHandler := h
+			|Fiq: fiqHandler := h
+		ELSE
+			Trace.String("Unknown exception offset: ");
+			Trace.Int(e, 0);
+			Trace.Ln;
+			HALT(99)
+		END
+	END InstallExceptionHandler;
+
+	(*
+		IRQGlue - every IRQ enters through this handler. It reads the IRR (Interrupt Request Register) of the PIC and calls the
+		appropriate handlers for each pending IRQ. A problem (which caused a nice debugging session of ~1 day...) is this
+		'VAR state: State' parameter. Theoretically, a interrupt handler can change the values of this field to continue execution in
+		a different process, e.g. the scheduler works that way. This means that we can't call the handlers of every pending IRQ in
+		one loop - we must exit this procedure after each handled IRQ.
+
+		- routine has been changed so that all handlers of pending interrupts will be called
+		- using ICIP (interrupt Pending Register) instead of IRR (Interrupt Request Register)
+		- after handler call we don't turn interrupts off explicitly
+	*)
+	PROCEDURE {NOPAF} IRQGlue;
+	CODE
+		CLREX
+		SUB		SP, SP, #72						; Room for the State record
+		STMIA		SP, {R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10, R11, FP, SP, LR}^
+		MOV		R0, R0
+		ADD		SP, SP, #60
+		SUB		R0, LR, #4						; return address = LR-4
+		STR		R0, [SP], #4					; push ('PC' in 'State'). SP points to offset 64
+		MRS		R0, SPSR						; get saved PSR
+		STR		R0, [SP], #4					; push ('PSR' in 'State'). SP points to offset 68
+
+		SUB		SP, SP, #68
+		MOV		R11, SP
+
+		LDR		R5, [pc, #state-$-8]			; load address of stateTag constant
+		STR		R5, [SP, #-4]!					; push value (type tag)
+		STR		R11, [SP, #-4]!					; push parameter (address of 'State' parameter)
+
+		BL			IRQCaller
+		LDR		R11, [SP], #4
+		ADD		SP, SP, #4
+
+		ADD		SP, SP, #72						; adjust SP & remove PAF
+		LDR		R0, [R11, #64]					; load 'State.PSR'
+		MSR		SPSR_cxsf, R0					; and store it into SPSR
+		LDR		R0, [R11, #60]					; load 'State.PC'...
+		MOV		LR, R0							; ...into LR (because we will return to LR, not PC)
+		ADD		R0, R11, #48					; R0 points to 'State.SP'
+		LDMIA		R0, { FP, SP, LR }^				; load 'State.SP' & 'State.LR' into user mode registers
+		MOV		R0, R0							; nop, to not access banked registers after LDM ^
+		LDMIA		R11, { R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10 } 				; restore unbanked regs
+		LDR		R11, [R11, #44]
+
+		MOVS		PC, LR							; SPSR -> CPSR & return
+
+		; Data section
+	state:	d32 stateTag							; address of stateTag
+	END IRQGlue;
+
+	(** Calls all handlers for all pending IRQs.
+		Is called by IRQGlue.
+	*)
+	PROCEDURE IRQCaller(VAR state: State);
+	VAR i, reg, irq, ack, count: LONGINT;  handler: Handler;  icip: SET;
+	BEGIN
+		ack := SYSTEM.GET32(Platform.ICCIAR);
+		(* service this irq *)
+		irq := ack MOD 1024;
+		IF irq # 1023 THEN (* not a spurious IRQ *)
+			state.INT := irq;
+			IF (MinIRQ <= irq) & (irq<= MaxIRQ) THEN
+				count := 0;
+				handler := irqHandler[irq, count];
+				WHILE (handler # NIL) & (count < MaxIRQHandlers - 1) DO
+					handler(state);
+					DisableInterrupts; (* handler may have reactivated interrupts *)
+					INC(count);
+					handler := irqHandler[irq, count];
+				END;
+				SYSTEM.PUT32(Platform.ICCEOIR, ack);
+			END;
+		END;
+
+		(* service pending IRQs *)
+		FOR reg := 0 TO 2 DO
+			SYSTEM.GET( Platform.ICDISPR+reg*4, icip );
+			i := 0;
+			WHILE (i <= 31) & (icip # {}) DO
+				IF (i IN icip) THEN
+					icip := icip - {i};
+					irq := i+reg*32;
+
+					(* Do not service pending interrupts that are disabled: this allows state triggered interrupts to be
+					 * handled by software in an interrupt process. *)
+					IF IsIRQEnabled(irq) & (irq # PrivateTimerIRQ) THEN
+						(*Trace.String("Pending IRQ "); Trace.Int(irq, 0); Trace.Ln;*)
+						count := 0;
+						state.INT := irq;
+						handler := irqHandler[irq, count];
+						WHILE (handler # NIL) & (count < MaxIRQHandlers - 1) DO
+							handler(state);
+							DisableInterrupts; (* handler may have reactivated interrupts *)
+							INC(count);
+							handler := irqHandler[irq, count]
+						END;
+						SYSTEM.PUT32(Platform.ICCEOIR, irq); (* end of interrupt *)
+						SYSTEM.PUT32(Platform.ICDICPR+reg*4, {i}); (* clear pending bit *)
+					END
+				END;
+				INC( i )
+			END
+		END;
+	END IRQCaller;
+
+	(** Undefined Exception Handler. Saves the processor state and calls the registered handler. *)
+	PROCEDURE {NOPAF} undefGlue;
+	CODE
+		CLREX
+		SUB		SP, SP, #72						; Room for the State record
+		STMIA		SP, {R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10, R11, FP, SP, LR}^
+		MOV		R0, R0
+		ADD		SP, SP, #60
+		SUB		R0, LR, #4						; return address = LR-4
+		STR		R0, [SP], #4					; push ('PC' in 'State'). SP points to offset 64
+		MRS		R0, SPSR						; get saved PSR
+		STR		R0, [SP], #4					; push ('PSR' in 'State'). SP points to offset 68
+		LDR		R0, [PC, #undefined-8-$]		; save -Undef in the INT field
+		STR		R0, [SP], #4
+
+		SUB		SP, SP, #72
+		MOV		R11, SP
+
+		LDR		R5, [pc, #state-$-8]			; load address of stateTag constant
+		STR		R5, [SP, #-4]!					; push value (type tag)
+		STR		R11, [SP, #-4]!					; push parameter (address of 'State' parameter)
+
+		LDR		R0, [PC,#handler-8-$]
+		LDR		R0, [R0, #0]
+		BLX		R0
+		LDR		R11, [SP], #4
+		ADD		SP, SP, #4
+
+		ADD		SP, SP, #72						; adjust SP & remove PAF
+		LDR		R0, [R11, #64]					; load 'State.PSR'
+		MSR		SPSR_cxsf, R0					; and store it into SPSR
+		LDR		R0, [R11, #60]					; load 'State.PC'...
+		MOV		LR, R0							; ...into LR (because we will return to LR, not PC)
+		ADD		R0, R11, #48					; R0 points to 'State.SP'
+		LDMIA		R0, { FP, SP, LR }^				; load 'State.SP' & 'State.LR' into user mode registers
+		MOV		R0, R0							; nop, to not access banked registers after LDM ^
+		LDMIA		R11, { R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10 } 				; restore unbanked regs
+		LDR		R11, [R11, #44]
+
+		MOVS		PC, LR							; SPSR -> CPSR & return
+
+		; Data section
+	state:			d32 stateTag					; address of stateTag
+	handler:		d32 undefHandler				; handler
+	undefined:		d32 -Undef					; INT number for undefined instructions
+	END undefGlue;
+
+	(**
+	 * Software Interrupt Handler. Saves the processor state and calls the registered handler.
+	 * The SWI number is stored into the INT field of the state record.
+	 *)
+	PROCEDURE {NOPAF} SWIGlue;
+	CODE
+		SUB		SP, SP, #72						; Room for the State record
+		STMIA		SP, {R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10, R11, FP, SP, LR}^
+		MOV		R0, R0
+		ADD		SP, SP, #60
+		SUB		R1, LR, #4						; return address = LR-4
+		STR		R1, [SP], #4					; push ('PC' in 'State'). SP points to offset 64
+		MRS		R0, SPSR						; get saved PSR
+		STR		R0, [SP], #4					; push ('PSR' in 'State'). SP points to offset 68
+		LDR		R0, [R1, #0]					; Load SWI instruction
+		LDR		R2, [PC, #HFFFFFF-8-$]
+		AND		R0, R0, R2						; R0 MOD 1000000 = SWI number
+		STR		R0, [SP], #4					; push SWI number as INT. SP points to offset 72
+
+		SUB		SP, SP, #72						; Update SP to correct location
+		MOV		R11, SP
+
+		LDR		R5, [pc, #state-$-8]			; load address of stateTag constant
+		STR		R5, [SP, #-4]!					; push value (type tag)
+		STR		R11, [SP, #-4]!					; push parameter (address of 'State' parameter)
+
+		LDR		R0, [PC,#handler-8-$]
+		LDR		R0, [R0, #0]
+		BLX		R0
+		LDR		R11, [SP], #4
+		ADD		SP, SP, #4
+
+		ADD		SP, SP, #72						; adjust SP & remove PAF
+		LDR		R0, [R11, #64]					; load 'State.PSR'
+		MSR		SPSR_cxsf, R0					; and store it into SPSR
+		LDR		R0, [R11, #60]					; load 'State.PC'...
+		MOV		LR, R0							; ...into LR (because we will return to LR, not PC)
+		ADD		R0, R11, #48					; R0 points to 'State.SP'
+		LDMIA		R0, { FP, SP, LR }^				; load 'State.SP' & 'State.LR' into user mode registers
+		MOV		R0, R0							; nop, to not access banked registers after LDM ^
+		LDMIA		R11, { R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10 } 				; restore unbanked regs
+		LDR		R11, [R11, #44]
+
+		MOVS		PC, LR							; SPSR -> CPSR & return
+
+		; Data section
+	state:		d32 stateTag						; address of stateTag
+	HFFFFFF:	d32 0FFFFFFH						; SWI number mask
+	handler:	d32 swiHandler					; swiHandler
+	END SWIGlue;
+
+	(**
+	 * Prefetch Abort Handler. Saves the processor state and calls the registered handler.
+	 * The PC field of the state record holds the address at which the prefetch fault occurred.
+	 *)
+	PROCEDURE {NOPAF} prefetchGlue;
+	CODE
+		CLREX
+		SUB		SP, SP, #72						; Room for the State record
+		STMIA		SP, {R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10, R11, FP, SP, LR}^
+		MOV		R0, R0
+		ADD		SP, SP, #60
+		SUB		R0, LR, #4						; return address = LR-4
+		STR		R0, [SP], #4					; push ('PC' in 'State'). SP points to offset 64
+		MRS		R0, SPSR						; get saved PSR
+		STR		R0, [SP], #4					; push ('PSR' in 'State'). SP points to offset 68
+		LDR		R0, [PC, #prefetchAbort-8-$]		; save -Data in the INT field
+		STR		R0, [SP], #4
+
+		SUB		SP, SP, #72
+		MOV		R11, SP
+
+		LDR		R5, [pc, #state-$-8]			; load address of stateTag constant
+		STR		R5, [SP, #-4]!					; push value (type tag)
+		STR		R11, [SP, #-4]!					; push parameter (address of 'State' parameter)
+
+		LDR		R0, [PC,#handler-8-$]
+		LDR		R0, [R0, #0]
+		BLX		R0
+		LDR		R11, [SP], #4
+		ADD		SP, SP, #4
+
+		ADD		SP, SP, #72						; adjust SP & remove PAF
+		LDR		R0, [R11, #64]					; load 'State.PSR'
+		MSR		SPSR_cxsf, R0					; and store it into SPSR
+		LDR		R0, [R11, #60]					; load 'State.PC'...
+		MOV		LR, R0							; ...into LR (because we will return to LR, not PC)
+		ADD		R0, R11, #48					; R0 points to 'State.SP'
+		LDMIA		R0, { FP, SP, LR }^				; load 'State.SP' & 'State.LR' into user mode registers
+		MOV		R0, R0							; nop, to not access banked registers after LDM ^
+		LDMIA		R11, { R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10 } 				; restore unbanked regs
+		LDR		R11, [R11, #44]
+
+		MOVS		PC, LR							; SPSR -> CPSR & return
+
+		; Data section
+	state:			d32 stateTag					; address of stateTag
+	handler:		d32 prefetchHandler			; handler
+	prefetchAbort:	d32 -Prefetch					; prefetch INT number
+	END prefetchGlue;
+
+	(**
+	 * Data Abort Handler. Saves the processor state and calls the registered handler.
+	 * Use procedure GetPageFault to get abort status and address.
+	 *)
+	PROCEDURE {NOPAF} dataGlue;
+	CODE
+		CLREX
+		SUB		SP, SP, #72						; Room for the State record
+		STMIA		SP, {R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10, R11, FP, SP, LR}^
+		MOV		R0, R0
+		ADD		SP, SP, #60
+		SUB		R0, LR, #8						; return address = LR-8
+		STR		R0, [SP], #4					; push ('PC' in 'State'). SP points to offset 64
+		MRS		R0, SPSR						; get saved PSR
+		STR		R0, [SP], #4					; push ('PSR' in 'State'). SP points to offset 68
+		LDR		R0, [PC, #dataAbort-8-$]		; save -Data in the INT field
+		STR		R0, [SP], #4
+
+		SUB		SP, SP, #72
+		MOV		R11, SP
+
+		LDR		R5, [pc, #state-$-8]			; load address of stateTag constant
+		STR		R5, [SP, #-4]!					; push value (type tag)
+		STR		R11, [SP, #-4]!					; push parameter (address of 'State' parameter)
+
+		LDR		R0, [PC,#handler-8-$]
+		LDR		R0, [R0, #0]
+		BLX		R0
+		LDR		R11, [SP], #4
+		ADD		SP, SP, #4
+
+		ADD		SP, SP, #72						; adjust SP & remove PAF
+		LDR		R0, [R11, #64]					; load 'State.PSR'
+		MSR		SPSR_cxsf, R0					; and store it into SPSR
+		LDR		R0, [R11, #60]					; load 'State.PC'...
+		MOV		LR, R0							; ...into LR (because we will return to LR, not PC)
+		ADD		R0, R11, #48					; R0 points to 'State.SP'
+		LDMIA		R0, { FP, SP, LR }^				; load 'State.SP' & 'State.LR' into user mode registers
+		MOV		R0, R0							; nop, to not access banked registers after LDM ^
+		LDMIA		R11, { R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10 } 				; restore unbanked regs
+		LDR		R11, [R11, #44]
+
+		MOVS		PC, LR							; SPSR -> CPSR & return
+
+		; Data section
+	state:			d32 stateTag					; address of stateTag
+	handler:		d32 dataHandler				; address of the handler variable
+	dataAbort:		d32 -Data
+	END dataGlue;
+
+	(** Fast Interrupt Handler. Saves the processor state and calls the registered handler *)
+	PROCEDURE {NOPAF} fiqGlue;
+	CODE
+		CLREX
+		SUB		SP, SP, #72						; Room for the State record
+		STMIA		SP, {R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10, R11, FP, SP, LR}^
+		MOV		R0, R0
+		ADD		SP, SP, #60
+		SUB		R0, LR, #4						; return address = LR-4
+		STR		R0, [SP], #4					; push ('PC' in 'State'). SP points to offset 64
+		MRS		R0, SPSR						; get saved PSR
+		STR		R0, [SP], #4					; push ('PSR' in 'State'). SP points to offset 68
+
+		SUB		SP, SP, #68
+		MOV		R11, SP
+
+		LDR		R5, [pc, #state-$-8]			; load address of stateTag constant
+		STR		R5, [SP, #-4]!					; push value (type tag)
+		STR		R11, [SP, #-4]!					; push parameter (address of 'State' parameter)
+
+		BL			fiqHandler
+		LDR		R11, [SP], #4
+		ADD		SP, SP, #4
+
+		ADD		SP, SP, #72						; adjust SP & remove PAF
+		LDR		R0, [R11, #64]					; load 'State.PSR'
+		MSR		SPSR_cxsf, R0					; and store it into SPSR
+		LDR		R0, [R11, #60]					; load 'State.PC'...
+		MOV		LR, R0							; ...into LR (because we will return to LR, not PC)
+		ADD		R0, R11, #48					; R0 points to 'State.SP'
+		LDMIA		R0, { FP, SP, LR }^				; load 'State.SP' & 'State.LR' into user mode registers
+		MOV		R0, R0							; nop, to not access banked registers after LDM ^
+		LDMIA		R11, { R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10 } 				; restore unbanked regs
+		LDR		R11, [R11, #44]
+
+		MOVS		PC, LR							; SPSR -> CPSR & return
+
+		; Data section
+	state:	d32 stateTag							; address of stateTag
+	END fiqGlue;
+
+	(** Initializes IRQ handling. *)
+	PROCEDURE InitInterrupts*;
+	CONST
+		EnableSecure=0;
+		EnableNonSecure=1;
+		NumberIRQs = 96;
+	VAR p: PROCEDURE; i: LONGINT;
+	BEGIN
+		Acquire(Interrupts);
+		IRQMask := {};
+		p := IRQGlue;
+		SYSTEM.PUT32(InterruptVector + Irq, SYSTEM.VAL(LONGINT, p));	(* install new IRQ handler *)
+		SYSTEM.PUT32(Platform.ICDDCR, 0);
+
+
+		FOR i := 32 DIV 16 TO (NumberIRQs-1) DIV 16 (* 2 bits per IRQ *)  DO
+			SYSTEM.PUT32(Platform.ICDICFR+i*4, 0);
+		END;
+
+		FOR i := 0  TO (NumberIRQs-1) DIV 4 (* 8 bits per IRQ *) DO
+			SYSTEM.PUT32(Platform.ICDIPR+i*4, LONGINT(0A0A0A0A0H)); (* set priority of each interrupt to 160 *)
+		END;
+
+		FOR i := (32 DIV 4) TO (NumberIRQs-1) DIV 4 (* 8 bits per IRQ *) DO
+			SYSTEM.PUT32(Platform.ICDIPTR+i*4, 1010101H); (* reset interrupt targets to processor 0 *)
+		END;
+
+		(* disable all interrupt forwardings *)
+		FOR i := 0 TO (NumberIRQs-1) DIV 32 (* 1 bit per IRQ *) DO
+			SYSTEM.PUT32(Platform.ICDICER+i*4, LONGINT(0FFFFFFFFH));
+		END;
+
+		SYSTEM.PUT32(Platform.ICCPMR, 0F0H);
+		SYSTEM.PUT32(Platform.ICCICR, {0,1,2});
+		SYSTEM.PUT32(Platform.ICCBPR, 0);
+
+		SYSTEM.PUT32(Platform.ICDDCR, {EnableSecure, EnableNonSecure});
+		Release(Interrupts);
+
+		(*InvalidateDCache(dCacheBase);*)
+		(*EnableIRQ(PrivateTimerIRQ);*)
+	END InitInterrupts;
+
+	(** Restore silent, infinitly-looping exception handlers *)
+	PROCEDURE EndInterrupts;
+	BEGIN
+		SYSTEM.PUT32(InterruptVector + Undef, InterruptVector + Undef);
+		SYSTEM.PUT32(InterruptVector + Swi, InterruptVector + Swi);
+		SYSTEM.PUT32(InterruptVector + Prefetch, InterruptVector + Prefetch);
+		SYSTEM.PUT32(InterruptVector + Data, InterruptVector + Data);
+		SYSTEM.PUT32(InterruptVector + Irq, InterruptVector + Irq);
+		SYSTEM.PUT32(InterruptVector + Fiq, InterruptVector + Fiq);
+	END EndInterrupts;
+
+	(* GetExceptionState *)
+	PROCEDURE GetExceptionState*(VAR int: State; VAR exc: ExceptionState);
+	BEGIN
+		(* save all state information while interrupts are still disabled *)
+		exc.locks := BreakAll();
+		IF int.INT = -Undef THEN
+			exc.halt := 17;
+			exc.instn := SYSTEM.GET32(int.PC)
+		ELSIF int.INT = -Prefetch THEN
+			exc.pf := int.PC;
+			exc.status := -1;
+			exc.halt := 19
+		ELSIF int.INT = -Data THEN
+			GetPageFault(exc.pf, exc.status);
+			IF exc.pf < 4 * k THEN
+				(* NIL pointer *)
+				exc.halt := 18
+			ELSE
+				exc.halt := 19
+			END
+		ELSE
+			(* SWI *)
+			exc.halt := int.INT
+		END
+	END GetExceptionState;
+
+	PROCEDURE GetPageFault * (VAR adr: ADDRESS; VAR status: LONGINT);
+	CODE
+		MRC	p15, 0, R0, C5, C0	; load fault status register (FSR)
+		AND	R0, R0, #0FFH			; only bits 7:0 are valid
+		LDR	R1, [FP, #status]
+		STR R0, [R1, #0]
+
+		MRC	p15, 0, R0, C6, C0	; load fault address (FAR)
+		LDR	R1, [FP, #adr]
+		STR R0, [R1, #0]
+	END GetPageFault;
+
+	(* FAR - returns the Fault Address Register. *)
+	PROCEDURE -FAR*(): LONGINT;
+	CODE
+		MRC	p15, 0, R0, C6, C0	; FAR is co-processor 15 register 6
+	END FAR;
+
+	(** Init global timer *)
+	PROCEDURE InitGlobalTimer;
+	CONST
+		TimerEnable=0;
+	BEGIN
+		(* disable first *)
+		SYSTEM.PUT32(Platform.GlobalTimerControlRegister, {});
+		(* reset global counter *)
+		SYSTEM.PUT32(Platform.GlobalTimerCounterRegister0, 0);
+		SYSTEM.PUT32(Platform.GlobalTimerCounterRegister1, 0);
+		SYSTEM.PUT32(Platform.GlobalTimerControlRegister, {TimerEnable});
+	END InitGlobalTimer;
+
+
+	(** Init private timer *)
+	PROCEDURE InitTicks;
+	CONST
+		TimerEnable=0;
+		AutoReload=1;
+		IRQEnable=2;
+	VAR
+		(* time slot in private timer counts; take into account that private timer clock frequency is equal to half of the CPU clock frequency *)
+		delay: LONGINT;
+	BEGIN
+		delay := ENTIER( ( LONGREAL(TimerPeriod) * 0.5D0 * LONGREAL(BootConfig.GetIntValue("CpuClockHz")) ) / 1.0D6 + 0.5D0 );
+		(* disable first *)
+		SYSTEM.PUT32(Platform.PrivateTimerControlRegister, {});
+
+		(*SYSTEM.PUT32(Platform.PrivateTimerCounterRegister, Delay);*)
+		SYSTEM.PUT32(Platform.PrivateLoadValueRegister, delay);
+		SYSTEM.PUT32(Platform.PrivateTimerControlRegister, {TimerEnable, AutoReload, IRQEnable});
+	END InitTicks;
+
+	PROCEDURE StopTicks;
+	BEGIN
+		SYSTEM.PUT32(Platform.PrivateTimerControlRegister, {});
+	END StopTicks;
+
+	(** Handle multiprocessor timer interrupt. *)
+	PROCEDURE HandleMPTimer*(VAR state: State);
+	BEGIN	(* {interrupts off} *)
+		timer(ID(), state); (* rarely used *)
+		(* Clear timer interrupt *)
+		SYSTEM.PUT32(Platform.GlobalTimerInterruptStatusRegister, 1);
+		EnableInterrupts;	(* enable interrupts before acquiring locks below - to avoid deadlock with StopAll. *)
+		Timeslice(state);	(* fixme: check recursive interrupt *)
+	END HandleMPTimer;
+
+	(** Handle uniprocessor timer interrupt. *)
+	PROCEDURE HandleUPTimer(VAR state: State);
+	BEGIN	(* {interrupts off} *)
+		timer(ID(), state);
+		SYSTEM.PUT32(Platform.PrivateTimerInterruptStatusRegister, 1);
+		
+		(* Shutdown if requested *)
+		IF ~(ID() IN allProcessors) THEN ShutdownSecondary END;
+		IF enableWatchdog THEN PrivateWatchdog.Feed(Second) END;
+		Timeslice(state)
+	END HandleUPTimer;
+
+	(** Install a processor timer event handler. *)
+	PROCEDURE InstallEventHandler* (h: EventHandler);
+	BEGIN
+		IF h # NIL THEN timer := h ELSE timer := DummyEvent END
+	END InstallEventHandler;
+
+	PROCEDURE DummyEvent*(id: LONGINT; CONST state: State);
+	BEGIN
+	END DummyEvent;
+
+	PROCEDURE DummyTimeslice*(VAR state: State);
+	BEGIN
+	END DummyTimeslice;
+
+	PROCEDURE DummyIRQ*;
+	BEGIN
+	END DummyIRQ;
+
+	PROCEDURE IRQBeginPrinter;
+	BEGIN
+		Trace.StringLn("IRQ BEGIN");
+	END IRQBeginPrinter;
+
+	PROCEDURE IRQEndPrinter;
+	BEGIN
+		Trace.StringLn("IRQ END");
+	END IRQEndPrinter;
+
+	(* Timer interrupt handler. *)
+	PROCEDURE TimerInterruptHandler(VAR state: State);
+	BEGIN
+		IF ID() = 0 THEN
+			INC(ticks);
+			DEC(eventCount);
+			IF eventCount = 0 THEN
+				eventCount := eventMax; event(state)
+			END;
+		END
+	END TimerInterruptHandler;
+
+	(* Set timer upcall. The handler procedure will be called at a rate of Second/divisor Hz. *)
+	PROCEDURE InstallTickHandler(handler: Handler; divisor: LONGINT);
+	BEGIN
+		eventMax := divisor; event := handler;
+		eventCount := eventMax
+	END InstallTickHandler;
+
+	(* ===== Low-Level Locks ===== *)
+	(* Initializes spinlocks. This is not exported in Intel version. *)
+	PROCEDURE InitLocks;
+	VAR
+		i: LONGINT;
+	BEGIN
+		FOR i := 0 TO MaxLock - 1 DO
+			lock[i].locked := FALSE
+		END;
+	END InitLocks;
+
+	PROCEDURE AcquirePreemption*(): LONGINT;
+	VAR
+		id: LONGINT;
+	BEGIN
+		id := ID();
+		INC(proc[id].preemptCount);
+		RETURN id
+	END AcquirePreemption;
+
+	PROCEDURE ReleasePreemption*;
+	VAR
+		id: LONGINT;
+	BEGIN
+		id := ID();
+
+		IF StrongChecks THEN
+			ASSERT(proc[id].preemptCount > 0);
+		END;
+
+		DEC(proc[id].preemptCount);
+	END ReleasePreemption;
+
+	PROCEDURE PreemptCount*(id: LONGINT): LONGINT;
+	BEGIN
+		IF StrongChecks THEN
+			ASSERT(id = ID());
+		END;
+		RETURN proc[id].preemptCount;
+	END PreemptCount;
+
+	(* Acquire spinlock 'lock'. Does not time out *)
+	PROCEDURE AcquireSpin(VAR lock: BOOLEAN); (* DEBUG: temporarily exported *)
+	CODE
+		CLREX
+		DMB
+		LDR R0, [FP, #lock]
+		MOV R1, #1
+	loop:
+		LDREXB R2, R0	; R2 := lock
+		CMP R2, #0		; if R2 != 0 => lock held, try again
+		BNE loop
+		STREXB R2, R1, R0	; lock := R1
+		CMP R2, #0		; if R2 # 0 exclusive write failed: try whole process again.
+		BNE loop
+		;DSB				; Data Memory Barrier
+	END AcquireSpin;
+
+	(* Releases spinlock 'lock'. *)
+	PROCEDURE ReleaseSpin(VAR lock: BOOLEAN); (* DEBUG: temporarily exported *)
+	BEGIN
+		CODE
+			CLREX
+			DMB
+		END;
+		lock := FALSE;
+		(*CODE
+		loop:
+			LDR R0, [FP, #lock]
+			LDREX R1, R0
+			MOV R1, #0
+			STREX R2, R1, R0
+			CMP R2, #0
+			BNE loop
+		END;*)
+		(*CODE
+			DMB
+			DSB
+			LDR R0, [FP, #lock]
+			MCR P15, 0, R0, C7, C10, 4
+		END;*)
+	END ReleaseSpin;
+
+	(* Acquire lock. Disables interrupts. *)
+	PROCEDURE Acquire*(level: LONGINT);
+	VAR
+		id: LONGINT;
+		enabled: BOOLEAN;
+	BEGIN
+		enabled := AreInterruptsEnabled();
+		DisableInterrupts();
+		id := AcquirePreemption();
+		IF proc[id].locksHeld = {} THEN
+			proc[id].intStatus := enabled
+		END;
+
+		IF StrongChecks THEN
+			ASSERT(~AreInterruptsEnabled());
+			ASSERT((~enabled) OR (proc[id].locksHeld = {}));	(* interrupts enabled => no locks held *)
+			ASSERT(~(level IN proc[id].locksHeld))	(* recursive locks not allowed *)
+		END;
+
+		AcquireSpin(lock[level].locked);
+		(* Now, we hold the lock *)
+		INCL(proc[id].locksHeld, level);
+
+		IF StrongChecks THEN	(* no lower-level locks currently held by this processor *)
+			ASSERT((level = 0) OR (proc[id].locksHeld * {0..level-1} = {}));
+		END;
+	END Acquire;
+
+	(* Release lock. Enables interrupts if last lock held. *)
+	PROCEDURE Release*(level: LONGINT);
+	VAR
+		id: LONGINT;
+	BEGIN
+		id := ID();
+
+		IF StrongChecks THEN
+			ASSERT(~AreInterruptsEnabled());
+			ASSERT(level IN proc[id].locksHeld);
+			ASSERT(lock[level].locked # FALSE)
+		END;
+
+		EXCL(proc[id].locksHeld, level);
+		ReleaseSpin(lock[level].locked);
+
+		ReleasePreemption;
+		IF (proc[id].locksHeld = {}) & proc[id].intStatus THEN
+			EnableInterrupts
+		END;
+	END Release;
+
+	PROCEDURE AcquireAll*;
+	VAR
+		i: LONGINT;
+	BEGIN
+		FOR i := MaxLock - 1 TO 0 BY -1 DO
+			Acquire(i)
+		END
+	END AcquireAll;
+
+	PROCEDURE ReleaseAll*;
+	VAR
+		i: LONGINT;
+	BEGIN
+		FOR i := 0 TO MaxLock - 1 DO
+			Release(i)
+		END
+	END ReleaseAll;
+
+	(** Acquire a fine-grained lock on an active object. *)
+	PROCEDURE AcquireObject*(VAR locked: BOOLEAN);
+	BEGIN
+		AcquireSpin(locked)
+	END AcquireObject;
+
+	(** Release an active object lock. *)
+	PROCEDURE ReleaseObject*(VAR locked: BOOLEAN);
+	BEGIN
+		ReleaseSpin(locked)
+	END ReleaseObject;
+
+	(** Break all locks held by current processor (for exception handling).  Returns levels released. *)
+	PROCEDURE BreakAll*(): SET;
+	VAR id, level: LONGINT; released: SET;
+	BEGIN
+		id := AcquirePreemption();
+		released := {};
+		FOR level := 0 TO MaxLock-1 DO
+			IF level IN proc[id].locksHeld THEN
+				lock[level].locked := FALSE;	(* break the lock *)
+				EXCL(proc[id].locksHeld, level);
+				INCL(released, level)
+			END
+		END;
+		(*IF released = {} THEN
+			ASSERT(proc[id].nestCount = 0)	(* no locks held *)
+		ELSE
+			ASSERT(proc[id].nestCount > 0);	(* some locks held *)
+			proc[id].nestCount := 0	(* interrupt state will be restored later *)
+		END;*)
+		IF proc[id].preemptCount > 1 THEN INCL(released, Preemption) END;
+		proc[id].preemptCount := 0;	(* clear preemption flag *)
+		RETURN released
+	END BreakAll; (* !!! interrupts are still off !!! *)
+
+	(* ===== Atomic Operations ===== *)
+	(* Atomic INC(x) *)
+	PROCEDURE -AtomicInc*(VAR x: LONGINT);
+	CODE
+	;loop:
+	;	ADD R0, SP, #x	; R0 := ADR(x)
+	;	LDREX R1, R0		; R1 := x
+	;	ADD R1, R1, #1	; increment x
+	;	STREX R2, R1, R0
+	;	CMP R2, #0
+	;	BEQ loop			; if store failed, try again, else exit
+	;	ADD SP, SP, #4
+
+		LDR	R0, [SP], #4
+	loop:
+		LDREX	R1, R0
+		ADD	R1, R1, #1
+		STREX	R2, R1, R0
+		CMP	R2, #0
+		BNE	loop
+	END AtomicInc;
+	
+	(* Atomic INC(x) *)
+	PROCEDURE -AtomicDec*(VAR x: LONGINT);
+	CODE
+		LDR	R0, [SP], #4
+	loop:
+		LDREX	R1, R0
+		SUB	R1, R1, #1
+		STREX	R2, R1, R0
+		CMP	R2, #0
+		BNE	loop
+	END AtomicDec;
+
+	PROCEDURE -AtomicAdd * (VAR x: LONGINT; y: LONGINT);
+	CODE
+		LDR R3, [SP, #y]	; R3 := y
+		LDR R0, [SP, #x]	; R0 := ADR(x)
+	loop:
+		LDREX R1, R0		; R1 := x
+		ADD R1, R1, R3	; increment x
+		STREX R2, R1, R0
+		CMP R2, #0
+		BNE loop			; if store failed, try again, else exit
+		ADD SP, SP, #8
+	END AtomicAdd;
+
+	(* Atomic compare-and-swap. Set x = new if x = old and return old value of x *)
+	PROCEDURE -AtomicCAS * (VAR x: LONGINT; old, new: LONGINT): LONGINT;
+	CODE
+		LDR	R3, [SP, #x]			; R0 := ADDRESSOF(x)
+		LDR	R1, [SP, #old]			; R1 := old
+		LDR	R2, [SP, #new]			; R2 := new
+		ADD	SP, SP, #12				; pop variable from stack
+
+	loop:
+		LDREX	R0, R3					; load excl x
+		CMP	R0, R1
+		BNE	exit						; x # old -> exit
+		STREX	R4, R2, R3				; x = old -> store excl new -> x
+		CMP	R4, #0
+		BNE	loop					; store exclusive failed: retry
+
+	exit:
+	END AtomicCAS;
+
+	(* ===== Virtual Memory Management ===== *)
+
+	PROCEDURE Ensure32BitAddress*(adr: ADDRESS): LONGINT;
+	BEGIN
+		RETURN adr
+	END Ensure32BitAddress;
+
+	PROCEDURE GetSecondLevelEntry(virtualAddress: ADDRESS): ADDRESS;
+	VAR
+		ptIdx, basePT, entry: ADDRESS;
+	BEGIN
+		IF (PS <= virtualAddress) & (virtualAddress < M) THEN
+			(* First 256 entries *)
+			basePT := sysSecondLvlPtStart
+		ELSIF (4 * G - M <= virtualAddress) THEN
+			(* Entries 256 to 511 *)
+			basePT := sysSecondLvlPtStart + 400H
+		ELSIF (memStackStart <= virtualAddress) & (virtualAddress < memStackStop) THEN
+			basePT := sysSecondLvlPtStart + 800H + virtualAddress DIV M * k - memStackStart DIV k
+		ELSIF (memConfigStart <= virtualAddress) & (virtualAddress < memConfigStop) THEN
+			basePT := sysSecondLvlPtStop - (memConfigStop DIV k - virtualAddress DIV M * k)
+		END;
+		ptIdx := SHR(virtualAddress MOD M, PSlog2);
+		ASSERT(basePT + 4 * ptIdx >= sysSecondLvlPtStart);
+		ASSERT(basePT + 4 * ptIdx < sysSecondLvlPtStop);
+		entry := SYSTEM.GET32(basePT + 4 * ptIdx);
+		RETURN entry
+	END GetSecondLevelEntry;
+
+	PROCEDURE SetSecondLevelEntry(virtualAddress, physicalAddress, flags: ADDRESS);
+	VAR ptIdx, basePT, entry: ADDRESS;
+		firstLvlEntry: ADDRESS;
+	BEGIN
+		IF (PS <= virtualAddress) & (virtualAddress < M) THEN
+			(* First 256 entries *)
+			basePT := sysSecondLvlPtStart
+		ELSIF (4 * G - M <= virtualAddress) THEN
+			(* Entries 256 to 511 *)
+			basePT := sysSecondLvlPtStart + 400H
+		ELSIF (memStackStart <= virtualAddress) & (virtualAddress < memStackStop) THEN
+			basePT := sysSecondLvlPtStart + 800H + virtualAddress DIV M * k - memStackStart DIV k
+		ELSIF (memConfigStart <= virtualAddress) & (virtualAddress < memConfigStop) THEN
+			basePT := sysSecondLvlPtStop - (memConfigStop DIV k - virtualAddress DIV M * k)
+		END;
+		ptIdx := SHR(virtualAddress MOD M, PSlog2);
+
+		IF physicalAddress = NilAdr THEN
+			entry := slFault
+		ELSE
+			ASSERT(physicalAddress MOD PS = 0);
+			ASSERT((flags DIV PS = 0) & (flags MOD 4 = 0));
+			entry := physicalAddress + flags + slSmall
+		END;
+		ASSERT(basePT + 4 * ptIdx >= sysSecondLvlPtStart);
+		ASSERT(basePT + 4 * ptIdx < sysSecondLvlPtStop);
+		SYSTEM.PUT32(basePT + 4 * ptIdx, entry);
+	END SetSecondLevelEntry;
+
+	PROCEDURE UnmapPhysical*(viartAdr: ADDRESS; size: SIZE);
+	BEGIN
+	END UnmapPhysical;
+
+	(* Unmap a virtual page and deallocate the corresponding physical page *)
+	PROCEDURE DeallocatePage(virtualAdr: ADDRESS);
+	VAR memoryAdr: LONGINT;
+	BEGIN
+		(* unmap the page *)
+		memoryAdr := GetSecondLevelEntry(virtualAdr);
+		ASSERT(memoryAdr MOD 4 = slSmall);	(* page must be mapped *)
+		SYSTEM.PUT32(virtualAdr, freePage); 	(* link freePage list (must be done as long as the page is still mapped !) *)
+		SetSecondLevelEntry(virtualAdr, NilAdr, 0);
+		InvalidateTLBEntry(SHRL(virtualAdr, PSlog2));
+
+		(* free the page *)
+		memoryAdr := SHRL(memoryAdr, 12);
+		freePage := memoryAdr;
+		INC(memory.free, PS)
+	END DeallocatePage;
+
+	(* AllocatePage - allocates and maps one memory page to [virtualAdr...virtualAdr+PageSize]. Returns TRUE iff successful *)
+	PROCEDURE AllocatePage(virtualAdr, accessPermissions, flags: ADDRESS): BOOLEAN;
+	VAR memoryAdr, entry: ADDRESS;
+	BEGIN
+		(* Use 1:1 Mapping for stack *)
+		(* map the page *)
+		entry := GetSecondLevelEntry(virtualAdr);
+		IF entry MOD 4 # slFault THEN
+			Acquire(TraceOutput);
+			Trace.String("Allocate Page: entry = ");
+			Trace.Address(entry);
+			Trace.String("; vadr = ");
+			Trace.Address(virtualAdr);
+			Trace.String("; CPU = ");
+			Trace.Int(ID(), 0);
+			Trace.Ln;
+			Release(TraceOutput)
+		END;
+		ASSERT(entry MOD 4 = slFault);	(* page must not be mapped *)
+		SetSecondLevelEntry(virtualAdr, (*memoryAdr*) virtualAdr, accessPermissions + flags);
+		InvalidateTLBEntry(SHRL(virtualAdr, PSlog2));
+		RETURN TRUE
+	END AllocatePage;
+
+	(** PhysicalAdr - returns the (real) physical address of the specified range of memory, or NilAdr if the range is not contiguous.
+		It is the caller's responsiblilty to assure the range remains allocated during the time it is in use.
+	*)
+	PROCEDURE PhysicalAdr*(adr, size: LONGINT): LONGINT;
+	VAR physAdr: ARRAY 400H OF Range; num, i, end: LONGINT;
+	BEGIN
+		TranslateVirtual(adr, size, num, physAdr);
+		IF (num > 0) THEN
+			FOR i := 1 TO num-1 DO
+				IF (physAdr[i].adr # (physAdr[i-1].adr + physAdr[i-1].size)) THEN
+					RETURN NilAdr
+				END
+			END;
+			RETURN physAdr[0].adr
+		ELSE
+			RETURN NilAdr
+		END
+	END PhysicalAdr;
+
+	(** TranslateVirtual - translates a virtual address range to num ranges of (real) physical addresses.  num returns 0 on error.*)
+	PROCEDURE TranslateVirtual*(virtAdr: ADDRESS; size: LONGINT; VAR num: LONGINT; VAR physAdr: ARRAY OF Range);
+	VAR entry, base, ofs, len: LONGINT; endAdr: ADDRESS; abort: BOOLEAN;
+	BEGIN
+		Acquire(Memory);
+
+		endAdr := virtAdr + size;
+		IF ((memHeapStart <= virtAdr) & (endAdr <= memHeapStop)) OR ((memStackStart <= virtAdr) & (endAdr <= memStackStop)) OR ((memIOStart <= virtAdr) & (endAdr <= memIOStop)) THEN
+			(* Optimizations: we know that heap, stacks and I/O regions are mapped 1:1. *)
+			(*! This code is very aggressive: it returns only 1 range, and not 1 range per physical page. It assumes that all stack pages of the specified region are mapped *)
+			num := 1;
+			physAdr[0].adr := virtAdr;
+			physAdr[0].size := size;
+		ELSE
+			abort := FALSE;
+			num := 0;
+			WHILE (size > 0) & (num < LEN(physAdr)) & ~abort DO
+				entry := (*SYSTEM.GET32(pageTable.virtual + 4*SHR(virtAdr, LogM));*)GetFirstLevelEntry(virtAdr - virtAdr MOD M);
+				IF (entry MOD 4 = flSection) THEN
+					ofs := virtAdr MOD M;
+					len := MIN(size, M - ofs);
+					physAdr[num].adr := SHRL(entry, LogM) + ofs;
+					physAdr[num].size := len;
+					INC(num);
+					INC(virtAdr, len); DEC(size, len)
+				ELSIF (entry MOD 4 = flCoarse) THEN
+					base := SHRL(entry, 10);
+
+					WHILE (size > 0) & (num < LEN(physAdr)) & ~abort DO
+						entry := GetSecondLevelEntry(virtAdr);
+						IF (entry MOD 4 = slSmall) THEN
+							ofs := virtAdr MOD PS;
+							len := MIN(size, PS - ofs);
+							physAdr[num].adr := SHRL(entry, 12) + ofs;
+							physAdr[num].size := len;
+							INC(num);
+							INC(virtAdr, len); DEC(size, len)
+						ELSE
+							num := 0;
+							abort := TRUE
+						END
+					END
+				ELSE
+					num := 0; abort := TRUE
+				END;
+			END;
+			IF (size > 0) & (num = LEN(physAdr)) THEN num := 0 END;	(* array 'physAdr' was too small *)
+		END;
+		Release(Memory)
+	END TranslateVirtual;
+
+	PROCEDURE SetFirstLevelEntry (virtual, physical: ADDRESS; permissions, flags, type: LONGINT);
+	VAR
+		index, entry: LONGINT;
+	BEGIN
+		index := SHR(virtual, LogM) * 4;
+		ASSERT(index >= 0);
+		ASSERT(sysFirstLvlPtStart + index < sysFirstLvlPtStop);
+		entry := physical + permissions * 400H + flags + type;
+		(*Trace.Address(virtual); Trace.String("	"); Trace.Address(physical); Trace.String("	"); Trace.Address(SysFirstLvlPtStart(*pageTable.virtual*) + index); Trace.String("	"); Trace.Address(entry); Trace.Ln;*)
+		SYSTEM.PUT32(sysFirstLvlPtStart + index, entry)
+	END SetFirstLevelEntry;
+
+	PROCEDURE GetFirstLevelEntry (virtual: ADDRESS): LONGINT;
+	VAR
+		index: LONGINT;
+	BEGIN
+		index := LSH(virtual, -(LogM)) * 4;
+		ASSERT(index >= 0);
+		ASSERT(sysFirstLvlPtStart + index < sysFirstLvlPtStop);
+		RETURN SYSTEM.GET32(sysFirstLvlPtStart + index)
+	END GetFirstLevelEntry;
+
+	(* AllocateHeap - allocates and maps [physicalAddress...physicalAddress+size] to [virtualAddress...virtualAddress+size] *)
+	PROCEDURE AllocateHeap(virtualAddress, physicalAddress, size: ADDRESS; accessPermissions, flags: LONGINT);
+	VAR i, entry: LONGINT;
+	BEGIN
+		ASSERT(size MOD M = 0);
+		FOR i := 0 TO (size DIV M) - 1 DO
+			entry := GetFirstLevelEntry(virtualAddress + i * M);
+			ASSERT(entry = 0);
+			SetFirstLevelEntry(virtualAddress + i * M, physicalAddress + i * M, accessPermissions, flags, flSection)
+		END
+	END AllocateHeap;
+
+	(** Enable Memory Management and virtual memory. *)
+	PROCEDURE EnableMM(translationBase, tbFlags, mmuFlags: ADDRESS);
+	BEGIN
+		InvalidateTLB;
+		CODE
+			; Disable AFE (special permission mode) and TRE (special memory mode)
+			ldr r0, [pc, #pattern-$-8]
+			mrc p15, 0, r1, c1, c0, 0
+			and r1, r0, r1
+			mcr p15, 0, r1, c1, c0, 0
+			isb
+
+			;mrc p15, 0, r0, c2, c0, 2
+			;ldr r1, [pc, #TLBCRPattern-$-8]
+			;and r0, r0, r1
+			;mcr p15, 0, r0, c2, c0, 2
+
+			ldr r0, [FP, #translationBase]
+			ldr	r1, [fp, #tbFlags]
+			orr r0, r0, r1
+			mcr p15, 0, r0, c2, c0, 0
+			isb
+			;mvn r0, #0 	; mmu domains: 16 x 11 = manager on all domains
+			ldr r0, [pc, #domains-$-8]
+			mcr p15, 0, r0, c3, c0, 0
+			isb
+			ldr r0, [FP, #mmuFlags]
+			ldr r1, [FP, #sctlr-$-8]
+			orr r0, r0, r1 ; 1 bits in SCTLR
+			mcr p15, 0, r0, c1, c0, 0
+			isb
+
+			;dsb
+			;isb
+
+			b exit
+		domains:	d32 55555555H		; Client on each domain
+		pattern:	d32 0CFFFFFFFH	; NOT(AFE+TRE)
+		sctlr:		d32 0C50078H
+		TLBCRPattern: d32 0FFFFFFF8H
+		exit:
+		END;
+		proc[ID()].mmu := TRUE
+	END EnableMM;
+
+	PROCEDURE InitMemory;
+	VAR
+		cr1: SET;
+		i: LONGINT;
+		base: ADDRESS;
+		coarseFlags, fineFlags, tbFlags: LONGINT;
+		trace: BOOLEAN;
+		val: ARRAY 8 OF CHAR;
+	BEGIN
+		BootConfig.GetValue("TraceMemory", val);
+		trace := val = '1';
+		IF trace THEN
+			(* Debug Tracing *)
+			Trace.String("Memory Layout");
+			IF ~enableCaching THEN Trace.String(" - WARNING: Caching Disabled") END;
+			Trace.Ln;
+			Trace.String("System Start:	"); Trace.Address(memSysLowStart); Trace.Ln;
+			Trace.String("System Stop:	"); Trace.Address(memSysLowStop); Trace.Ln;
+			Trace.String("System Size:	"); Trace.Address(memSysLowStop - memSysLowStart); Trace.Ln;
+			Trace.String("	Interrupt Stack Start:		"); Trace.Address(sysIntStackStart); Trace.Ln;
+			Trace.String("	Interrupt Stack Stop:		"); Trace.Address(sysIntStackStop); Trace.Ln;
+			Trace.String("	Interrupt Stack Size:		"); Trace.Address(sysIntStackStop - sysIntStackStart); Trace.Ln;
+			Trace.String("	First Page Table Start:	"); Trace.Address(sysFirstLvlPtStart); Trace.Ln;
+			Trace.String("	First Page Table Stop:		"); Trace.Address(sysFirstLvlPtStop); Trace.Ln;
+			Trace.String("	First Page Table Size:		"); Trace.Address(sysFirstLvlPtStop - sysFirstLvlPtStart); Trace.Ln;
+			Trace.String("	Second Page Table Start:	"); Trace.Address(sysSecondLvlPtStart); Trace.Ln;
+			Trace.String("	Second Page Table Stop:	"); Trace.Address(sysSecondLvlPtStop); Trace.Ln;
+			Trace.String("	Second Page Table Size:	"); Trace.Address(sysSecondLvlPtStop - sysSecondLvlPtStart); Trace.Ln;
+			Trace.String("Heap Start:		"); Trace.Address(memHeapStart); Trace.Ln;
+			Trace.String("Heap Stop:		"); Trace.Address(memHeapStop); Trace.Ln;
+			Trace.String("Heap Size:		"); Trace.Address(memHeapStop - memHeapStart); Trace.Ln;
+			Trace.String("Stack Start:	"); Trace.Address(memStackStart); Trace.Ln;
+			Trace.String("Stack Stop:		"); Trace.Address(memStackStop); Trace.Ln;
+			Trace.String("Stack Size:		"); Trace.Address(memStackStop - memStackStart); Trace.Ln;
+			Trace.String("Config Start:	"); Trace.Address(memConfigStart); Trace.Ln;
+			Trace.String("Config Stop:	"); Trace.Address(memConfigStop); Trace.Ln;
+			Trace.String("Config Size:	"); Trace.Address(memConfigStop - memConfigStart); Trace.Ln;
+			Trace.String("I/O Start:		"); Trace.Address(memIOStart); Trace.Ln;
+			Trace.String("I/O Stop:		"); Trace.Address(memIOStop); Trace.Ln;
+			Trace.String("I/O Size:		"); Trace.Address(memIOStop - memIOStart); Trace.Ln;
+			Trace.String("SysHigh Start:	"); Trace.Address(memSysHighStart); Trace.Ln;
+			Trace.String("SysHigh Stop:	"); Trace.Hex(memSysHighStop - 1, -8); Trace.Ln;
+			Trace.String("	Interrupt Vector Start:	"); Trace.Address(sysVectorStart); Trace.Ln;
+			Trace.String("	Interrupt Vector Stop:	"); Trace.Address(sysVectorStop); Trace.Ln;
+			Trace.String("	Interrupt Vector Size:	"); Trace.Address(sysVectorStop - sysVectorStart); Trace.Ln;
+			Trace.String("	Cache References Start:	"); Trace.Address(sysCacheRefStart); Trace.Ln;
+			Trace.String("	Cache References Stop:	"); Trace.Address(sysCacheRefStop); Trace.Ln;
+			Trace.String("	Cache References Size:	"); Trace.Address(sysCacheRefSize); Trace.Ln;
+			Trace.String("	Cache References Stack Offset:	"); Trace.Address(sysCacheStackOfs); Trace.Ln;
+		END;
+
+		(* disable caching & buffering globally *)
+		cr1 := GetControlRegister();
+		SetControlRegister(cr1 - {DCache, ICache});
+		(*InvalidateDCache(dCacheBase);*)
+		(*InvalidateDCacheRange(0, MemStackStop);*)
+		InvalidateICache;
+		DrainWriteBuffer;
+
+		(* initialize memory ranges *)
+		heapLow.physical := memHeapStart;
+		heapLow.virtual := memHeapStart;
+		heapHigh.physical := memHeapStop;
+		heapHigh.virtual := memHeapStart;
+
+		stackHigh.physical := memStackStop;
+		stackHigh.virtual := memStackStop;
+		stackLow.physical := memStackStop;
+		stackLow.virtual := memStackStart;
+
+		(* initialize global variables *)
+		pageTable.virtual := sysFirstLvlPtStart;
+		pageTable.memory := sysFirstLvlPtStart;
+		stackPT := sysSecondLvlPtStart + 2 * k;
+		freePage := NilAdr;
+
+		(* Determine global caching parameter *)
+		IF enableCaching THEN
+			coarseFlags := Cacheable + Shareable;
+			fineFlags := CB
+		ELSE
+			coarseFlags:= Shareable;
+			fineFlags := 0;
+		END;
+
+		(* Clear first level page table *)
+		Fill32(sysFirstLvlPtStart, sysFirstLvlPtStop - sysFirstLvlPtStart, 0);
+
+		(* Clear second level page table *)
+		Fill32(sysSecondLvlPtStart, sysSecondLvlPtStop - sysSecondLvlPtStart, 0);
+
+		(* Map system area *)
+		SetFirstLevelEntry(0, sysSecondLvlPtStart, 0, 0, flCoarse);
+		FOR i := 1 TO 0FFH DO
+			SetSecondLevelEntry(PS * i, PS * i, FullAccess + fineFlags);
+		END;
+
+		(* Map page for high part of OCM *)
+		AllocateHeap(memSysHighStart, memSysHighStart, MAX(M, LONGINT(memSysHighStop - memSysHighStart)), SrwUrw, coarseFlags);
+
+		(* Map heap area *)
+		AllocateHeap(memHeapStart, memHeapStart, memHeapStop - memHeapStart, SrwUrw, coarseFlags);
+
+		(* Map I/O area, device-type, shared memory *)
+		AllocateHeap(memIOStart, memIOStart, memIOStop - memIOStart, SrwUrw, Shareable + B);
+
+		(* initialize stack & config page tables (excluding the last MB that is already initalized) *)
+		base := SHR(memStackStart, LogM);
+		FOR i := memStackStart TO memConfigStop - 1 BY M DO
+			SetFirstLevelEntry(i, sysSecondLvlPtStart + 2 * k + PTSize * (SHR(i, LogM) - base), 0, 0, flCoarse)
+		END;
+
+		(* Map config region directly *)
+		FOR i := 0 TO SHR(memConfigStop - memConfigStart, PSlog2) - 1 DO
+			SetSecondLevelEntry(memConfigStart + PS * i, memConfigStart + PS * i, FullAccess + fineFlags)
+		END;
+
+		(* flush all caches & the write buffer and invalidate both TLBs *)
+		FlushDCacheRange(0, SYSTEM.VAL(ADDRESS, LastAddress));
+		InvalidateICache;
+		DrainWriteBuffer;
+		InvalidateTLB;
+
+		(* get memory size *)
+		memory.size := memHeapStop - memHeapStart;
+		memory.free := memory.size;
+
+		(* get heap size (check how many MBs are allocated) *)
+		heapLow.physical := 1*M; heapLow.virtual := memHeapStart;
+		heapHigh.physical := 1*M;
+		i := SHR(memHeapStart, LogM);
+		WHILE (SYSTEM.GET32(pageTable.virtual + i*4) MOD 4 = flSection) DO
+			INC(heapHigh.physical, M);
+			DEC(memory.free, M);
+			INC(i)
+		END;
+		heapHigh.virtual := heapLow.virtual + heapHigh.physical - heapLow.physical;
+
+		(* allocate empty memory block with enough space for at least one free block *)
+		memBlockHead := SYSTEM.VAL (MemoryBlock, ADDRESSOF (initialMemBlock));
+		memBlockTail := memBlockHead;
+		initialMemBlock.beginBlockAdr := SYSTEM.VAL (ADDRESS, LastAddress);
+		initialMemBlock.endBlockAdr := heapHigh.virtual;
+		initialMemBlock.size := initialMemBlock.endBlockAdr - initialMemBlock.beginBlockAdr;
+		initialMemBlock.next := NIL;
+		freeStackIndex := 0;
+
+		(* init stack bitmap *)
+		FOR i := 0 TO (maxUserStacks) DIV 32 - 1 DO
+			freeStack[i] := {0..31};
+		END;
+		freeStack[(maxUserStacks) DIV 32] := {0 .. (LONGINT(maxUserStacks) MOD 32) - 1};
+		freeStackIndex := 0;
+
+		(* Init cache ref counts *)
+		cacheRefs := sysCacheRefStart;
+		Fill32(sysCacheRefStart, sysCacheRefSize, 0);
+
+		IF enableCaching THEN
+			tbFlags := 7BH
+		ELSE
+			tbFlags := 0
+		END;
+
+		(* Copy interrupt vector *)
+		SYSTEM.MOVE(0, 0FFFF0000H, 4096);
+
+		EnableMM(sysFirstLvlPtStart, tbFlags, 2000H + 1007H);
+	END InitMemory;
+
+	(** GetFreeK - returns information on free memory in Kbytes.*)
+	PROCEDURE GetFreeK*(VAR total, lowFree, highFree: SIZE);
+	BEGIN
+		Acquire(Memory);
+		total := SHR(memory.size, 10);
+		lowFree := 0;
+		highFree := SHR(memory.free, 10);
+		Release(Memory)
+	END GetFreeK;
+
+	(* ===== Stack Management ===== *)
+	(** Extend stack to address. Returns TRUE iff done.
+	 * If a procedure that holds Memory needs triggers a stack extension, this procedure is called by the interrupt handler:
+	 * we get a trap because Acquire is not reentrant. To solve this, we do not acquire memory iff:
+	 *	- we are in interrupt mode
+	 *	- the current processor holds Memory already
+	 *)
+	PROCEDURE ExtendStack*(VAR s: Stack; virtAdr: ADDRESS): BOOLEAN;
+	VAR
+		ok, doLock: BOOLEAN;
+		access, psr, flags: LONGINT;
+	BEGIN
+		IF enableCaching THEN
+			flags := CB
+		ELSE
+			flags := 0
+		END;
+
+		SYSTEM.STPSR(0, psr);
+		(* Need to acquire Memory: if we are not in interrupt mode or if we do not hold this lock yet. *)
+		doLock := ~(Memory IN proc[ID()].locksHeld);
+		IF doLock THEN Acquire(Memory) END;
+		ok := FALSE;
+
+		IF (virtAdr < s.high) & (virtAdr >= s.low) THEN
+			(* Get page boundary *)
+			DEC(virtAdr, virtAdr MOD PS);
+
+			(* Check if page is mapped *)
+			(*InvalidateDCacheRange(memSysLowStart, memSysLowStop - memSysLowStart);*)
+			Initializer.InvalidateDCache;
+			IF (GetSecondLevelEntry(virtAdr) MOD 4 = slSmall) THEN
+				InvalidateTLBEntry(virtAdr);
+				Acquire(TraceOutput);
+				Trace.String("Stack address already mapped: ");
+				Trace.Address(virtAdr); Trace.Ln;
+				Release(TraceOutput)
+			ELSE
+				(* Allocate page: set different permissions for last stack page. *)
+				(*IF (virtAdr <= s.low) THEN HALT(100) ELSE access := FullAccess END;*)
+				access := FullAccess;
+
+				ok := AllocatePage(virtAdr, access, flags);
+				IF virtAdr < s.adr THEN
+					s.adr := virtAdr
+				END
+			END
+		ELSE
+			Acquire(TraceOutput);
+			Trace.StringLn("Address not in stack");Trace.Address(virtAdr); 
+			Release(TraceOutput);
+			ok := FALSE
+		END;
+		IF doLock THEN Release(Memory) END;
+		RETURN ok
+	END ExtendStack;
+
+	(** Create a new stack s, for process with the initial stack pointer initSP. *)
+	PROCEDURE NewStack*(VAR s: Stack; process: ANY; VAR initSP: ADDRESS);
+	VAR
+		old, flags: LONGINT;
+		adr: ADDRESS;
+		free: SET;
+		b: BOOLEAN;
+	BEGIN
+		Acquire(Memory);
+		old := freeStackIndex;
+		IF enableCaching THEN
+			flags := CB
+		ELSE
+			flags := 0
+		END;
+		LOOP
+			free := freeStack[freeStackIndex];
+			IF free # {} THEN
+				(* Find the free stack space in that region and mark it as allocated *)
+				adr := 0;
+				WHILE ~(adr IN free) DO INC(adr) END;
+				EXCL(freeStack[freeStackIndex], SIZE(adr));
+				adr := memStackStart + (freeStackIndex * 32 + adr) * maxUserStackSize;
+
+				EXIT
+			END;
+			INC(freeStackIndex);
+			IF freeStackIndex = LEN(freeStack) THEN freeStackIndex := 0 END;
+			IF freeStackIndex = old THEN HALT(1503) END
+		END;
+		s.high := adr + maxUserStackSize; s.low := adr + StackGuardSize;
+		s.adr := s.high - InitStackSize;	(* at the top of the virtual area *)
+		initSP := s.high;
+		b := AllocatePage(s.adr, FullAccess, flags); (* allocate one physical page first *)
+		
+		ASSERT(b);
+		FlushDCacheRange(sysSecondLvlPtStart, sysSecondLvlPtStop - sysSecondLvlPtStart);
+		Release(Memory)
+	END NewStack;
+
+	PROCEDURE DisposeStack*(s: Stack);
+	VAR
+		adr: ADDRESS;
+	BEGIN
+		Acquire(Memory);
+		adr := s.adr;
+		WHILE adr # s.high DO
+			DeallocatePage(adr);
+			INC(adr, PS)
+		END;
+
+		adr := (adr - maxUserStacks - memStackStart) DIV maxUserStackSize;
+		INCL(freeStack[adr DIV 32], SIZE(adr MOD 32));
+		Release(Memory)
+	END DisposeStack;
+
+	(** Check if the specified stack is valid. *)
+	PROCEDURE ValidStack*(CONST s: Stack; sp: ADDRESS): BOOLEAN;
+	VAR valid: BOOLEAN;
+	BEGIN
+		Acquire(Memory);
+		valid := (sp MOD 4 = 0) & (sp >= s.adr) & (sp <= s.high);
+		WHILE valid & (sp < s.high) DO
+			valid := GetSecondLevelEntry(sp) MOD 4 = slSmall;
+			INC(sp, PS)
+		END;
+		Release(Memory);
+		RETURN valid
+	END ValidStack;
+
+	PROCEDURE GetStack(adr: ADDRESS; VAR s: Stack): BOOLEAN;
+	BEGIN
+		IF (adr < memStackStart) OR (adr > memStackStop) THEN
+			RETURN FALSE
+		END;
+		s.high := adr - PS;
+		s.low := adr - 4;
+		s.adr := adr;
+		RETURN TRUE
+	END GetStack;
+
+	(* ===== Heap ===== *)
+	PROCEDURE ValidHeapAddress*(p: ADDRESS): BOOLEAN;
+	BEGIN
+		RETURN (memHeapStart <= p) & (p < memHeapStop) & (p MOD 4 = 0)
+	END ValidHeapAddress;
+
+	(* Free unused memory block *)
+	PROCEDURE FreeMemBlock*(memBlock: MemoryBlock);
+	BEGIN
+		HALT(515) (* impossible to free heap *)
+	END FreeMemBlock;
+
+	(** Set memory block end address *)
+	PROCEDURE SetMemoryBlockEndAddress*(memBlock: MemoryBlock; endBlockAdr: ADDRESS);
+	BEGIN
+		ASSERT(endBlockAdr >= memBlock.beginBlockAdr);
+		memBlock.endBlockAdr := endBlockAdr
+	END SetMemoryBlockEndAddress;
+
+	(* Policy decision for heap expansion. NewBlock for the same block has failed try times. *)
+	(*PROCEDURE ExpandNow(try: LONGINT): BOOLEAN;
+	VAR size: SIZE;
+	BEGIN
+		size := LSH(memBlockTail.endBlockAdr - memBlockHead.beginBlockAdr, -10);	(* heap size in KB *)
+		RETURN (~ODD(try) OR (size < heapMinKB)) & (size < heapMaxKB)
+	END ExpandNow;*)
+
+	(** Attempt to set the heap end address to the specified address. The returned value is the actual new end address (never smaller than previous value). *)
+	(*PROCEDURE SetHeapEndAdr(VAR endAdr: ADDRESS);
+	VAR n, m: SIZE;
+	BEGIN
+		HALT(100);
+		Acquire(Memory);
+		(*
+		n := LSH(endAdr+(PS-1), -PSlog2) - LSH(heapEndAdr, -PSlog2);	(* pages requested *)
+		m := LSH(pageHeapAdr, -PSlog2) - LSH(heapEndAdr, -PSlog2) - ReservedPages;	(* max pages *)
+		IF n > m THEN n := m END;
+		IF n > 0 THEN INC(heapEndAdr, n*PS); DEC(freeHighPages, n) END;
+		endAdr := heapEndAdr;
+		*)
+		Release(Memory)
+	END SetHeapEndAdr;*)
+
+	(* Try to expand the heap by at least "size" bytes *)
+	PROCEDURE ExpandHeap*(try: LONGINT; size: SIZE; VAR memBlock: MemoryBlock; VAR beginBlockAdr, endBlockAdr: ADDRESS);
+	BEGIN
+		(*IF ExpandNow(try) THEN
+			IF size < expandMin THEN size := expandMin END;
+			beginBlockAdr := memBlockHead.endBlockAdr;
+			endBlockAdr := beginBlockAdr;
+			INC(endBlockAdr, size);
+			SetHeapEndAdr(endBlockAdr);	(* in/out parameter *)
+			memBlock := memBlockHead;
+			(* endBlockAdr of memory block is set by caller after free block has been set in memory block - this process is part of lock-free heap expansion *)
+		ELSE*)
+			beginBlockAdr := memBlockHead.endBlockAdr;
+			endBlockAdr := memBlockHead.endBlockAdr;
+			memBlock := NIL
+		(*END*)
+	END ExpandHeap;
+
+	(** GetStaticHeap - get page range (beginAdr..endAdr-1) and first and last block of static heap.*)
+	PROCEDURE GetStaticHeap*(VAR beginBlockAdr, endBlockAdr, freeBlockAdr: ADDRESS);
+	BEGIN
+		beginBlockAdr := initialMemBlock.beginBlockAdr;
+		endBlockAdr := initialMemBlock.endBlockAdr;
+		freeBlockAdr := beginBlockAdr;
+	END GetStaticHeap;
+
+	(* ===== Caches, TLB & other ===== *)
+	(** SHR - logical shift right *)
+	PROCEDURE SHR(value, shift: ADDRESS): LONGINT;
+	CODE
+		LDR R0, [FP, #value]
+		LDR R1, [FP, #shift]
+
+		MOV R0, R0, LSR R1
+	END SHR;
+
+	(** SHRL - shift right and left. Mask out 'shift' lowest bits *)
+	PROCEDURE SHRL(value, shift: LONGINT): LONGINT;
+	(*CODE
+		LDR R0, [FP, #value]
+		LDR R1, [FP, #shift]
+
+		MOV R0, R0, LSR R1
+		MOV R0, R0, LSL R1*)
+	BEGIN
+		value := LSH(value, -shift);
+		value := LSH(value, shift);
+		RETURN value
+	END SHRL;
+
+	(** Fills 'size' bytes with 'filler', from 'destAdr' on. size must be multiple of 4 *)
+	PROCEDURE Fill32*(destAdr: ADDRESS; size: SIZE; filler: LONGINT);
+	CODE
+		LDR	R0, [FP, #filler]
+		LDR	R1, [FP, #size]
+		LDR	R3, [FP, #destAdr]
+		ADD	R4, R1, R3				; R4 = size + destAdr
+
+		AND	R5, R3, #3				; R5 := R3 MOD 4
+		CMP	R5, #0					; ASSERT(R5 = 0)
+		BEQ	CheckSize
+		SWI	#8
+
+	CheckSize:
+		AND	R5, R1, #3				; R5 := R1 MOD 4
+		CMP	R5, #0					; ASSERT(R5 = 0)
+		BEQ	Loop
+		SWI	#8
+
+	Loop:
+		CMP	R4, R3
+		BLS		Exit
+		STR	R0, [R3, #0]			; put(destAdr + counter, filler)
+		ADD	R3, R3, #4				; R3 := R3 + 4
+		B		Loop
+	Exit:
+	END Fill32;
+
+	(* GetPageTableBase - returns the memory address of the first level page table *)
+	PROCEDURE -GetPageTableBase(): LONGINT;
+	CODE
+		MRC p15, 0, R0, c2, c0, 0
+		MOV R0, R0, LSR #14	; clear bits 13..0
+		MOV R0, R0, LSL #14
+	END GetPageTableBase;
+
+	(** GC Initialization -- Set machine-dependent parameters gcThreshold, expandMin, heapMinKB and heapMaxKB *)
+	PROCEDURE SetGCParams*;
+	VAR size, t: SIZE;
+	BEGIN
+		GetFreeK(size, t, t);	(* size is total memory size in KB *)
+		heapMinKB := size * HeapMin DIV 100;
+		heapMaxKB := size * HeapMax DIV 100;
+		expandMin := size * ExpandRate DIV 100 * 1024;
+		IF expandMin < 0 THEN expandMin := MAX(LONGINT) END;
+		gcThreshold := size * Threshold DIV 100 * 1024;
+		IF gcThreshold < 0 THEN gcThreshold := MAX(LONGINT) END
+	END SetGCParams;
+
+	(** Called when spinning, just does a NOP *)
+	PROCEDURE -SpinHint*;
+	CODE
+		MOV R0, R0
+	END SpinHint;
+
+	(** Convert a string to an integer. Parameter i specifies where in the string scanning should begin (usually 0 in the first call). Scanning stops at the first non-valid character, and i returns the updated position. Parameter s is the string to be scanned. The value is returned as result, or 0 if not valid. Syntax: number = ["-"] digit {digit} ["H" | "h"] . digit = "0" | ... "9" | "A" .. "F" | "a" .. "f" . If the number contains any hexdecimal letter, or if it ends in "H" or "h", it is interpreted as hexadecimal. *)
+	PROCEDURE StrToInt* (VAR i: LONGINT; CONST s: ARRAY OF CHAR): LONGINT;
+	VAR vd, vh, sgn, d: LONGINT; hex: BOOLEAN;
+	BEGIN
+		vd := 0; vh := 0; hex := FALSE;
+		IF s[i] = "-" THEN sgn := -1; INC (i) ELSE sgn := 1 END;
+		LOOP
+			IF (s[i] >= "0") & (s[i] <= "9") THEN d := ORD (s[i])-ORD ("0")
+			ELSIF (CAP (s[i]) >= "A") & (CAP (s[i]) <= "F") THEN d := ORD (CAP (s[i]))-ORD ("A") + 10; hex := TRUE
+			ELSE EXIT
+			END;
+			vd := 10*vd + d; vh := 16*vh + d;
+			INC (i)
+		END;
+		IF CAP (s[i]) = "H" THEN hex := TRUE; INC (i) END;	(* optional H *)
+		IF hex THEN vd := vh END;
+		RETURN sgn * vd
+	END StrToInt;
+
+	(** Read Non Volatile memory. Not implemented. Required by clock. *)
+	PROCEDURE GetNVByte* (ofs: LONGINT): CHAR;
+	BEGIN
+		RETURN 10X
+	END GetNVByte;
+
+	(** Write Non Volatile memory. Not implemented. Required by clock. *)
+	PROCEDURE PutNVByte* (ofs: LONGINT; val: CHAR);
+	END PutNVByte;
+
+	(* empty section allocated at end of bootfile *)
+	PROCEDURE {NOPAF, ALIGNED(32)} LastAddress;
+	CODE
+	END LastAddress;
+
+	PROCEDURE GetConfig*(CONST name: ARRAY OF CHAR; VAR res: ARRAY OF CHAR);
+	BEGIN
+		BootConfig.GetValue(name, res)
+	END GetConfig;
+
+	PROCEDURE MulH * (x, y: HUGEINT): HUGEINT;
+	BEGIN
+		RETURN LONGINT(x) * LONGINT(y)
+	END MulH;
+
+	PROCEDURE DivH * (x, y: HUGEINT): HUGEINT;
+	BEGIN
+		RETURN x DIV y
+	END DivH;
+
+	(** Not implemented yet *)
+	PROCEDURE  Portin8*(port: LONGINT; VAR val: CHAR);
+	END Portin8;
+
+	PROCEDURE  Portout8*(port: LONGINT; val: CHAR);
+	END Portout8;
+
+	PROCEDURE TraceState * (CONST state: State);
+	BEGIN
+		Trace.StringLn("General Purpose Registers");
+		Trace.String("R0	"); Trace.Address(state.R[0]); Trace.Ln;
+		Trace.String("R1	"); Trace.Address(state.R[1]); Trace.Ln;
+		Trace.String("R2	"); Trace.Address(state.R[2]); Trace.Ln;
+		Trace.String("R3	"); Trace.Address(state.R[3]); Trace.Ln;
+		Trace.String("R4	"); Trace.Address(state.R[4]); Trace.Ln;
+		Trace.String("R5	"); Trace.Address(state.R[5]); Trace.Ln;
+		Trace.String("R6	"); Trace.Address(state.R[6]); Trace.Ln;
+		Trace.String("R7	"); Trace.Address(state.R[7]); Trace.Ln;
+		Trace.String("R8	"); Trace.Address(state.R[8]); Trace.Ln;
+		Trace.String("R9	"); Trace.Address(state.R[9]); Trace.Ln;
+		Trace.String("R10	"); Trace.Address(state.R[10]); Trace.Ln;
+		Trace.String("R11	"); Trace.Address(state.R[11]); Trace.Ln;
+
+		Trace.StringLn("Stack");
+		Trace.String("SP	"); Trace.Address(state.SP); Trace.Ln;
+		Trace.String("FP	"); Trace.Address(state.BP); Trace.Ln;
+
+		Trace.StringLn("Code");
+		Trace.String("LR	"); Trace.Address(state.LR); Trace.Ln;
+		Trace.String("PC	"); Trace.Address(state.PC); Trace.Ln;
+
+		Trace.Ln;
+		Trace.String("PSR	"); Trace.Address(state.PSR); Trace.Ln;
+
+		Trace.Ln;
+		Trace.String("int	"); Trace.Address(state.INT); Trace.Ln;
+	END TraceState;
+
+	PROCEDURE ReadMemLayout;
+	VAR
+		value: ARRAY 64 OF CHAR;
+		i: LONGINT;
+	BEGIN
+		memSysLowStart := Platform.OCMStart;
+		memSysLowStop := memSysLowStart + Platform.OCMSize;
+
+		GetConfig("KernelLoadAdr", value);
+		i := 0;
+		memHeapStart := StrToInt(i, value);
+		GetConfig("HeapSize", value);
+		i := 0;
+		memHeapStop := memHeapStart + StrToInt(i, value);
+
+		GetConfig("DDRSize", value);
+		i := 0;
+		memConfigStop := Platform.DDRStart + StrToInt(i, value);
+		GetConfig("ConfigSize", value);
+		i := 0;
+		memConfigStart := memConfigStop - StrToInt(i, value);
+
+		memStackStart := memHeapStop;
+		memStackStop := memConfigStart;
+
+		memIOStart := Platform.IOStart;
+		memIOStop := memIOStart + Platform.IOSize;
+
+		memSysHighStart := ADDRESS(4 * G - M);
+		memSysHighStop := 4 * G - 1;
+
+		(* System Parameters *)
+		sysIntStackStart := memSysLowStart + 1000H;
+		sysIntStackStop := memSysLowStart + Platform.MaxCpuNb * 4 * 4 * k;
+
+		(* The first level page table is always 16 k large, to map all 4 G of virtual memory space *)
+		sysFirstLvlPtStop := memSysLowStop;
+		sysFirstLvlPtStart := sysFirstLvlPtStop - 16 * k;
+		(*
+		 * Second Level Page Table:
+		 *		- 2 * 256 entries for the system area (first and last MB of VMem)
+		 *		- 256 entries for each MB of virtual stack space
+		 * 256 entries take 1kB memory space.
+		 *)
+		sysSecondLvlPtStop := sysFirstLvlPtStart;
+		sysSecondLvlPtStart := sysSecondLvlPtStop - (2 + (memStackStop - memStackStart + memConfigStop - memConfigStart) DIV M) * k;
+		(*
+		 * Interrupt Vector. Located at 0FFFFFFF0H
+		 *)
+		sysVectorStart := 0FFFF0000H;
+		sysVectorStop := sysVectorStart + PS;
+		sysCacheRefStart := 0FFFF1000H;
+		(*
+		 * Number of ref counters: 1 per 1st level heap page, 1 per 2nd level stack page.
+		 * This memory region is organized as follows: the first part [0 .. SysCacheStackOfs) is used for heap pages,
+		 * the second part, [SysCacheStackOfs .. SysCacheRefSize) is used for stack pages.
+		 *)
+		sysCacheRefSize := (memHeapStop - memHeapStart) DIV M + (memStackStop - memStackStart) DIV PS;
+		INC(sysCacheRefSize, 4 - sysCacheRefSize MOD 4);
+		(* Offset in the ref count table for stack pages. *)
+		sysCacheStackOfs := (memHeapStop - memHeapStart) DIV M;
+
+		sysCacheRefStop := sysCacheRefStart + sysCacheRefSize;
+
+		(* Process stack system *)
+		GetConfig("StackSize", value);
+		i := 0;
+		maxUserStackSize := StrToInt(i, value);
+		maxUserStacks := (memStackStop - memStackStart) DIV maxUserStackSize;
+
+		GetConfig("EnableCaching", value);
+		enableCaching := value[0] # '0';
+
+		traceInterrupts := BootConfig.GetBoolValue("TraceInterrupts")
+	END ReadMemLayout;
+
+	PROCEDURE GetProcessorNumber;
+	VAR
+		value: ARRAY 64 OF CHAR;
+		i: LONGINT;
+	BEGIN
+		FOR i := 0 TO 63 DO value[i] := 0X END;
+		GetConfig("CpuNb", value);
+		i := 0;
+		numProcessors := StrToInt(i, value)
+	END GetProcessorNumber;
+
+	(** Analyse reasons for reset, print and record them. *)
+	PROCEDURE ResetAnalysis;
+	CONST
+		(* We need these constants as Platform.slcr is not initialized yet *)
+		SLCR_REBOOT_STATUS = ADDRESS(0F8000258H);
+		SLCR_LOCK = ADDRESS(0F8000004H);
+		SLCR_UNLOCK = ADDRESS(0F8000008H);
+		Lock = LONGINT(767BH);
+		Unlock = LONGINT(0DF0DH);
+	VAR
+		val: SET;
+		error: LONGINT;
+	BEGIN
+		SYSTEM.GET(SLCR_REBOOT_STATUS, val);
+		IF 16 IN val THEN
+			lastReboot := RebootSystemWatchdog;
+			Trace.StringLn("Starting after system watchdog reset")
+		END;
+		IF 17 IN val THEN
+			lastReboot := RebootWatchdogCPU0;
+			Trace.StringLn("Starting after private watchdog reset on CPU0")
+		END;
+		IF 18 IN val THEN
+			lastReboot := RebootWatchdogCPU1;
+			Trace.StringLn("Starting after private watchdog reset on CPU1")
+		END;
+		IF 19 IN val THEN
+			lastReboot := RebootSoftReset;
+			Trace.StringLn("Starting after software reboot")
+		END;
+		IF 20 IN val THEN
+			lastReboot := RebootDebug;
+			Trace.StringLn("Starting after debug reset")
+		END;
+		IF 21 IN val THEN
+			lastReboot := RebootHardReset;
+			Trace.StringLn("Starting after hard reset")
+		END;
+		IF 22 IN val THEN
+			lastReboot := RebootPowerOn;
+			Trace.StringLn("Starting after power on")
+		END;
+		error := SYSTEM.VAL(INTEGER, val);
+		IF error # 0 THEN
+			Trace.String("BootROM error code: "); Trace.Int(error, 0); Trace.Ln
+		END;
+		SYSTEM.PUT(SLCR_UNLOCK, Unlock);
+		SYSTEM.PUT(SLCR_REBOOT_STATUS, LONGINT(0));
+		SYSTEM.PUT(SLCR_LOCK, Lock)
+	END ResetAnalysis;
+
+	PROCEDURE InitWatchdog;
+	VAR val: ARRAY 32 OF CHAR;
+	BEGIN
+		GetConfig("EnableKernelWatchdog", val);
+		enableWatchdog := val = '1';
+		IF enableWatchdog THEN
+			PrivateWatchdog.Init(BootConfig.GetIntValue("CpuClockHz") DIV 2);
+			PrivateWatchdog.Start(PrivateWatchdog.Reset, Second)
+		END
+	END InitWatchdog;
+
+	PROCEDURE Init;
+	BEGIN
+		BootConfig.Init;
+		ReadMemLayout;
+
+		sp := GetSP();
+		fp := GetBP();
+
+		SYSTEM.LDPSR( 0, Platform.IRQMode + Platform.FIQDisabled + Platform.IRQDisabled );
+		SYSTEM.SETSP(sysIntStackStart + 1000H);
+
+		SYSTEM.LDPSR( 0, Platform.UndefMode + Platform.FIQDisabled + Platform.IRQDisabled );
+		SYSTEM.SETSP(sysIntStackStart + 1000H * 2);
+
+		SYSTEM.LDPSR( 0, Platform.AbortMode + Platform.FIQDisabled + Platform.IRQDisabled );
+		SYSTEM.SETSP(sysIntStackStart + 1000H * 3);
+
+		SYSTEM.LDPSR( 0, Platform.SVCMode + Platform.FIQDisabled + Platform.IRQDisabled );
+		SYSTEM.SETSP(sysIntStackStart + 1000H * 4);
+
+		SYSTEM.LDPSR( 0, Platform.SystemMode + Platform.FIQDisabled + Platform.IRQDisabled );   (* Disable interrupts, init SP, FP *)
+		SYSTEM.SETSP(sp);
+		SYSTEM.SETFP(fp);
+
+		TraceDevice.Install;
+		Trace.String("Machine: "); Trace.StringLn (Version);
+		version := Version;
+		InitProcessor;
+		InstallDefaultInterrupts;
+		DisableInterrupts;
+		ResetAnalysis;
+
+		(* Ensure memory layout consistency *)
+		ASSERT(sysIntStackStart >= 4 * k);
+		ASSERT(sysIntStackStop <= sysSecondLvlPtStart);
+		ASSERT(sysFirstLvlPtStart MOD (16 * k) = 0);
+		ASSERT(memStackStop <= memIOStart);
+		ASSERT(memIOStop <= memSysHighStart);
+		ASSERT(sysVectorStop <= sysCacheRefStart);
+		ASSERT(sysCacheRefStop <= memSysHighStop);
+
+		InitMemory;
+		GetProcessorNumber;
+		InitLocks;
+		InitGlobalTimer
+	END Init;
+	
+	PROCEDURE {INITIAL} Initial;
+	BEGIN
+		Init;
+	END Initial;
+	
+END Machine.

source/ARM.Objects.Mod

@@ -0,0 +1,1882 @@
+MODULE Objects;	(** AUTHOR "pjm"; PURPOSE "Active object runtime support"; *)
+
+IMPORT SYSTEM, Trace, Machine, Heaps, Modules;
+
+CONST
+	(** Process flags *)
+	Restart* = 0;			(* Restart/Destroy process on exception (hardcoded in compiler (OPC.CallRecBody / PCC.SysStart)) *)
+	PleaseHalt* = 10;		(* Process requested to Halt itself soon *)
+	Unbreakable*= 11;		(* FINALLY shall not catch HALT exception (PleaseHalt is also set) *)
+	SelfTermination*=12;	(* Indicates the process has requested to terminate ifself (PleaseHalt is also set) *)
+	Preempted* = 27;		(* Has been preempted. *)
+	Resistant* = 28;		(* Can only be destroyed by itself *)
+
+	(** Process modes *)
+	Unknown* = 0; Ready* = 1; Running* = 2; AwaitingLock* = 3;
+	AwaitingCond* = 4; AwaitingEvent* = 5; Suspended* = 6; (* Suspened for compatibility with WinAos, not used for native A2 *)
+	Terminated* = 7;
+
+	(** Process priorities *)
+	MinPriority = 0;							(* only system idle processes run at this priority level *)
+	Low* = 1; Normal* = 2; High* = 3;		(* "user" priorities *)
+	GCPriority* = 4;							(* priority of garbage collector *)
+	Realtime* = 5;							(* reserved for interrupt handling and realtime apps, these processes are not allowed to allocate memory *)
+	NumPriorities = Heaps.NumPriorities; 	(* number of priority levels *)
+
+	(* Process termination halt codes *)
+	halt* = 2222;
+	haltUnbreakable* = 2223;
+
+	MinIRQ = Machine.IRQ0;
+	NumIRQ = Machine.MaxIRQ-MinIRQ+1;
+
+	Stats* = FALSE;	  (* maintain statistical counters *)
+	TraceVerbose = FALSE;	(* write out verbose trace info *)
+	StrongChecks = TRUE;	(* strong sanity checks *)
+	VeryConservative = FALSE;	(* temp - be very conservative about stack-based pointers *)
+	YieldTrick = FALSE;	(* avoid yield when no ready process available *)
+
+	HandlePriorityInv = TRUE; (* enables or disables priority inversion handling. Handling of priority inversion leads to a simplified locking, see Lock, Unlock and Await *)
+
+	(* constant used in GC Process.FindPointers *)
+	InitDiff = MAX(LONGINT);
+
+	AddressSize = SIZEOF(ADDRESS);
+	ReturnStackDisplacement = 2 * AddressSize;
+	
+TYPE
+	CpuCyclesArray* = ARRAY Machine.MaxCPU OF HUGEINT;
+
+	EventHandler* = PROCEDURE {DELEGATE};
+
+	Timer* = POINTER TO RECORD
+		next*, prev* : Timer;
+		trigger: LONGINT;
+		handler: EventHandler
+	END;
+
+	ProtectedObject = POINTER TO RECORD END; (* protected object *)
+
+	ProcessQueue = Heaps.ProcessQueue;
+
+	Body = PROCEDURE (self: ProtectedObject);
+	Condition = PROCEDURE (slink: ADDRESS): BOOLEAN;
+
+	InterruptList = POINTER TO RECORD
+		next: InterruptList;
+		handler: EventHandler
+	END;
+
+TYPE
+
+	(** All exported fields and variables should be considered read-only. *)
+	Process* = OBJECT (Heaps.ProcessLink)
+	VAR
+		rootedNext : Process;	(** for rootedProcesses *)
+		obj-: ProtectedObject;	(** associated active object *)
+		state-: Machine.State;	(** processor state of suspended process *)
+		sse: Machine.NEONState;	(* fpu and sse state of preempted process (only valid if Preempted IN flag) *)
+		sseAdr: ADDRESS;
+		condition-: Condition;	(** awaited process' condition *)
+		condFP-: ADDRESS;	(** awaited process' condition's context *)
+		mode-: LONGINT;	(** process state *)	(* only changed inside Objects lock ??? *)
+		procID-: LONGINT;	(** processor ID where running *)
+		waitingOn-: ProtectedObject;	(** obj this process is waiting on (for lock or condition) *)
+		id-: LONGINT;	(** unique process ID for tracing *)
+		flags*: SET;	(** process flags *)
+		priority-, staticPriority*: LONGINT;	(** process dynamic priority (can change during priority inversion handling) and static priority *)	(* exported for AosExceptions *)
+		stack*: Machine.Stack;	(** user-level stack of process *)
+		restartPC-: ADDRESS;	(** entry point of body, for SAFE exception recovery *)
+		restartSP-: ADDRESS;	(** stack level at start of body, for SAFE exception recovery *)
+		exp*: Machine.ExceptionState;
+		oldReturnPC: ADDRESS;
+		cpuCycles-, lastCpuCycles- : CpuCyclesArray;
+		prioRequests : ARRAY NumPriorities OF LONGINT; (* priorities of processes that wait for resources locked by this process, only the highest priority per resource is stored *)
+		context: ANY;
+
+		(* set priority of process: Machine.Objects lock is taken *)
+		PROCEDURE SetPriority(p : LONGINT);
+		BEGIN
+			DEC(prioRequests[staticPriority]);
+			staticPriority := p;
+			INC(prioRequests[staticPriority]);
+			priority := MaxPrio(prioRequests)
+		END SetPriority;
+
+		PROCEDURE FindRoots; (* override *)
+		VAR pc, bp, curbp, sp: ADDRESS; d0, d1: SIZE; first : BOOLEAN;
+		BEGIN
+			IF traceProcess # NIL THEN traceProcess(SELF) END;
+
+					(* stack garbage collection *)
+			IF (priority >= Low) & (priority <= High) & (mode >= Ready) & (mode # Terminated) THEN
+			(* only processes with priority < GCPriority are preempted during GC,
+			    only those are allowed to allocate memory and their stacks are inspected.
+			    Furthermore, the process must be in a valid state, e.g. terminated processes have a disposed stack. *)
+
+				IF Heaps.GCType = Heaps.HeuristicStackInspectionGC THEN
+
+					IF VeryConservative THEN
+						Heaps.RegisterCandidates(stack.adr, stack.high-stack.adr)
+					ELSE
+						sp := state.SP;	(* cf. Enter *)
+						IF sp # 0 THEN
+							IF Machine.ValidStack(stack, sp) THEN
+								Heaps.RegisterCandidates(sp, stack.high - sp)
+							END
+						ELSE
+							Trace.String("[Objects.FindRoots sp=0]")
+						END
+					END;
+					
+					IF TraceProcessHook # NIL THEN
+						bp := state.BP; pc := state.PC; sp := state.SP;
+						TraceProcessHook(SELF,pc,bp,sp,stack.high);
+					END;
+
+				ELSIF Heaps.GCType = Heaps.MetaDataForStackGC THEN
+
+					bp := state.BP; pc := state.PC; first := TRUE;
+					IF pc # 0 THEN 	(* process is running already *)
+						WHILE (bp # Heaps.NilVal) & (stack.adr <= bp) & (bp < stack.high) DO
+							FindPointers(bp, pc, d0, d1);
+ 							IF first THEN
+ 								IF (d0 = 0) OR (d0 = 1) OR (d1 = 3) THEN
+ 									(* 	situation where pc and bp are not synchronized: *)
+ 									(* 	entry protocol of a procedure:
+ 										PUSH 	EBP			-- 1 byte instruction length, if pc points to this instruction at offset 0 from the codeoffset then bp still refers to caller frame -> critical
+ 										MOV	EBP, ESP	-- 2 bytes instruction length, do. for offset 1 from the codeoffset
+ 										(followed by initialization of local variables)
+ 										exit protocol of a procedure:
+ 										MOV	ESP, EBP	-- 2 bytes instruction length
+ 										POP	EBP			-- 1 byte instruction length
+ 										RET		n			-- 3 bytes instruction length, if pc points to this instruction at offset 3 from the last statement then bp already refers to caller's frame -> critical
+ 									*)
+ 									IF (d0 = 0) OR (d1 = 3) THEN
+ 										SYSTEM.GET(state.SP, pc);		(* matching pc is at position of stack pointer *)
+ 									ELSE
+ 										SYSTEM.GET(state.SP+AddressSize, pc);		(* matching pc is at 4 bytes after stack pointer, pushed base pointer is at stack pointer position *)
+									END;
+ 								ELSE
+ 									(* regular case: bp and pc were synchronized *)
+ 									curbp := bp;
+									SYSTEM.GET(curbp, bp);
+									SYSTEM.GET(curbp+AddressSize, pc);
+ 								END;
+ 								first := FALSE;
+ 							ELSE
+ 								(* regular case: bp and pc were synchronized *)
+ 								curbp := bp;
+								SYSTEM.GET(curbp, bp);
+								SYSTEM.GET(curbp+AddressSize, pc);
+							END
+						END
+					END
+
+				ELSE
+					HALT(900) (* wrong GCType constant *)
+				END
+
+			END
+
+		END FindRoots;
+		
+		(*!TODO: adapt the code according to the new Modules/Reflection *)
+		PROCEDURE FindPointers(bp, pc : ADDRESS; VAR diff0, diff1: SIZE);
+		(*VAR data: Modules.ProcTableEntry; startIndex, i: LONGINT; ptr : ADDRESS; success: BOOLEAN;
+		BEGIN
+			diff0 := InitDiff; diff1 := InitDiff;
+			(*Modules.FindProc(pc, data, startIndex, success);*)
+			IF success THEN
+				diff0 := pc - data.pcFrom;
+				diff1 := pc - data.pcStatementEnd;
+				IF (data.noPtr > 0) & (pc >= data.pcStatementBegin) & (pc <= data.pcStatementEnd) THEN
+					FOR i := 0 TO data.noPtr - 1 DO
+						SYSTEM.GET(bp + Modules.ptrOffsets[startIndex + i], ptr);
+						IF ptr # Heaps.NilVal THEN
+							Heaps.Mark(SYSTEM.VAL(ANY, ptr))
+						END
+					END
+				END
+			END*)
+		END FindPointers;
+
+	END Process;
+
+	TraceProcess* = PROCEDURE (p: Process);
+
+	ExceptionHandler* = PROCEDURE(p: Process; VAR int: Machine.State; VAR exc: Machine.ExceptionState; VAR return: BOOLEAN);
+
+	Idle = OBJECT
+	BEGIN {ACTIVE, SAFE, PRIORITY(-1)}	(* negative priority equivalent to MinPriority *)
+		LOOP
+			REPEAT
+				IF ProcessorHLT # NIL THEN ProcessorHLT	(* UP *)
+				ELSE
+					Machine.SpinHint; (* MP *)
+				END;
+			UNTIL maxReady >= lowestAllowedPriority;
+			Yield
+		END
+	END Idle;
+
+	Clock = OBJECT
+	VAR h: Timer;
+	BEGIN {ACTIVE, SAFE, PRIORITY(High)}
+		LOOP
+			Machine.Acquire(Machine.Objects);
+			LOOP
+				h := event.next;
+				IF (h = event) OR (h.trigger - Machine.ticks > 0) THEN EXIT END;
+				event.next := h.next; event.next.prev := event; (* unlink *)
+				h.next := NIL; h.prev := NIL;
+				Machine.Release(Machine.Objects);
+				h.handler;	(* assume handler will return promptly *)
+				Machine.Acquire(Machine.Objects)
+			END;
+			ASSERT(timer = NIL); (* temp strong check *)
+			timer := running[Machine.ID ()];
+			timer.mode := AwaitingEvent;
+			SwitchToNew
+		END
+	END Clock;
+
+	ReadyProcesses = OBJECT(Heaps.RootObject)
+		VAR q {UNTRACED}: ARRAY NumPriorities OF ProcessQueue;
+
+		PROCEDURE &Init;
+		VAR i: LONGINT;
+		BEGIN
+			FOR i := 0 TO NumPriorities - 1 DO
+				q[i].head := NIL; q[i].tail := NIL
+			END
+		END Init;
+
+		PROCEDURE FindRoots; (* override *)
+		VAR i: LONGINT;
+		BEGIN
+			(* only mark queues of user processes since these will not change during GC *)
+			FOR i := Low TO High DO
+				Heaps.Mark(q[i].head);
+				Heaps.Mark(q[i].tail)
+			END
+		END FindRoots;
+
+	END ReadyProcesses;
+
+	GCStatusExt = OBJECT(Heaps.GCStatus)
+		VAR gcOngoing: BOOLEAN;
+
+		PROCEDURE &Init;
+		BEGIN
+			gcOngoing := FALSE;
+		END Init;
+
+		(* called from Heaps.InvokeGC, i.e. this is a hidden upcall. However, it is necessary to take the Machine.Objects lock here since writing
+		    the set of variables here must not be interrupted, i.e. atomic writing of the set of variables is absolutely necessary.  They system may hang
+		    if the lock is not taken. *)
+		PROCEDURE SetgcOngoing(value: BOOLEAN);
+		VAR p: Process;
+		BEGIN
+			IF value THEN
+				Machine.Acquire(Machine.Objects);
+				IF ~gcOngoing THEN
+					gcOngoing := TRUE;
+					lowestAllowedPriority := GCPriority;
+					gcBarrier := Machine.allProcessors
+				END;
+				p := running[Machine.ID()];
+				Enter(p);
+				p.mode := Ready;
+				SwitchToNew (* this method cannot schedule the running user process with priority Low, Normal or High since
+				                            lowestAllowedPriority is set to GCPriority *)
+			ELSE
+				Machine.Acquire(Machine.Objects);
+				gcOngoing := FALSE;
+				lowestAllowedPriority := Low;
+				Machine.Release(Machine.Objects)
+			END;
+		END SetgcOngoing;
+
+		(* caller must hold Machine.Objects lock *)
+		PROCEDURE GetgcOngoing(): BOOLEAN;
+		BEGIN
+			RETURN gcOngoing
+		END GetgcOngoing;
+
+	END GCStatusExt;
+
+	GCActivity = OBJECT
+	BEGIN {ACTIVE, SAFE, PRIORITY(GCPriority)}
+		UpdateState;
+		LOOP
+			Machine.Acquire(Machine.Objects);
+
+			ASSERT(gcProcess = NIL); (* temp strong check *)
+			gcProcess := running[Machine.ID()];
+			gcProcess.mode := AwaitingEvent;
+			SwitchToNew; (* SwitchTo called by SwitchToNew will release the lock Machine.Objects *)
+
+			(* process is scheduled -> gcProcess = NIL set by scheduler (Timeslice), perform garbage collection now *)
+			Heaps.CollectGarbage(Modules.root);
+			Machine.Acquire(Machine.Objects);
+			IF finalizerProcess # NIL THEN
+			(* it is safe to move finalizerProcess to the ready queue and set the variable to NIL
+			    since the process has been marked by the GC already - marking is finished here *)
+				Enter(finalizerProcess);
+				finalizerProcess := NIL
+			END;
+			Machine.Release(Machine.Objects);
+			Heaps.gcStatus.SetgcOngoing(FALSE)
+		END
+	END GCActivity;
+
+	FinalizedCollection* = OBJECT (* base type for collection, extended in Kernel.Mod *)
+		PROCEDURE RemoveAll*(obj: ANY); (** abstract *)
+		BEGIN HALT(301) END RemoveAll;
+	END FinalizedCollection;
+
+	FinalizerNode* = POINTER TO RECORD (Heaps.FinalizerNode)
+		c*: FinalizedCollection (* base type for collection containing object *)
+	END;
+
+	FinalizerCaller = OBJECT	(* separate active object that calls finalizers *)
+		VAR n: Heaps.FinalizerNode;
+
+	BEGIN {ACTIVE, SAFE, PRIORITY(High)}
+		LOOP
+			Machine.Acquire(Machine.Objects);
+
+			ASSERT(finalizerProcess = NIL); (* temp strong check *)
+			finalizerProcess := running[Machine.ID()];
+			finalizerProcess.mode := AwaitingEvent;
+			SwitchToNew; (* SwitchTo called by SwitchToNew will release the lock Machine.Objects *)
+
+			(* process is scheduled -> finalizerProcess = NIL set by GCActivity, perform finalization now *)
+			LOOP
+				n := Heaps.GetFinalizer();
+				IF n = NIL THEN EXIT END;
+				IF n IS FinalizerNode THEN
+					n(FinalizerNode).c.RemoveAll(n.objStrong)	(* remove it if it is not removed yet *)
+				END;
+				IF n.finalizer # NIL THEN
+					n.finalizer(n.objStrong)	(* may acquire locks *)
+				END
+			END;
+		END
+	END FinalizerCaller;
+
+	Interrupter = OBJECT (ProtectedObject)	(* to do: like Timer *)
+	VAR interruptNumber: LONGINT;
+	END Interrupter;
+
+VAR
+	ready: ReadyProcesses;	(* ready queue represented as an object that contains the queues *)
+	maxReady: LONGINT;	(* for all i : MinPriority <= maxReady < i < NumPriorities : Empty(ready.q[i]) *)
+	lowestAllowedPriority: LONGINT;  (* denotes the minimal user or realtime priority greater than the idle priority that can be
+	                                                            scheduled depending on the GC status, minPriority = Low if GC is not running,
+	                                                            minPrioriy = GCPriority otherwise *)
+	running-{UNTRACED}: ARRAY Machine.MaxCPU OF Process;	(** processes currently running, exported for Traps, not traced by the GC since it may change during collection *)
+	nextProcessID: LONGINT;
+
+	gcBarrier: SET; 			(* barrier that must be passed by all processors before actual collection starts *)
+	gcActivity: GCActivity;	(* active object for GC handling *)
+	gcProcess: Process;		(* suspended GC process, is NIL when collection has started, not equal NIL when no garbage collection is in process, same behaviour as for timer *)
+	finalizerProcess: Process;	(* finalizer process, regarded as part of GC *)
+	interrupt: ARRAY NumIRQ OF RECORD
+		root: InterruptList;
+		process: Process
+	END;
+
+	processingIRQ: ARRAY NumIRQ OF BOOLEAN;
+
+	rootedProcesses: ARRAY NumPriorities OF Process; (* list of potential processes that are not traced by GC when processing the ready queues, since GC only traces processes with
+	                                                                                      priorities Low ... High in ready queues. The potentially not traced processes are rooted here and traced by the GC *)
+
+	event: Timer;	(* list of events *)
+
+	timer (*, realtimeTimer *): Process;	(* suspended timer processes *)
+	terminate: PROCEDURE;
+	trap, trapReturn: ARRAY 2 OF PROCEDURE;
+
+	ProcessorHLT*: PROCEDURE;	 (** installable procedure to halt the current processor while idle *)
+	traceProcess*: TraceProcess;	(** for debugging purposes (see Info.Active) *)
+
+	entry: ADDRESS;
+	init: Process;
+
+	initObject: ProtectedObject;	(* Active object for the init process *)
+
+	(* Performance monitoring *)
+	idlecount*: ARRAY Machine.MaxCPU OF LONGINT; (** count of idle process timeslice interrupts *)
+	idleCycles- : ARRAY Machine.MaxCPU OF HUGEINT; (** CPU cycles of idle threads *)
+	perfTsc-: ARRAY Machine.MaxCPU OF HUGEINT;
+
+	(* Statistics *)
+	Nlock-, Nunlock-, Nawait-, NawaitNoIF-, NawaitTrue-, Ncreate-, Nterminate-,
+	Ncondition-, Ncondition1True-, Ncondition2-, Ncondition2True-,
+	Ntimeslice-, NtimesliceTaken-, NtimesliceNothing-, NtimesliceIdle-,
+	NtimesliceKernel-, NtimesliceV86-, NtimesliceCritical-,
+	Npreempt-, NpreemptTaken-, NpreemptNothing-,
+	NpreemptKernel-, NpreemptV86-, NpreemptCritical-,
+	Nenter- : LONGINT;
+
+	debugCounter: LONGINT;
+
+PROCEDURE GetMaxPrio(VAR queue: ProcessQueue; VAR new: Process);
+VAR
+	t: Heaps.ProcessLink;
+	maxPriority : LONGINT;
+BEGIN
+	ASSERT(new = NIL);
+	t := queue.head;
+	maxPriority := MIN(LONGINT);
+	WHILE (t # NIL) DO
+		IF (t(Process).priority > maxPriority) THEN
+			new := t(Process); maxPriority := t(Process).priority;
+		END;
+		t := t.next;
+	END;
+	IF new = NIL THEN	(* zero elements in queue *)
+		(* skip *)
+	ELSE	(* more than one element in queue *)
+		IF new.next # NIL THEN new.next.prev := new.prev END;
+		IF new.prev # NIL THEN new.prev.next := new.next END;
+		IF queue.head = new THEN
+			queue.head := new.next
+		END;
+		IF queue.tail = new THEN
+			queue.tail := new.prev
+		END;
+		new.next := NIL; new.prev := NIL
+	END;
+END GetMaxPrio;
+
+(* Get a process from a queue (NIL if none). Caller must hold lock for specific queue. *)
+PROCEDURE Get(VAR queue: ProcessQueue; VAR new: Process);
+VAR t: Heaps.ProcessLink;
+BEGIN
+	t := queue.head;
+	IF t = NIL THEN (* zero elements in queue *)
+		(* skip *)
+	ELSIF t = queue.tail THEN (* one element in queue *)
+		queue.head := NIL; queue.tail := NIL (* {(t.next = NIL) & (t.prev = NIL)} *)
+	ELSE	(* more than one element in queue *)
+		queue.head := t.next; t.next := NIL; queue.head.prev := NIL
+	END;
+	ASSERT((t = NIL) OR (t.next = NIL) & (t.prev = NIL)); (* temp strong check *)
+	IF t = NIL THEN
+		new := NIL
+	ELSE
+		ASSERT(t IS Process);
+		new := t(Process)
+	END;
+END Get;
+
+(* Put a process in a queue. Caller must hold lock for specific queue. *)
+(* If t was running, be careful to protect Put and the subsequent SwitchTo with the ready lock. *)
+PROCEDURE Put(VAR queue: ProcessQueue; t: Process);
+BEGIN (* {t # NIL & t.next = NIL} *)
+	ASSERT((t.next = NIL) & (t.prev = NIL));
+	IF queue.head = NIL THEN (* queue empty *)
+		queue.head := t
+	ELSE (* queue not empty *)
+		queue.tail.next := t; t.prev := queue.tail
+	END;
+	queue.tail := t
+END Put;
+
+(* Select a process of at least the specified priority to run next on current processor (returns NIL if none). Caller must hold ready lock. *)
+PROCEDURE Select(VAR new: Process; priority: LONGINT);
+VAR thresholdPrio: LONGINT;
+BEGIN
+	IF Heaps.gcStatus.GetgcOngoing() THEN
+		thresholdPrio := GCPriority
+	ELSE
+		thresholdPrio := priority
+	END;
+	LOOP
+		IF maxReady < thresholdPrio THEN
+			IF priority < thresholdPrio THEN Get(ready.q[MinPriority], new) ELSE new := NIL END;
+			EXIT
+		END;
+		Get(ready.q[maxReady], new);
+		IF (new # NIL) OR (maxReady = MinPriority) THEN EXIT END;
+		DEC(maxReady)
+	END
+END Select;
+
+(* Enter a process in the ready queue. Caller must hold ready lock. *)
+(* If t was running, be careful to make Enter and the subsequent SwitchTo atomic, as the process could be grabbed by another process while it is still running. *)
+PROCEDURE Enter(t: Process);
+BEGIN
+	IF Stats THEN Machine.AtomicInc(Nenter) END;
+	t.mode := Ready;
+	Put(ready.q[t.priority], t);
+	IF t.priority > maxReady THEN
+		maxReady := t.priority (* to do: re-establish global priority invariant *)
+	END
+END Enter;
+
+(* Remove a process from a queue that contains it. Caller must hold lock for specific queue. *)
+(* Not intended for frequent use. *)
+PROCEDURE Remove(VAR queue: ProcessQueue; t: Process);
+BEGIN
+	IF t.prev # NIL THEN t.prev.next := t.next END;
+	IF t.next # NIL THEN t.next.prev := t.prev END;
+	IF t = queue.head THEN queue.head := t.next END;
+	IF t = queue.tail THEN queue.tail := t.prev END;
+	ASSERT((queue.head = NIL) OR (queue.head.prev = NIL) & (queue.tail.next = NIL));
+	t.prev := NIL;
+	t.next := NIL
+END Remove;
+
+(* Switch to the specified process. Caller must hold ready lock. Return may be on different processor! *)
+PROCEDURE SwitchTo(VAR running: Process; new: Process);	(* parameters used in SwitchToState, TerminateThis, New *)
+VAR id: LONGINT;
+BEGIN
+	id := Machine.ID ();
+	INC (running.cpuCycles[id], Machine.GetTimer () - perfTsc[id]);
+	(*TRACE(CurrentProcessTime(), perfTsc[id], Machine.GetTimer());*)
+	IF running.priority = MinPriority THEN (* Special treatment for idle threads *)
+		INC (idleCycles[id], Machine.GetTimer () - perfTsc[id]);
+	END;
+	(* save current state *)
+	running.state.PC := SYSTEM.PC();(*Machine.CurrentPC ();*)	(* for GC *) (* ug *)
+	running.state.SP := SYSTEM.SP();(*Machine.CurrentSP ();*)	(* for GC *)
+	running.state.BP := SYSTEM.FP();(*Machine.CurrentBP ();*)	(* save state *)
+	Machine.FPUSaveMin(running.sse);
+	running := new; new.mode := Running;
+	IF Preempted IN new.flags THEN
+		EXCL(new.flags, Preempted);
+		perfTsc[id] := Machine.GetTimer ();
+		(*Machine.SetSP (new.state.SP);*) (* for UpdateState - run on new stack (EBP on old) *)
+		SYSTEM.SETSP(new.state.SP);
+		Machine.PushState(new.state);
+		Machine.FPURestoreFull(new.sse);
+		Machine.Release(Machine.Objects);
+		Machine.JumpState (* pops the state parameter from the stack and returns from interrupt *)
+	ELSE
+		Machine.FPURestoreMin(new.sse);
+		perfTsc[id] := Machine.GetTimer ();
+		SYSTEM.SETSP(new.state.SP);
+		SYSTEM.SETFP(new.state.BP);
+		Machine.Release(Machine.Objects);
+	END
+END SwitchTo;
+
+(* Select a new process to run and switch to it. Caller must hold ready lock. *)
+PROCEDURE SwitchToNew;
+VAR new: Process;
+BEGIN
+	Select(new, MinPriority); (* will return at least an Idle process *)
+	new.procID := Machine.ID ();
+	SwitchTo(running[new.procID], new)
+END SwitchToNew;
+
+PROCEDURE GetProcessPtr(): Process;
+VAR
+	p: Process;
+BEGIN
+	Machine.Acquire(Machine.Objects);
+	p := GetProcessPtr0();
+	Machine.Release(Machine.Objects);
+	RETURN p
+END GetProcessPtr;
+
+PROCEDURE GetProcessPtr0(): Process;
+BEGIN
+	RETURN running[Machine.ID()]
+END GetProcessPtr0;
+
+(** Relinquish control. *)
+PROCEDURE Yield*;
+VAR r, new: Process;
+BEGIN
+	IF ~YieldTrick OR (maxReady >= lowestAllowedPriority) THEN
+		r := GetProcessPtr ();
+		Machine.Acquire(Machine.Objects);
+		Select(new, r.priority);
+		IF new # NIL THEN (* found another process *)
+			Enter(r);
+			new.procID := Machine.ID ();
+			SwitchTo(running[new.procID], new)
+		ELSE (* stay with same process *)
+			Machine.Release(Machine.Objects)
+		END
+	END
+END Yield;
+
+PROCEDURE SwitchToState(new: Process; VAR state: Machine.State);
+BEGIN
+	(* simulate return from SwitchTo - MOV ESP, EBP; POP EBP; RET 8 *)
+	state.SP := new.state.BP+AddressSize*2;	(* AddressSize*4 is effect of POP, RET AddressSize*2 *)
+	SYSTEM.GET (new.state.BP, state.BP);	(* effect of POP *)
+	SYSTEM.GET (new.state.BP + AddressSize, state.PC);	(* effect of RET *)
+END SwitchToState;
+
+(** Preempt the current process. *)
+PROCEDURE Timeslice*(VAR state: Machine.State);
+VAR id: LONGINT; new: Process;
+BEGIN
+	(* handle a timer tick *)
+	Machine.Acquire(Machine.Objects);
+	IF Stats THEN Machine.AtomicInc(Ntimeslice) END;
+	id := Machine.ID ();
+	IF id = 0 THEN (* process 0 checks event queues *)
+		IF event.next.trigger - Machine.ticks <= 0 THEN (* next normal event due *)
+			IF event.next # event THEN (* not dummy event *)
+				IF timer # NIL THEN
+					ASSERT(timer.mode = AwaitingEvent);
+					Enter(timer); timer := NIL
+				END
+			ELSE (* reset dummy event *)
+				event.trigger := Machine.ticks + MAX(LONGINT) DIV 2	(* ignore overflow *)
+			END
+		END
+	END;
+	IF Heaps.gcStatus.GetgcOngoing() & (id IN gcBarrier) THEN
+		EXCL(gcBarrier, id);
+		IF gcBarrier = {} THEN
+			ASSERT(gcProcess.mode = AwaitingEvent);
+			Enter(gcProcess); gcProcess := NIL
+		END
+	END;
+	(* pre-empt the current process *)
+	IF Machine.PreemptCount(id) = 1 THEN (* check against 1, because we are holding one lock *)
+		IF running[id].priority # MinPriority THEN	(* idle processes are not timesliced *)
+			Select(new, running[id].priority);
+			IF new # NIL THEN
+				INC (running[id].cpuCycles[id], Machine.GetTimer () - perfTsc[id]);
+				(*TRACE(CurrentProcessTime(), perfTsc[id], Machine.GetTimer());*)
+				IF Stats THEN Machine.AtomicInc(NtimesliceTaken) END;
+				INCL(running[id].flags, Preempted);
+				Machine.CopyState(state, running[id].state);
+				Machine.FPUSaveFull(running[id].sse); (* to do: floating-point exception possible / Machine.SetupFPU *)
+				Enter(running[id]);
+				running[id] := new;
+				new.mode := Running; new.procID := id;
+				IF Preempted IN new.flags THEN
+					EXCL(new.flags, Preempted);
+					Machine.CopyState(new.state, state);
+					Machine.FPURestoreFull(new.sse)
+				ELSE
+					SwitchToState(new, state);
+					Machine.FPURestoreMin(new.sse)
+				END;
+
+				perfTsc[id] := Machine.GetTimer ()
+			ELSE
+				IF Stats THEN Machine.AtomicInc(NtimesliceNothing) END;
+			END;
+
+			(* Check if the process has the PleasHalt flag and handle it. *)
+			IF PleaseHalt IN running[id].flags THEN
+				(* Simulate procedure call: Increase stack & put return PC *)
+				DEC(state.SP, AddressSize);
+				SYSTEM.PUT (state.SP, state.PC); (* Here an stack overflow could happen! *)
+
+				(* Set the right halt procedure *)
+				IF (Unbreakable IN running[id].flags) THEN
+					state.PC := SYSTEM.VAL (ADDRESS, trap[1]);
+				ELSE
+					state.PC := SYSTEM.VAL (ADDRESS, trap[0]);
+				END;
+			END;
+
+		ELSE
+			INC(idlecount[id]);
+			IF Stats THEN Machine.AtomicInc(NtimesliceIdle) END;
+		END
+	ELSE
+		IF Stats THEN Machine.AtomicInc(NtimesliceCritical) END (* not preemptable *)
+	END;
+	Machine.Release(Machine.Objects)
+END Timeslice;
+
+(** Return current process. (DEPRECATED, use ActiveObject) *)
+PROCEDURE CurrentProcess*( ): Process;
+BEGIN
+	RETURN GetProcessPtr();
+END CurrentProcess;
+
+(** Return current process' context *)
+PROCEDURE CurrentContext*(): ANY;
+VAR p: Process;
+BEGIN
+	p := CurrentProcess();
+	IF p # NIL THEN RETURN p.context
+	ELSE RETURN NIL
+	END;
+END CurrentContext;
+
+PROCEDURE SetContext*(context: ANY);
+VAR p: Process;
+BEGIN
+	p := CurrentProcess();
+	IF p # NIL THEN p.context := context END;
+END SetContext;
+
+(* Return stack bottom of process. For compatibility WinAos/UnixAos/NativeAos  *)
+PROCEDURE GetStackBottom*(p: Process): ADDRESS;
+BEGIN
+	RETURN p.stack.high
+END GetStackBottom;
+
+(** Return the active object currently executing. *)
+PROCEDURE ActiveObject* (): ANY;
+VAR r: Process;
+BEGIN
+	r := GetProcessPtr ();
+	ASSERT(r # NIL);
+	ASSERT(r.obj # NIL);
+	RETURN r.obj
+END ActiveObject;
+
+(** Return the ID of the active currently executing process. *)
+PROCEDURE GetProcessID* (): LONGINT;
+VAR r: Process;
+BEGIN
+	r := GetProcessPtr ();
+	RETURN r.id
+END GetProcessID;
+
+(** Set the current process' priority. *)
+PROCEDURE SetPriority*(priority: LONGINT);
+VAR id: LONGINT;
+BEGIN
+	ASSERT((priority >= Low) & (priority <= Realtime)); (* priority in bounds *)
+	IF HandlePriorityInv THEN
+		Machine.Acquire(Machine.Objects);
+		id := Machine.ID();
+		running[id].SetPriority(priority);
+		Machine.Release(Machine.Objects)
+	ELSE
+		id := Machine.AcquirePreemption ();
+		running[id].priority := priority;
+		Machine.ReleasePreemption
+		(* to do: re-establish global priority invariant *)
+	END
+END SetPriority;
+
+(** Return TRUE iff the specified protected object is locked exclusive to the current process. *)
+PROCEDURE LockedByCurrent*(obj: ANY): BOOLEAN;
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; id: LONGINT; res: BOOLEAN;
+BEGIN
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+	ASSERT(hdr IS Heaps.ProtRecBlock);
+	IF HandlePriorityInv THEN
+		Machine.Acquire(Machine.Objects);
+		id := Machine.ID();
+		res := (hdr.lockedBy = running[id]);
+		Machine.Release(Machine.Objects)
+	ELSE
+		id := Machine.AcquirePreemption ();
+		Machine.AcquireObject(hdr.locked);
+		res := (hdr.lockedBy = running[id]);
+		Machine.ReleaseObject(hdr.locked);
+		Machine.ReleasePreemption;
+	END;
+	RETURN res
+END LockedByCurrent;
+
+(** Return number of ready and running processes, excluding idle processes. *)
+PROCEDURE NumReady* (): LONGINT;
+VAR i, n: LONGINT; p: Heaps.ProcessLink;
+BEGIN
+	n := 0;
+	Machine.Acquire(Machine.Objects);
+	FOR i := MinPriority+1 TO NumPriorities-1 DO
+		p := ready.q[i].head; WHILE p # NIL DO INC(n); p := p.next END
+	END;
+	FOR i := 0 TO Machine.MaxCPU-1 DO
+		IF (running[i] # NIL) & (running[i].priority > MinPriority) THEN INC(n) END
+	END;
+	Machine.Release(Machine.Objects);
+	RETURN n
+END NumReady;
+
+(** Return number of CPU cycles consumed by the specified process for each processor. If all is TRUE,
+	return the number of cycles since the process has been created. If FALSE, return the number of cycles
+	consumed since the last time asked. *)
+PROCEDURE GetCpuCycles*(process : Process; VAR cpuCycles : CpuCyclesArray; all : BOOLEAN);
+VAR i : LONGINT;
+BEGIN
+	ASSERT(process # NIL);
+	FOR i := 0 TO Machine.MaxCPU-1 DO cpuCycles[i] := process.cpuCycles[i]; END;
+	IF ~all THEN
+		FOR i := 0 TO Machine.MaxCPU-1 DO
+			cpuCycles[i] := cpuCycles[i] - process.lastCpuCycles[i];
+			process.lastCpuCycles[i] := process.cpuCycles[i]; (* actually could have changed meanwhile *)
+		END;
+	END;
+END GetCpuCycles;
+
+(*! DEBUG *)
+VAR
+	currentProcessTime- : HUGEINT;
+PROCEDURE CurrentProcessTime * (): HUGEINT;
+VAR result: HUGEINT; process: Process; i: LONGINT;
+BEGIN
+	currentProcessTime := Machine.GetTimer();
+	result := currentProcessTime - perfTsc[Machine.ID()];
+	process := CurrentProcess();
+	FOR i := 0 TO Machine.MaxCPU-1 DO result := result + process.cpuCycles[i]; END;
+	RETURN result;
+END CurrentProcessTime;
+
+PROCEDURE TimerFrequency * (): HUGEINT;
+BEGIN
+	RETURN 333000000
+END TimerFrequency;
+
+(* Handle hardware interrupt and route it to an interrupt handler process. *)
+PROCEDURE FieldIRQ(VAR state: Machine.State);
+VAR t: Process; id: LONGINT; new: Process; preempt: BOOLEAN;
+BEGIN
+	Machine.DisableIRQ(state.INT);	(* do this before acknowledging irq *)
+
+	IF StrongChecks THEN
+		IF processingIRQ[state.INT-MinIRQ] THEN
+			(*Trace.String("IRQ recursion "); Trace.Address(state.INT); Trace.Ln;*)
+			RETURN
+		ELSE
+			processingIRQ[state.INT-MinIRQ] := TRUE;
+		END;
+	END;
+	
+	(* if the reenabling of interrupts cannot be circumvented, then it is REQUIRED to acknowledge interrupts
+		BEFORE reenabling. Otherwise spurious IRQs cannot be identified as such.
+		Please note that this particular problem with spurious IRQs cannot observed on many machines but IF it is observed
+		then the machine will behave unexpected. Very hard to debug and trace!
+
+		Machine.Ack(state.INT); 
+		Machine.Sti ();	(* avoid Processors.StopAll deadlock when waiting for locks below (remove this) *)
+	*)
+	
+	Machine.Acquire(Machine.Objects);
+	(*IF state.INT = 53 THEN Trace.String("|") END;*)
+	t := interrupt[state.INT-MinIRQ].process;
+	IF StrongChecks THEN ASSERT(t.mode = AwaitingEvent) END;
+	id := Machine.ID ();
+	preempt := (t.priority > maxReady) & (maxReady # MinPriority) & (t.priority > running[id].priority);
+	Enter(t);
+	IF preempt THEN
+		IF Stats THEN Machine.AtomicInc(Npreempt) END;
+		(* pre-empt the current process *)
+		IF Machine.PreemptCount(id) = 1 THEN (* check against 1, because we are holding one lock *)
+			Select(new, running[id].priority + 1);
+			IF new # NIL THEN
+				INC (running[id].cpuCycles[id], Machine.GetTimer () - perfTsc[id]);
+				IF running[id].priority = MinPriority THEN (* Special treatment for idle threads *)
+					INC (idleCycles[id], Machine.GetTimer () - perfTsc[id]);
+				END;
+				IF Stats THEN Machine.AtomicInc(NpreemptTaken) END;
+				INCL(running[id].flags, Preempted);
+				Machine.CopyState(state, running[id].state);
+				Machine.FPUSaveFull(running[id].sse); (* to do: floating-point exception possible / Machine.SetupFPU *)
+				Enter(running[id]);
+				running[id] := new;
+				new.mode := Running; new.procID := id;
+				IF Preempted IN new.flags THEN
+					EXCL(new.flags, Preempted);
+					Machine.CopyState(new.state, state);
+					Machine.FPURestoreFull(new.sse)
+				ELSE
+					SwitchToState(new, state);
+					Machine.FPURestoreMin(new.sse)
+				END;
+				perfTsc[id] := Machine.GetTimer ()
+			ELSE
+				IF Stats THEN Machine.AtomicInc(NpreemptNothing) END
+			END
+		ELSE
+			IF Stats THEN Machine.AtomicInc(NpreemptCritical) END (* not preemptable *)
+		END
+	END;
+	Machine.Release(Machine.Objects)
+END FieldIRQ;
+
+(* Process scheduled to handle an interrupt. *)
+PROCEDURE InterruptProcess(self: ProtectedObject);
+VAR h: InterruptList; t: Process; int: LONGINT;
+BEGIN
+	int := self(Interrupter).interruptNumber;
+	t := interrupt[int-MinIRQ].process;
+	LOOP
+		h := interrupt[int-MinIRQ].root;	(* concurrent updates allowed in InstallHandler and RemoveHandler *)
+		WHILE h # NIL DO h.handler (); h := h.next END;
+		Machine.Acquire(Machine.Objects);
+		ASSERT(running[Machine.ID ()] = t); (* strong check *)
+		t.mode := AwaitingEvent;
+		processingIRQ[int-MinIRQ] := FALSE;
+		Machine.EnableIRQ(int);
+		SwitchToNew
+	END
+END InterruptProcess;
+
+(** Install interrupt handler. *)
+PROCEDURE InstallHandler*(h: EventHandler; int: LONGINT);
+VAR t: Process; new: BOOLEAN; ih: Interrupter; n: InterruptList; i: LONGINT;
+BEGIN
+	ASSERT((int >= MinIRQ) & (int-MinIRQ < NumIRQ));	(* range check *)
+	IF interrupt[int-MinIRQ].process = NIL THEN	(* first handler for this irq *)
+		(* allocate process outside lock region, to avoid GC lock problems. *)
+		(* hack: use type parameter to pass int index & set obj to h, for System.ShowProcesses *)
+		NEW(ih); ih.interruptNumber := int;
+		NewProcess(InterruptProcess, {Resistant}, ih, t);
+		t.priority := High;  (* second-level interrupt handling processes have high priority, handlers may allocate memory, use exclusive locks and awaits *)
+		t.staticPriority := t.priority;
+		FOR i := 0 TO LEN(t.prioRequests) - 1 DO t.prioRequests[i] := 0 END;
+		INC(t.prioRequests[t.priority])
+	END;
+	NEW(n); n.handler := h;
+	Machine.Acquire(Machine.Objects);
+	IF interrupt[int-MinIRQ].process = NIL THEN	(* still first handler for this irq *)
+		t.id := nextProcessID; INC(nextProcessID);
+		t.mode := AwaitingEvent;
+		interrupt[int-MinIRQ].process := t;
+		new := TRUE
+	ELSE
+		new := FALSE
+	END;
+	n.next := interrupt[int-MinIRQ].root; (* can be concurrent with loop in InterruptProcess *)
+	interrupt[int-MinIRQ].root := n;
+	Machine.Release(Machine.Objects);
+	IF new THEN Machine.InstallHandler(FieldIRQ, int) END (* do outside lock region to avoid NEW/GC deadlock *)
+END InstallHandler;
+
+(** Remove interrupt handler. *)
+PROCEDURE RemoveHandler*(h: EventHandler; int: LONGINT);
+VAR p, c: InterruptList;
+BEGIN
+	ASSERT((int >= MinIRQ) & (int-MinIRQ < NumIRQ)); (* range check *)
+	Machine.Acquire(Machine.Objects);
+	p := NIL; c := interrupt[int-MinIRQ].root;
+	WHILE (c.handler # h) & (c # NIL) DO p := c; c := c.next END;
+	IF c.handler = h THEN (* handler found *)
+		IF p = NIL THEN
+			interrupt[int-MinIRQ].root := c.next;
+	(*
+			IF c.inext = NIL THEN (* this was the last handler *)
+				Machine.RemoveHandler(FieldIRQ, int)
+				(* to do: synchronize with FieldIRQ and InterruptProcess *)
+			END
+	*)
+		ELSE
+			p.next := c.next
+		END
+	ELSE
+		HALT(99); (* handler not found *)
+	END;
+	(* can not clear c.next field, because InterruptProcess may be traversing it. *)
+	Machine.Release(Machine.Objects)
+END RemoveHandler;
+
+(* local procedure *)
+PROCEDURE SetTimeoutAbsOrRel(t: Timer; h: EventHandler; ms: LONGINT; isAbsolute: BOOLEAN);
+VAR e: Timer; trigger: LONGINT;
+BEGIN
+	ASSERT(Machine.Second= 1000);	(* assume milliseconds for now *)
+	ASSERT((t # NIL) & (h # NIL));
+	IF ms < 1 THEN ms := 1 END;
+	Machine.Acquire(Machine.Objects);
+	IF isAbsolute THEN trigger := ms ELSE trigger := Machine.ticks + ms (* ignore overflow *) END;
+	IF t.next # NIL THEN (* cancel previous timeout *)
+		t.next.prev := t.prev; t.prev.next := t.next
+	END;
+	t.trigger := trigger; t.handler := h;
+	e := event.next;	(* performance: linear search! *)
+	WHILE (e # event) & (e.trigger - trigger <= 0) DO e := e.next END;
+	t.prev := e.prev; e.prev := t; t.next := e; t.prev.next := t;
+	Machine.Release(Machine.Objects)
+END SetTimeoutAbsOrRel;
+
+(** Set (or reset) an event handler object's timeout value. *)
+PROCEDURE SetTimeout*(t: Timer; h: EventHandler; ms: LONGINT);
+BEGIN
+	SetTimeoutAbsOrRel(t, h, ms, FALSE)
+END SetTimeout;
+
+(** Set (or reset) an event handler object's timeout value. Here ms is absolute *)
+PROCEDURE SetTimeoutAt*(t: Timer; h: EventHandler; ms: LONGINT);
+BEGIN
+	SetTimeoutAbsOrRel(t, h, ms, TRUE)
+END SetTimeoutAt;
+
+(** Cancel an event handler object's timeout, if any. It is possible that the timer has expired, but not yet been scheduled to run. *)
+PROCEDURE CancelTimeout*(t: Timer);
+BEGIN
+	Machine.Acquire(Machine.Objects);
+	ASSERT(t # event);
+	IF t.next # NIL THEN
+		t.next.prev := t.prev; t.prev.next := t.next;
+		t.next := NIL; t.prev := NIL
+	END;
+	Machine.Release(Machine.Objects)
+END CancelTimeout;
+
+(** Terminate the current process and switch to next process. *)
+PROCEDURE Terminate*; (* exported for Linker *)
+VAR id: LONGINT;
+BEGIN
+	IF Stats THEN Machine.AtomicInc(Nterminate) END;
+	Machine.Acquire(Machine.Objects);
+	id := Machine.ID ();
+	(*running[id].state.PC := CallerPC ();*) (* for tracing *)
+	running[id].mode := Terminated;	(* a process can also be "terminated" if the queue containing it is garbage collected *)
+	SwitchToNew;
+	HALT(2201)	(* process resurrected *)
+END Terminate;
+
+PROCEDURE Halt;
+BEGIN
+	HALT(halt); (* process halted *)
+END Halt;
+
+PROCEDURE HaltUnbreakable;
+BEGIN
+	HALT(haltUnbreakable); (* process halted *)
+END HaltUnbreakable;
+
+(* Set the return PC which is saved in the process and set it to -1 *)
+PROCEDURE HaltAltPC(haltCode: LONGINT);
+VAR bp: ADDRESS; p: Process;
+BEGIN
+	p := running[Machine.ID ()];
+	ASSERT(p.oldReturnPC # -1);
+	bp := Machine.CurrentBP ();
+	SYSTEM.PUT (bp + AddressSize, p.oldReturnPC);
+	CASE haltCode OF
+		|halt: HALT(halt);
+		|haltUnbreakable: HALT(haltUnbreakable);
+	END;
+END HaltAltPC;
+
+PROCEDURE HaltReturn;
+VAR bp: ADDRESS;
+BEGIN
+	bp := Machine.CurrentBP ();
+	SYSTEM.GET (bp, bp); (* Get the dynamic link *)
+	Machine.SetBP (bp); (* Undo the actual paf *)
+	HaltAltPC(halt);
+END HaltReturn;
+
+PROCEDURE HaltUnbreakableReturn;
+VAR bp: ADDRESS;
+BEGIN
+	bp := Machine.CurrentBP ();
+	SYSTEM.GET (bp, bp); (* Get the dynamic link *)
+	Machine.SetBP (bp); (* Undo the actual paf *)
+	HaltAltPC(haltUnbreakable);
+END HaltUnbreakableReturn;
+
+PROCEDURE TerminateThis*(t: Process; unbreakable: BOOLEAN);
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; pc, fp : ADDRESS;
+
+	(* terminates a process that is either in mode AwaitingLock or AwaitingCond *)
+	PROCEDURE TerminateAwaiting(t: Process);
+	VAR hdr {UNTRACED}: Heaps.ProtRecBlock;
+	BEGIN
+		SYSTEM.GET(SYSTEM.VAL(ADDRESS, t.waitingOn) + Heaps.HeapBlockOffset, hdr);
+		ASSERT(hdr IS Heaps.ProtRecBlock);
+		IF t.mode = AwaitingLock THEN
+			fp := t.state.BP; 		(* SwitchTo PAF *)
+			SYSTEM.GET (fp, fp);		(* SwitchToNew PAF *)
+			SYSTEM.GET (fp, fp);		(* Lock PAF*)
+			SYSTEM.GET (fp + AddressSize, pc);	(* Get the return address*)
+			IF ~Modules.IsExceptionHandled(pc, fp, FALSE) THEN
+				Remove(hdr.awaitingLock, t);
+				t.waitingOn := NIL; SYSTEM.GET (t.state.BP + AddressSize, t.oldReturnPC);
+				IF unbreakable THEN
+					SYSTEM.PUT (t.state.BP + AddressSize, SYSTEM.VAL (ADDRESS, trapReturn[1]))
+				ELSE
+					SYSTEM.PUT (t.state.BP + AddressSize, SYSTEM.VAL (ADDRESS, trapReturn[0]))
+				END;
+				Enter(t)
+			ELSE
+				Machine.Acquire (Machine.TraceOutput);
+				Trace.String(" Not allowed to kill "); Trace.Int(t.id, 1); Trace.Char(" "); Trace.Int(t.mode, 1); Trace.Ln;
+				Machine.Release (Machine.TraceOutput);
+			END
+		ELSIF t.mode = AwaitingCond THEN
+			SYSTEM.GET (t.state.BP, fp);
+			SYSTEM.GET (t.state.PC, pc);
+			IF ~Modules.IsExceptionHandled(pc, fp, TRUE) THEN
+				Remove(hdr.awaitingCond, t);
+				t.waitingOn := NIL; SYSTEM.GET (t.state.BP + AddressSize, t.oldReturnPC);
+				IF unbreakable THEN
+					SYSTEM.PUT (t.state.BP + AddressSize, SYSTEM.VAL (ADDRESS, trapReturn[1]))
+				ELSE
+					SYSTEM.PUT (t.state.BP + AddressSize, SYSTEM.VAL (ADDRESS, trapReturn[0]))
+				END;
+				Enter(t)
+			ELSE
+				Machine.Acquire (Machine.TraceOutput);
+				Trace.String(" Not allowed to kill "); Trace.Int(t.id, 1); Trace.Char(" "); Trace.Int(t.mode, 1); Trace.Ln;
+				Machine.Release (Machine.TraceOutput);
+			END
+		END
+	END TerminateAwaiting;
+
+BEGIN
+	IF PleaseHalt IN t.flags THEN
+		IF TraceVerbose THEN
+			Machine.Acquire (Machine.TraceOutput);
+			Trace.String("Process (ID="); Trace.Int(t.id, 0); Trace.StringLn (") is already halting!");
+			Machine.Release (Machine.TraceOutput);
+		END;
+		RETURN
+	ELSE
+		Machine.Acquire(Machine.Objects);
+		IF (t = running[Machine.ID ()]) THEN INCL(t.flags, SelfTermination); END;
+		IF TraceVerbose THEN
+			Machine.Acquire (Machine.TraceOutput);
+			Trace.String(" Kill "); Trace.Int(t.id, 1); Trace.Char(" "); Trace.Int(t.mode, 1); Trace.Ln;
+			Machine.Release (Machine.TraceOutput);
+		END;
+		CASE t.mode OF
+			|Running:
+				INCL(t.flags, PleaseHalt);
+				IF unbreakable THEN INCL(t.flags, Unbreakable) END
+			|Ready:
+				DEC(t.state.SP, AddressSize);	SYSTEM.PUT (t.state.SP, t.state.PC);
+				IF unbreakable THEN t.state.PC := SYSTEM.VAL (ADDRESS, trap[1])
+				ELSE t.state.PC := SYSTEM.VAL (ADDRESS, trap[0]) END
+			|AwaitingLock, AwaitingCond:
+				IF HandlePriorityInv THEN
+					TerminateAwaiting(t)
+				ELSE
+					SYSTEM.GET(SYSTEM.VAL(ADDRESS, t.waitingOn) + Heaps.HeapBlockOffset, hdr);
+					ASSERT(hdr IS Heaps.ProtRecBlock);
+					IF ~hdr.locked THEN
+						Machine.AcquireObject(hdr.locked);
+						TerminateAwaiting(t);
+						Machine.ReleaseObject(hdr.locked)
+					END
+				END
+			| AwaitingEvent, Unknown, Terminated:	(* skip *)
+		END;
+		Machine.Release(Machine.Objects)
+	END
+END TerminateThis;
+
+(* called by WMProcessInfo to obtain the current state of a running process *)
+PROCEDURE UpdateProcessState*( p: Process );	
+BEGIN
+	(*  update p.stat.{PC,BP,SP}  *)
+END UpdateProcessState;
+
+(* Finalize a process. *)
+PROCEDURE FinalizeProcess(t: ANY);
+BEGIN
+	Machine.DisposeStack(t(Process).stack)
+END FinalizeProcess;
+
+(* Allocate a new process associated with "obj". Must be outside lock region, because of potential GC. *)
+PROCEDURE NewProcess(body: Body; flags: SET; obj: ProtectedObject; VAR new: Process);
+VAR t: Process; sp: ADDRESS; id: LONGINT; fn: Heaps.FinalizerNode;
+BEGIN
+	NEW(t); NEW(fn); (* implicit call Heaps.NewRec *)
+	t.next := NIL; t.prev := NIL; t.rootedNext := NIL;
+	t.waitingOn := NIL; t.flags := flags;
+	t.obj := obj; t.mode := Unknown;
+	(* initialize the stack *)
+	Machine.NewStack(t.stack, t, sp);
+	IF VeryConservative THEN
+		Machine.Fill32(t.stack.adr, sp-t.stack.adr, LONGINT(0D0D0DEADH))
+	END;
+	SYSTEM.PUT (sp-1*AddressSize, obj); (* self parameter for body *)
+	SYSTEM.PUT (sp-2*AddressSize, terminate); (* return address for body *)
+	SYSTEM.PUT (sp-3*AddressSize, NIL); (* FP for body *)
+	(* the following two are not necessary because the compiler instruments the caller to cleanup parameters, not the callee! *)
+	(*SYSTEM.PUT (sp-3*AddressSize, NIL);*)	(* parameter for SwitchTo (ADR(running)) *)
+	(*SYSTEM.PUT (sp-4*AddressSize, NIL);*)	(* parameter for SwitchTo (new) *)
+	SYSTEM.PUT (sp-4*AddressSize, SYSTEM.VAL(ADDRESS, body) + ReturnStackDisplacement); (* return address for SwitchTo (body entry point) *)
+	SYSTEM.PUT (sp-5*AddressSize, sp-3*AddressSize);	(* end of dynamic link list (FP value at entry to body) *)
+	t.sseAdr := ADDRESSOF(t.sse) + ((-ADDRESSOF(t.sse)) MOD 16);
+	Machine.FPUSaveMin(t.sse);	(* inherit FPU state of caller *)
+	t.state.BP := sp - 5*AddressSize;
+	t.state.SP := t.state.BP;
+	t.state.PC := 0; (* indicating that process is not running yet *)
+	(* set up exception handling *)
+	IF Restart IN flags THEN	(* restart object body *)
+		t.restartPC := SYSTEM.VAL (ADDRESS, body) + ReturnStackDisplacement;
+		t.restartSP := sp-3*AddressSize	(* 1 parameter and return address of body *)
+	ELSE (* terminate process *)
+		t.restartPC := SYSTEM.VAL (ADDRESS, terminate) + ReturnStackDisplacement;
+		t.restartSP := sp-AddressSize
+	END;
+	fn.finalizer := FinalizeProcess;
+	Heaps.AddFinalizer(t, fn);
+	(* return *)
+	FOR id := 0 TO Machine.MaxCPU-1 DO t.cpuCycles[id] := 0 END;
+	new := t
+END NewProcess;
+
+(* Create the process associated with an active object (kernel call). *)
+PROCEDURE CreateProcess*(body: Body; priority: LONGINT; flags: SET; obj: ProtectedObject);
+VAR t: Process; type: ADDRESS; heapBlock {UNTRACED}: Heaps.HeapBlock; i: LONGINT;
+BEGIN
+	IF Stats THEN Machine.AtomicInc(Ncreate) END;
+
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, heapBlock);
+	ASSERT(heapBlock IS Heaps.ProtRecBlock); (* protected object *)
+
+	SYSTEM.GET (SYSTEM.VAL (ADDRESS, obj) + Heaps.TypeDescOffset, type); (* type tag *)
+	IF Restart IN flags THEN INCL(flags, Resistant) END; (* SAFE => Restart & Resistant *)
+	NewProcess(body, flags, obj, t);
+	Machine.Acquire(Machine.Objects);
+	t.id := nextProcessID; INC(nextProcessID);
+	IF priority = 0 THEN	(* no priority specified *)
+		t.priority := running[Machine.ID ()].priority (* inherit priority of creator *)
+	ELSIF priority > 0 THEN (* positive priority specified *)
+		t.priority := priority
+	ELSE (* negative priority specified (only for Idle process) *)
+		t.priority := MinPriority
+	END;
+	t.staticPriority := t.priority;
+	FOR i := 0 TO LEN(t.prioRequests) - 1 DO t.prioRequests[i] := 0 END;
+	INC(t.prioRequests[t.priority]);
+	CASE t.priority OF
+		MinPriority			: t.rootedNext := rootedProcesses[t.priority]; rootedProcesses[t.priority] := t
+	|	Low, Normal, High	: (* do nothing, processes with this priority are traced by GC automatically *)
+	|	GCPriority, Realtime	: t.rootedNext := rootedProcesses[t.priority]; rootedProcesses[t.priority] := t
+	END;
+	Enter(t);
+	Machine.Release(Machine.Objects)
+END CreateProcess;
+
+(* Lock a protected object (kernel call) *)
+(* There are two different procedures for locking a protected object in case of priority inversion handling enabled or disabled due to the different
+    locking strategy. *)
+PROCEDURE Lock*(obj: ProtectedObject; exclusive: BOOLEAN);
+BEGIN
+	IF HandlePriorityInv THEN
+		LockPriorityInv(obj, exclusive)
+	ELSE
+		LockNoPriorityInv(obj, exclusive)
+	END
+END Lock;
+
+(* Lock a protected object if priority inversion handling is disabled. Header locks, preemption and Machine.Objects locks are used. *)
+PROCEDURE LockNoPriorityInv(obj: ProtectedObject; exclusive: BOOLEAN);
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; r: Process; id: LONGINT;
+BEGIN (* {called from user level} *)
+	IF Stats THEN Machine.AtomicInc(Nlock) END;
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+	IF StrongChecks THEN
+		ASSERT(hdr IS Heaps.ProtRecBlock); (* protected object *)
+		ASSERT(exclusive)	(* shared not implemented yet *)
+	END;
+	id := Machine.AcquirePreemption ();
+	Machine.AcquireObject(hdr.locked);
+	IF hdr.count = 0 THEN (* not locked *)
+		hdr.count := -1; hdr.lockedBy := GetProcessPtr (); (* set exclusive lock *)
+		Machine.ReleaseObject(hdr.locked);
+		Machine.ReleasePreemption;
+	ELSE  (* locked *)
+		r := GetProcessPtr ();
+		IF hdr.lockedBy = r THEN
+			Machine.ReleaseObject(hdr.locked);
+			Machine.ReleasePreemption;
+			ASSERT(hdr.lockedBy # r, 2203);	(* nested locks not allowed *)
+		END;
+		ASSERT(r.waitingOn = NIL);
+		r.waitingOn := obj; r.mode := AwaitingLock;
+		Machine.Acquire(Machine.Objects);
+		Put(hdr.awaitingLock, r);
+		Machine.ReleaseObject(hdr.locked);
+		Machine.ReleasePreemption;
+		SwitchToNew
+	END
+END LockNoPriorityInv;
+
+(*
+(* propagation of priorities - lock Machine.Objects is taken.
+    This is a procedure that calls itself recursively if a higher priority is propagated along a chain of resources and processes where each resource
+    is locked by a process that itself waits on a resource. The procedure can be rewritten into a non-recursive procedure if needed..
+    Remark: parameters of type Heaps.HeapBlock or extensions of it are not passed as parameters for clarity and safety reasons .
+    Instead, a ProtectedObject pointer is passed as the first parameter.   *)
+PROCEDURE PropagatePrio(obj: ProtectedObject; prevMaxWaitingPrio, waitingPrio: LONGINT);
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; p: Process;
+BEGIN
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+	IF hdr.lockedBy # NIL THEN
+		p := hdr.lockedBy(Process);
+		DEC(p.prioRequests[prevMaxWaitingPrio]);
+		INC(p.prioRequests[waitingPrio]);
+		IF (p.waitingOn # NIL) & (waitingPrio > p.priority) THEN
+			obj := p.waitingOn;
+			SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+			prevMaxWaitingPrio := MaxPrio(hdr.waitingPriorities);
+			DEC(hdr.waitingPriorities[p.priority]);
+			INC(hdr.waitingPriorities[waitingPrio]);
+			IF waitingPrio > prevMaxWaitingPrio THEN PropagatePrio(obj, prevMaxWaitingPrio, waitingPrio) END
+		END;
+		IF waitingPrio > p.priority THEN
+			IF p.mode = Ready THEN Remove(ready.q[p.priority], p) END; (* remove p from the lower priority queue ... *)
+			p.priority := waitingPrio;
+			IF p.mode = Ready THEN Enter(p) END;  (* ... and add it to the higher priority queue *)
+		END
+	END;
+END PropagatePrio;
+*)
+
+(* propagation of priorities - lock Machine.Objects is taken.
+    This procedure is the iterative version of the above commented out recursive procedure.
+    Remark: hdr is an actually UNTRACED parameter. The GC, however, can handle this, see procedure Heaps.Mark, there is a check whether the
+    pointer to the header part is valid. In case of hdr, the pointer ot the header part is NIL. *)
+PROCEDURE PropagatePrio(hdr: Heaps.ProtRecBlock; prevMaxWaitingPrio, waitingPrio: LONGINT);
+VAR propagateFurther: BOOLEAN; p: Process; obj: ProtectedObject;
+BEGIN
+	propagateFurther := TRUE;
+	WHILE propagateFurther & (waitingPrio > prevMaxWaitingPrio) DO
+		IF hdr.lockedBy # NIL THEN
+			p := hdr.lockedBy(Process);
+			DEC(p.prioRequests[prevMaxWaitingPrio]);
+			INC(p.prioRequests[waitingPrio]);
+			IF (p.waitingOn # NIL) & (waitingPrio > p.priority) THEN
+				obj := p.waitingOn;
+				SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+				prevMaxWaitingPrio := MaxPrio(hdr.waitingPriorities);
+				DEC(hdr.waitingPriorities[p.priority]);
+				INC(hdr.waitingPriorities[waitingPrio]);
+			ELSE (* p is not waiting for a resource or waitingPrio is less or equal to p's priority - priority propagation finishes *)
+				propagateFurther := FALSE
+			END;
+			IF waitingPrio > p.priority THEN (* independently of whether p is waiting on a resource or not the priority of p is changed if it is lower than waitingPrio *)
+				IF p.mode = Ready THEN Remove(ready.q[p.priority], p) END; (* remove p from the lower priority queue ... *)
+				p.priority := waitingPrio;
+				IF p.mode = Ready THEN Enter(p) END;  (* ... and add it to the higher priority queue *)
+			END
+		ELSE (* current resource is not locked - priority propagation finishes *)
+			propagateFurther := FALSE
+		END
+	END
+END PropagatePrio;
+
+(* TO DO: adapt priority inversion algorithm such that priority of a process is not raised higher than High, it must not become Realtime, otherwise
+    GC may be corrupted *)
+(* Lock a protected object if priority inversion handling is enabled. Machine.Objects lock is used. *)
+PROCEDURE LockPriorityInv(obj: ProtectedObject; exclusive: BOOLEAN);
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; r: Process;
+	maxWaitingPrio, prevMaxWaitingPrio: LONGINT;
+BEGIN (* {called from user level} *)
+	IF Stats THEN Machine.AtomicInc(Nlock) END;
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+	IF StrongChecks THEN
+		ASSERT(hdr IS Heaps.ProtRecBlock); (* protected object *)
+		ASSERT(exclusive)	(* shared not implemented yet *)
+	END;
+	Machine.Acquire(Machine.Objects);
+	r := (*GetProcessPtr0();*) running[Machine.ID()];
+	ASSERT(r # NIL);
+	IF hdr.count = 0 THEN (* not locked *)
+		hdr.count := -1; hdr.lockedBy := r; (* set exclusive lock *)
+		maxWaitingPrio := MaxPrio(hdr.waitingPriorities);
+		INC(r.prioRequests[maxWaitingPrio]);
+		r.priority := MaxPrio(r.prioRequests);
+		Machine.Release(Machine.Objects);
+	ELSE	(* locked (to do: on multiprocessors, perhaps spin here for a while, if lockedBy.mode = running) *)
+		IF hdr.lockedBy = NIL THEN
+			Machine.Release(Machine.Objects);
+			ASSERT(hdr.lockedBy # NIL)
+		END;
+		IF hdr.lockedBy = r THEN
+			Machine.Release(Machine.Objects);
+			ASSERT(hdr.lockedBy # r, 2203);	(* nested locks not allowed *)
+		END;
+		IF r.waitingOn # NIL THEN
+			Machine.Acquire(Machine.TraceOutput);
+			Trace.String("Objects: LockPriorityInv - hdr.count # NIL, but r.waitingOn # NIL");
+			Machine.Release(Machine.TraceOutput)
+		END;
+		ASSERT(r.waitingOn = NIL);
+		r.waitingOn := obj; r.mode := AwaitingLock;
+		prevMaxWaitingPrio := MaxPrio(hdr.waitingPriorities);
+		INC(hdr.waitingPriorities[r.priority]);
+		IF r.priority > prevMaxWaitingPrio THEN PropagatePrio(hdr, prevMaxWaitingPrio, r.priority) END;
+		Put(hdr.awaitingLock, r);
+		SwitchToNew
+	END
+END LockPriorityInv;
+
+(* Find the first true condition from the queue and remove it. Assume the object is currently locked. *)
+PROCEDURE FindCondition(VAR q: ProcessQueue): Process;
+VAR first, cand: Process;
+BEGIN
+	IF Stats THEN Machine.AtomicInc(Ncondition) END;
+	Get(q, first);
+	IF first.condition(first.condFP) THEN
+		IF Stats THEN Machine.AtomicInc(Ncondition1True) END;
+		RETURN first
+	END;
+	Put(q, first);
+	WHILE q.head # first DO
+		IF Stats THEN Machine.AtomicInc(Ncondition2) END;
+		Get(q, cand);
+		IF cand.condition(cand.condFP) THEN
+			IF Stats THEN Machine.AtomicInc(Ncondition2True) END;
+			RETURN cand
+		END;
+		Put(q, cand)
+	END;
+	RETURN NIL
+END FindCondition;
+
+(* Find highest priority in array of priority counts *)
+PROCEDURE MaxPrio(CONST priorityCounts: ARRAY OF LONGINT): LONGINT;
+VAR i: LONGINT;
+BEGIN
+	i := LEN(priorityCounts) - 1;
+	WHILE (i >= 0) & (priorityCounts[i] = 0) DO DEC(i) END;
+	IF priorityCounts[i] = 0 THEN
+		Machine.Acquire(Machine.TraceOutput);
+		Trace.StringLn("Objects: MaxPrio - SEVERE ERROR: priorityCounts contains all zeros");
+		Machine.Release(Machine.TraceOutput);
+	END;
+	RETURN i
+END MaxPrio;
+
+(* Unlock a protected object (kernel call). *)
+(* There are two different procedures for locking a protected object in case of priority inverison handling enabled or disabled due to the different
+    locking strategy. *)
+PROCEDURE Unlock*(obj: ProtectedObject; dummy: BOOLEAN);
+BEGIN
+	IF HandlePriorityInv THEN
+		UnlockPriorityInv(obj)
+	ELSE
+		UnlockNoPriorityInv(obj)
+	END
+END Unlock;
+
+(* transfer the lock from a resource to another process.
+    Remark: hdr is an actually UNTRACED parameter. The GC, however, can handle this, see procedure Heaps.Mark, there is a check whether the
+    pointer to the header part is valid. In case of hdr, the pointer ot the header part is NIL. *)
+PROCEDURE TransferLock(hdr: Heaps.ProtRecBlock; p: Process);
+VAR maxWaitingPrio: LONGINT;
+BEGIN
+	ASSERT(p # NIL);
+	p.waitingOn := NIL; hdr.lockedBy := p;
+	IF HandlePriorityInv THEN
+		DEC(hdr.waitingPriorities[p.priority]);
+		maxWaitingPrio := MaxPrio(hdr.waitingPriorities);
+		INC(p.prioRequests[maxWaitingPrio]);
+		p.priority := MaxPrio(p.prioRequests)
+	END
+END TransferLock;
+
+(* Unlock a protected object if priority inversion handling is disabled. Header locks, preemption and Machine.Objects locks are used. *)
+PROCEDURE UnlockNoPriorityInv(obj: ProtectedObject);
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; t, c, r: Process; id: LONGINT;
+BEGIN
+	IF Stats THEN Machine.AtomicInc(Nunlock) END;
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+	IF StrongChecks THEN
+		ASSERT(hdr IS Heaps.ProtRecBlock) (* protected object *)
+	END;
+	ASSERT(hdr.count = -1);	(* exclusive locked *)
+	IF hdr.awaitingCond.head # NIL THEN (* evaluate the waiting conditions *)
+			(* we are holding the lock, so the queue can not change (to do: except in TerminateThis) *)
+		c := FindCondition(hdr.awaitingCond) (* interrupts should be on during this call *)
+	ELSE
+		c := NIL
+	END;
+	id := Machine.AcquirePreemption ();
+	Machine.AcquireObject(hdr.locked);
+	r := running[Machine.ID ()];
+	ASSERT(r # NIL);
+	IF hdr.lockedBy # r THEN
+		Machine.ReleaseObject(hdr.locked);
+		Machine.ReleasePreemption;
+		ASSERT(hdr.lockedBy = r)
+	END;
+	IF c = NIL THEN (* no true condition found, check the lock queue *)
+		Get(hdr.awaitingLock, t);
+		IF t # NIL THEN
+			IF StrongChecks THEN
+				ASSERT((t.mode = AwaitingLock) & (t.waitingOn = obj))
+			END;
+			TransferLock(hdr, t)
+		ELSE
+			hdr.lockedBy := NIL; hdr.count := 0
+		END
+	ELSE (* true condition found, transfer the lock *)
+		TransferLock(hdr, c);
+		t := NIL
+	END;
+
+	Machine.ReleaseObject(hdr.locked);
+	IF (c # NIL) OR (t # NIL) THEN
+		Machine.Acquire(Machine.Objects);
+		IF c # NIL THEN Enter(c) END;
+		IF t # NIL THEN Enter(t) END;
+		Machine.Release(Machine.Objects);
+	END;
+	Machine.ReleasePreemption;
+END UnlockNoPriorityInv;
+
+(* Unlock a protected object in case priority inversion handling is enabled. Machine.Objects lock is used. *)
+PROCEDURE UnlockPriorityInv(obj: ProtectedObject);
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; t, c, r: Process; maxWaitingPrio: LONGINT;
+BEGIN
+	IF Stats THEN Machine.AtomicInc(Nunlock) END;
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+	IF StrongChecks THEN
+		ASSERT(hdr IS Heaps.ProtRecBlock) (* protected object *)
+	END;
+	ASSERT(hdr.count = -1);	(* exclusive locked *)
+	IF hdr.awaitingCond.head # NIL THEN (* evaluate the waiting conditions *)
+			(* we are holding the lock, so the queue can not change (to do: except in TerminateThis) *)
+		c := FindCondition(hdr.awaitingCond) (* interrupts should be on during this call *)
+	ELSE
+		c := NIL
+	END;
+	Machine.Acquire(Machine.Objects);
+	r := running[Machine.ID ()];
+	ASSERT(r # NIL);
+	IF hdr.lockedBy # r THEN
+		Machine.Release(Machine.Objects);
+		ASSERT(hdr.lockedBy = r)
+	END;
+	maxWaitingPrio := MaxPrio(hdr.waitingPriorities);
+	DEC(r.prioRequests[maxWaitingPrio]);
+	r.priority := MaxPrio(r.prioRequests);
+	IF c = NIL THEN (* no true condition found, check the lock queue *)
+		t := NIL;
+		GetMaxPrio(hdr.awaitingLock, t);
+		IF t = NIL THEN
+			hdr.lockedBy := NIL; hdr.count := 0
+		ELSE
+			IF StrongChecks THEN ASSERT((t.mode = AwaitingLock) & (t.waitingOn = obj)) END;
+			TransferLock(hdr, t)
+		END
+	ELSE (* true condition found, transfer the lock *)
+		TransferLock(hdr, c);
+		t := NIL
+	END;
+
+	IF (c # NIL) OR (t # NIL) THEN
+		IF c # NIL THEN Enter(c) END;
+		IF t # NIL THEN Enter(t) END;
+	END;
+	Machine.Release(Machine.Objects);
+END UnlockPriorityInv;
+
+(* Await a condition (kernel call). *)
+(* There are two different procedures for locking a protected object in case of priority inverison handling enabled or disabled due to the different
+    locking strategies, i.e. there are no header locks in case of priority inversion handling. *)
+PROCEDURE Await*(cond: Condition; slink: ADDRESS; obj: ProtectedObject; flags: SET);
+BEGIN
+	IF HandlePriorityInv THEN
+		AwaitPriorityInv(cond, slink, obj, flags)
+	ELSE
+		AwaitNoPriorityInv(cond, slink, obj, flags)
+	END
+END Await;
+
+(* Await a condition if priority inversion handling is disabled. Header locks, preemption and Machine.Objects locks are used. *)
+PROCEDURE AwaitNoPriorityInv(cond: Condition; slink: ADDRESS; obj: ProtectedObject; flags: SET);
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; r, c, t: Process; id: LONGINT;
+BEGIN
+	IF Stats THEN Machine.AtomicInc(Nawait) END;
+	IF 1 IN flags THEN (* compiler did not generate IF *)
+		IF Stats THEN Machine.AtomicInc(NawaitNoIF) END;
+		IF cond(slink) THEN
+			IF Stats THEN Machine.AtomicInc(NawaitTrue) END;
+			RETURN (* condition already true *)
+		END
+	END;
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+	IF StrongChecks THEN
+		ASSERT(hdr IS Heaps.ProtRecBlock) (* protected object *)
+	END;
+	id := Machine.AcquirePreemption ();
+	Machine.AcquireObject(hdr.locked);	(* must acquire object lock before other locks *)
+	r := running[id];
+	ASSERT(r # NIL);
+	IF hdr.lockedBy = r THEN	(* current process holds exclusive lock *)
+		IF StrongChecks THEN ASSERT(hdr.count = -1) END; (* exclusive locked *)
+		IF hdr.awaitingCond.head # NIL THEN	(* evaluate the waiting conditions *)
+			(* we are holding the lock, so the queue can not change (to do: except in TerminateThis) *)
+			c := FindCondition(hdr.awaitingCond)	(* interrupts should be on during this call *)
+		ELSE
+			c := NIL
+		END;
+
+		IF c = NIL THEN
+			Get(hdr.awaitingLock, t);
+			IF t = NIL THEN	(* none waiting - remove lock *)
+				hdr.count := 0; hdr.lockedBy := NIL;
+			ELSE	(* transfer lock to first waiting process *)
+				IF StrongChecks THEN ASSERT(t.mode = AwaitingLock) END;
+				TransferLock(hdr, t)
+			END;
+		ELSE
+			TransferLock(hdr, c);
+			t := NIL
+		END;
+	ELSE (* no lock, or some other process may hold the lock, but that's the user's indaba (may be monotonic condition) *)
+		Machine.ReleaseObject(hdr.locked);
+		Machine.ReleasePreemption;
+		HALT(2204)	(* await must be exclusive region *)
+	END;
+	Machine.Acquire(Machine.Objects); (* Put and SwitchTo must be protected *)
+	IF c # NIL THEN Enter(c)  END;
+	IF t # NIL THEN Enter(t) END;
+	IF StrongChecks THEN ASSERT(r.waitingOn = NIL) END;
+	r.condition := cond; r.condFP := slink;
+	r.waitingOn := obj; r.mode := AwaitingCond;
+	Put(hdr.awaitingCond, r);
+	Machine.ReleaseObject(hdr.locked);
+	Machine.ReleasePreemption;
+	(* reschedule *)
+	SwitchToNew;
+	IF StrongChecks THEN
+		ASSERT(cond(slink));
+		ASSERT(hdr.lockedBy = r) (* lock held again *)
+	END
+END AwaitNoPriorityInv;
+
+(* Await a condition in case priority inversion handling is enabled. Machine.Objects lock is used. *)
+PROCEDURE AwaitPriorityInv(cond: Condition; slink: ADDRESS; obj: ProtectedObject; flags: SET);
+VAR hdr {UNTRACED}: Heaps.ProtRecBlock; r, c, t: Process; id, maxWaitingPrio, prevMaxWaitingPrio: LONGINT;
+BEGIN
+	IF Stats THEN Machine.AtomicInc(Nawait) END;
+	IF 1 IN flags THEN (* compiler did not generate IF *)
+		IF Stats THEN Machine.AtomicInc(NawaitNoIF) END;
+		IF cond(slink) THEN
+			IF Stats THEN Machine.AtomicInc(NawaitTrue) END;
+			RETURN (* condition already true *)
+		END
+	END;
+	SYSTEM.GET(SYSTEM.VAL(ADDRESS, obj) + Heaps.HeapBlockOffset, hdr);
+	IF StrongChecks THEN
+		ASSERT(hdr IS Heaps.ProtRecBlock) (* protected object *)
+	END;
+	Machine.Acquire(Machine.Objects);
+	id := Machine.ID();
+	r := running[id];
+	ASSERT(r # NIL);
+	IF hdr.lockedBy = r THEN	(* current process holds exclusive lock *)
+		IF StrongChecks THEN ASSERT(hdr.count = -1) END; (* exclusive locked *)
+		maxWaitingPrio := MaxPrio(hdr.waitingPriorities);
+		DEC(r.prioRequests[maxWaitingPrio]);
+		r.priority := MaxPrio(r.prioRequests);
+		IF hdr.awaitingCond.head # NIL THEN	(* evaluate the waiting conditions *)
+			(* we are holding the lock, so the queue can not change (to do: except in TerminateThis) *)
+			c := FindCondition(hdr.awaitingCond)	(* interrupts should be on during this call *)
+		ELSE
+			c := NIL
+		END;
+
+		IF c = NIL THEN
+			t := NIL;
+			GetMaxPrio(hdr.awaitingLock, t);
+			IF t = NIL THEN	(* none waiting - remove lock *)
+				hdr.count := 0; hdr.lockedBy := NIL;
+			ELSE	(* transfer lock to first waiting process *)
+				IF StrongChecks THEN ASSERT(t.mode = AwaitingLock) END;
+				TransferLock(hdr, t);
+			END;
+		ELSE  (* true condition found, transfer the lock *)
+			TransferLock(hdr, c);
+			t := NIL;
+		END;
+	ELSE (* no lock, or some other process may hold the lock, but that's the user's indaba (may be monotonic condition) *)
+		Machine.Release(Machine.Objects);
+		HALT(2204)	(* await must be exclusive region *)
+	END;
+	IF c # NIL THEN Enter(c) END;
+	IF t # NIL THEN Enter(t) END;
+	IF StrongChecks THEN ASSERT(r.waitingOn = NIL) END;
+	r.condition := cond; r.condFP := slink;
+	r.waitingOn := obj; r.mode := AwaitingCond;
+	IF hdr.lockedBy # NIL THEN
+		prevMaxWaitingPrio := MaxPrio(hdr.waitingPriorities);
+		INC(hdr.waitingPriorities[r.priority]);
+		IF r.priority > prevMaxWaitingPrio THEN PropagatePrio(hdr, prevMaxWaitingPrio, r.priority) END;
+	ELSE (* it may happen that hdr is not locked - in that case no priority propagation takes place *)
+		INC(hdr.waitingPriorities[r.priority])
+	END;
+	Put(hdr.awaitingCond, r);
+	(* reschedule *)
+	SwitchToNew;
+	IF StrongChecks THEN
+		ASSERT(cond(slink));
+		ASSERT(hdr.lockedBy = r) (* lock held again *)
+	END
+END AwaitPriorityInv;
+
+(** Update the state snapshot of the current process for GC. (for Processors) *)
+PROCEDURE UpdateState;
+VAR t: Process;
+BEGIN (* interrupts off *)
+	Machine.Acquire(Machine.Objects);
+	t := running[Machine.ID ()];
+	ASSERT(t # NIL);
+	IF t # NIL THEN
+ 		t.state.PC := Machine.CurrentPC(); (* ug: required information for GC with meta data for stack inspection *)
+ 		t.state.SP := Machine.CurrentSP(); (* ug: not necessarily needed for GC *)
+ 		t.state.BP := Machine.CurrentBP(); (* ug: necessary information for GC with meta data for stack inspection *)
+	END;
+	Machine.Release(Machine.Objects)
+END UpdateState;
+
+(** Start executing user processes. Every processor calls this during initialization. *)
+PROCEDURE Start*;
+VAR id, ignored: LONGINT; idle: Idle; new: Process;
+BEGIN (* running at kernel level (not preemptable) *)
+	ignored := Machine.AcquirePreemption();
+	id := Machine.ID (); (* preemption not enabled yet, because we are running at kernel level *)
+	NEW(idle); (* create process with MinPriority *)
+	Machine.Acquire(Machine.Objects);
+	Get(ready.q[MinPriority], new); (* can not use Select here, as it might return a preempted process *)
+	ASSERT(~(Preempted IN new.flags)); (* will at least get the Idle process just created *)
+	Machine.Release(Machine.Objects);
+	running[id] := new; (* schedule new process *)
+	perfTsc[id] := Machine.GetTimer();
+	new.mode := Running; new.procID := id;
+	Machine.FPURestoreMin(new.sse);
+	Machine.ReleasePreemption;
+	Machine.JumpToUserLevel(new.state.BP);
+	HALT(100); (* does never return here *)
+END Start;
+
+(* Initialize module. *)
+PROCEDURE Init; (* can not use NEW *)
+VAR
+	i: LONGINT;
+BEGIN
+	ProcessorHLT := NIL;
+	maxReady := High; (* scan all queues at start *)
+	lowestAllowedPriority := Low; (* normal case, will be set to GCPriority if GC is running *)
+	gcBarrier := {};
+	FOR i := 0 TO Machine.MaxCPU - 1 DO running[i] := NIL END;
+	FOR i := 0 TO NumPriorities - 1 DO rootedProcesses[i] := NIL END;
+	FOR i := 0 TO NumIRQ-1 DO processingIRQ[i] := FALSE END;
+	nextProcessID := 0; Machine.ticks := 0;
+	traceProcess := NIL;
+	terminate := Terminate;
+	trap[0] := Halt;
+	trap[1] := HaltUnbreakable;
+	trapReturn[0] := HaltReturn;
+	trapReturn[1] := HaltUnbreakableReturn;
+END Init;
+
+PROCEDURE InitEventHandling;
+VAR i: LONGINT; clock: Clock; (* realtimeClock: RealtimeClock; *)
+BEGIN
+	FOR i := 0 TO NumIRQ-1 DO
+		interrupt[i].root := NIL; interrupt[i].process := NIL
+	END;
+	(* create normal event list *)
+	NEW(event); event.next := event; event.prev := event;
+	event.trigger := Machine.ticks + MAX(LONGINT) DIV 2;
+	(* create normal timer processes *)
+	timer := NIL; NEW(clock);
+END InitEventHandling;
+
+PROCEDURE InitGCHandling;
+VAR finalizerCaller: FinalizerCaller;
+BEGIN
+	gcProcess := NIL; NEW(gcActivity);
+	finalizerProcess := NIL; NEW(finalizerCaller);
+END InitGCHandling;
+
+PROCEDURE InitStats;
+BEGIN
+	Nlock := 0; Nunlock := 0; Nawait := 0; NawaitNoIF := 0; NawaitTrue := 0;
+	Ncreate := 0; Nterminate := 0; Ncondition := 0; Ncondition1True := 0;
+	Ncondition2 := 0; Ncondition2True := 0;
+	Ntimeslice := 0; NtimesliceTaken := 0; NtimesliceNothing := 0;
+	NtimesliceIdle := 0; NtimesliceKernel := 0; NtimesliceV86 := 0; NtimesliceCritical := 0;
+	Npreempt := 0; NpreemptTaken := 0; NpreemptNothing := 0;
+	NpreemptKernel := 0; NpreemptV86 := 0; NpreemptCritical := 0;
+	Nenter := 0;
+END InitStats;
+
+PROCEDURE GCStatusFactory(): Heaps.GCStatus;
+VAR gcStatusExt : GCStatusExt;
+BEGIN
+	ASSERT(Heaps.gcStatus = NIL);
+	NEW(gcStatusExt);
+	RETURN gcStatusExt
+END GCStatusFactory;
+
+(** Return current user stack *)
+PROCEDURE GetCurrentStack(VAR stack: Machine.Stack);
+BEGIN
+	stack := running[Machine.ID()].stack;
+END GetCurrentStack;
+
+PROCEDURE InitPrioRequest;
+VAR
+	i: LONGINT;
+BEGIN
+	FOR i := 0 TO LEN(init.prioRequests) - 1 DO init.prioRequests[i] := 0 END;
+END InitPrioRequest;
+
+VAR
+	(* for compatibility and later extension *)
+	TraceProcessHook*: PROCEDURE (prcoess: Process; pc, bp: ADDRESS; stacklow, stackhigh: ADDRESS);
+
+BEGIN
+	IF Stats THEN InitStats; END;
+	Init;
+	(* initialize memory management *)
+	Machine.UpdateState; (* for gc *)
+	Machine.getStack := GetCurrentStack;
+	Heaps.CollectGarbage(Modules.root); (* still in single-processor mode *)
+	(* now NEW can be used *)
+	NEW(ready); (* create the ready queues *)
+	Machine.InitInterrupts;
+	(*Machine.Start; initialize interrupts *)
+	InitEventHandling;
+	InitGCHandling;
+	Heaps.gcStatus := GCStatusFactory();
+	(* create a process for rest of init code, which runs at user level *)
+	entry := Machine.CurrentBP ();
+	SYSTEM.GET (entry+AddressSize, entry);	(* return address into linker-generated call table *)
+	NEW(initObject);
+	NewProcess(SYSTEM.VAL (Body, entry-ReturnStackDisplacement), {Resistant}, initObject, init); (* create init process *)
+	init.priority := High;
+	init.staticPriority := init.priority;
+	(* initialize prioRequests for init process *)
+	InitPrioRequest;
+	INC(init.prioRequests[init.priority]);
+	Machine.Acquire(Machine.Objects);
+	init.id := -1; Enter(init); init := NIL;
+	Machine.Release(Machine.Objects);
+	Start (* start it *)
+	(* linker call table will end with a call to Terminate. So after executing all module bodies,
+	the init process will terminate and other processes created during init will continue running. *)
+END Objects.
+
+(*
+24.03.1998	pjm	Started
+06.05.1998	pjm	CreateProcess init process, page fault handler
+06.08.1998	pjm	Moved exception interrupt handling here for current process
+17.08.1998	pjm	FindRoots method
+02.10.1998	pjm	Idle process
+06.11.1998	pjm	snapshot
+25.03.1999	pjm	Scope removed
+28.05.1999	pjm	EventHandler object
+01.06.1999	pjm	Fixed InterruptProcess lock error
+16.06.1999	pjm	Flat IRQ priority model to avoid GC deadlock
+23.06.1999	pjm	Flat IRQ priority experiment failed, rather do STI in FieldIRQ to avoid GC deadlock
+29.06.1999	pjm	Timeout in EventHandler object
+13.01.2000	pjm	Overed (Interrupt Objects, Event Handlers, Process ID, Process state, Process mode, Process stack, Await)
+17.10.2000	pjm	Priorities
+22.10.2003	mib	SSE2 extension
+24.10.2003	phk	Priority inversion / cycle counters
+19.06.2007	ug	Garbage Collector using meta data for stack inspection
+*)
+
+(*
+Location	Stack
+Lock	Current process
+ SwitchTo.A	Current process
+ SwitchTo.B
+*)

source/ARM.Raster.Mod

@@ -0,0 +1,3905 @@
+MODULE Raster; (** non-portable *)	(* eos, TF  **)
+(** AUTHOR "eos"; PURPOSE "Raster operations"; *)
+
+	(**
+		Raster image bitmaps and basic image processing
+	**)
+
+
+	(*
+		19.9.1999 - spawned from GfxMaps
+		25.10.1999 - fixed bytes-per-row calculation in Create (need to make arguments LONG)
+		17.11.1999 - eliminated F8 format, replaced by D8 (implemented with module Colors)
+		19.11.1999 - fixed missing alpha component in computed palettes
+		16.05.2000 - module Raster as Oberon-independent part of Images
+		19.06.2000 - replaced DisplayFormat and InitDisplay by DisplayFormat()
+		25.02.2006 - raster operations with SSE2 added (student project by Myrto Zehnder)
+		28.02.2008 - added capabilities for 16-bit palette & grayscale images often used in scientific,medical imaging and professional photography (Patrick Hunziker)
+
+		To do:
+		- store in compressed format
+		- add capabilities for RGB and multiplane images with >8 bits per color as used in scientific imaging and professional photography
+	*)
+
+	IMPORT
+		SYSTEM, KernelLog, Machine, Streams, CLUTs, Displays;
+
+
+	(*
+		Interfacing with display hardware and foreign framebuffers may suffer a performance hit if their
+		bit or byte ordering can't be made compatible with that of a built-in format and has to be converted
+		manually. Predefined format A1 has the leftmost pixel in the least significant bit of a byte, and
+		all hi/true color formats have their blue component at the lowest address, followed by green,
+		red, and possibly alpha (which conforms to the specification of the transfer formats in Displays).
+
+		As SYSTEM is imported, the module is not portable and has always to be compiled to native code.
+		However, it should usually suffice to recompile the module on other platforms without changing
+		any code.
+
+		Assumptions:
+			* CHR(l) = CHR(l MOD 100H) for all l: LONGINT
+			* SIZEOF(LONGINT)=4
+	*)
+
+
+	CONST
+		b* = 0; g* = 1; r* = 2; a* = 3;	(** index of blue, green, red, and alpha components in a Pixel **)
+
+		(** format codes **)
+		custom* = 0; a1* = 1; a8* = 2; d8* = 3; p8* = 4; bgr555* = 5; bgr565* = 6; bgr466* = 7; bgr888* = 8; bgra8888* = 9; p16* =10;
+
+		(** components **)
+		color* = 0; alpha* = 1; index* = 2;
+
+		(** compositing operations (srcCopy = replace, srcOverDst = paint **)
+		clear* = 0; srcCopy* = 1; dstCopy* = 2; srcOverDst* = 3; dstOverSrc* = 4; srcInDst* = 5; dstInSrc* = 6;
+		srcWithoutDst* = 7; dstWithoutSrc* = 8; srcAtopDst* = 9; dstAtopSrc* = 10; srcXorDst* = 11; InvDst*=12;
+		InvOverDst*=13;
+
+		MAXCOL = 10000H; (*current implementation limitation for number of color indexes *)
+
+	TYPE
+		(** general pixels with red, green, blue, and alpha information in range 0..255; alpha is pre-multiplied into RGB **)
+		Pixel* = ARRAY 4 OF CHAR;
+
+		(** palette structure for indexed formats **)
+		Palette* = OBJECT
+			VAR
+			col*: POINTER TO ARRAY OF Pixel;	(** color table **)
+			used*: LONGINT;	(** number of valid entries in color table **)
+			clut: CLUTs.CLUT;	(* reverse color lookup structure *)
+			PROCEDURE &New*; BEGIN NEW(col,256); used:=256 END New; (*initialized to 256 colors; for backwards compatibility*)
+			PROCEDURE Init*(used:LONGINT); BEGIN SELF.used:=used; NEW(col,used) END Init; (*initialize to size # 256*) (*bugfix PH090122*)
+		END Palette;
+
+		(** image format **)
+		Format0* = RECORD
+			code*: SHORTINT;	(** format code for quick format checks **)
+			bpp*: SHORTINT;	(** number of bits per pixel **)
+			align*: SHORTINT;	(** bytes per row must be multiple of this **)
+			components*: SET;	(** components that are stored in a pixel **)
+			pal*: Palette;	(** optional palette for indexed formats **)
+		END;
+
+		PackProc* = PROCEDURE (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+
+		Format* = RECORD (Format0)
+			pack*: PackProc;	(** store supported pixel components at given address **)
+			unpack*: PackProc;	(** load supported pixel components from given address **)
+		END;
+
+		(** raster image **)
+		Image* = OBJECT (* POINTER TO ImageDesc;
+		ImageDesc* = RECORD *)
+		VAR
+			width*, height*: LONGINT;	(** image dimensions **)
+			fmt*: Format;	(** pixel format **)
+			bpr*: LONGINT;	(** number of bytes per row (may be negative) **)
+			adr*: ADDRESS;	(** address of lower left pixel **)
+			mem*: POINTER TO ARRAY OF CHAR;	(** block where pixels are stored; mem#NIL implies adr=ADR(mem[0]) **)
+		END Image;
+
+		(** transfer mode **)
+		Mode0* = RECORD
+			src*, dst*: Format;	(** source and destination format **)
+			op*: LONGINT;	(** compositing operation **)
+			col*: Pixel;	(** substitute color used when transfering from pure alpha formats to colored ones **)
+			buf: Pixel;	(* constant area for special-case moving *)
+			map: POINTER TO ARRAY OF INTEGER;	(* color map for transfer between indexed formats *)
+		END;
+
+		TransferProc* = PROCEDURE (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+
+		Mode* = RECORD (Mode0)
+			transfer*: TransferProc;	(** procedure transfering pixels from source to destination **)
+		END;
+
+		PictureTransferParameters* = POINTER TO RECORD
+			img* : Image;
+			name* : ARRAY 128 OF CHAR;
+			done* : BOOLEAN
+		END;
+
+	VAR
+		A1*, A8*, D8*, BGR555*, BGR565*, BGR466*, BGR888*, BGRA8888*: Format;	(** predefined formats **)
+		PixelFormat*: Format;	(** special formats **)
+
+		Clamp*: ARRAY 500H OF CHAR;	(** Clamp[200H+i] = CHR(min(max(i, 0), 0FFH)) **)
+
+		Zero: Pixel;	(* pixel with all components cleared *)
+		Bit: ARRAY 100H, 8 OF BOOLEAN;	(* Bit[b, i] means bit i in byte b is set *)
+		Set, Clr: ARRAY 100H, 8 OF CHAR;	(* Set/Clr[b, i] is byte b with bit i set/cleared *)
+
+(*		d8display: Displays.Display;	(* only one system-wide D8 display supported *)
+		plugin: Plugins.Plugin; *)
+
+		MMXenabled*,SSE2enabled*  : BOOLEAN;
+
+	(**--- Color/Pixel conversions ---**)
+
+	(** set pixel to opaque RGB value **)
+	PROCEDURE SetRGB* (VAR pix: Pixel; red, green, blue: LONGINT);
+	CODE
+		LDR R0, [FP, #pix] ; ADDRESSOF(pix[0])
+		LDRB R1, [FP, #blue]
+		LDRB R2, [FP, #green]
+		LDRB R3, [FP, #red]
+		
+		ANDS R5, R0, #3 ; R0 MOD 4
+		BNE unaligned_store
+		
+		; aligned store
+		ORR R1, R1, R2, LSL #8
+		ORR R1, R1, R3, LSL #16
+		ORR R1, R1, #0FF000000H
+		STR R1, [R0, #0]
+		B exit
+
+	unaligned_store:
+		MOV R4, #0FFH
+		STRB R1, [R0, #0]
+		STRB R2, [R0, #1]
+		STRB R3, [R0, #2]
+		STRB R4, [R0, #3]
+	exit:
+	(*BEGIN
+		pix[b] := CHR(blue); pix[g] := CHR(green); pix[r] := CHR(red); pix[a] := 0FFX*)
+	END SetRGB;
+
+	(** set pixel to partly transparent RGB value **)
+	PROCEDURE SetRGBA* (VAR pix: Pixel; red, green, blue, alpha: LONGINT);
+	CODE
+		LDR R0, [FP, #pix] ; ADDRESSOF(pix[0])
+		LDRB R1, [FP, #blue]
+		LDRB R2, [FP, #green]
+		LDRB R3, [FP, #red]
+		LDRB R4, [FP, #alpha]
+		
+		CMP R4, #255
+		BEQ store ; if alpha =255 store colors as they are
+		
+		; apply alpha to the colors
+		MUL R1, R1, R4 ; blue * alpha 
+		MUL R2, R2, R4 ; green * alpha
+		MUL R3, R3, R4 ; red * alpha
+		LSR R1, R1, #8 ; (blue * alpha) DIV 256 
+		LSR R2, R2, #8 ; (green * alpha) DIV 256 
+		LSR R3, R3, #8 ; (red * alpha) DIV 256 
+	
+	store:
+		ANDS R5, R0, #3 ; R0 MOD 4
+		BNE unaligned_store
+		
+		; aligned store
+		ORR R1, R1, R2, LSL #8
+		ORR R1, R1, R3, LSL #16
+		ORR R1, R1, R4, LSL #24
+		STR R1, [R0, #0]
+		B exit
+
+	unaligned_store:
+		STRB R1, [R0, #0]
+		STRB R2, [R0, #1]
+		STRB R3, [R0, #2]
+		STRB R4, [R0, #3]
+	exit:
+	(*BEGIN
+		pix[b] := CHR(blue * alpha DIV 255);
+		pix[g] := CHR(green * alpha DIV 255);
+		pix[r] := CHR(red * alpha DIV 255);
+		pix[a] := CHR(alpha)*)
+	END SetRGBA;
+
+	(** retrieve RGB and alpha values from pixel **)
+	PROCEDURE GetRGBA* (pix: Pixel; VAR red, green, blue, alpha: LONGINT);
+	BEGIN
+		alpha := ORD(pix[a]);
+		IF alpha = 0 THEN	(* color has disappeared *)
+			red := 255; green := 255; blue := 255
+		ELSE
+			red := ORD(pix[r]); green := ORD(pix[g]); blue := ORD(pix[b]);
+			IF alpha # 255 THEN	(* un-multiply alpha *)
+				red := 255 * red DIV alpha; IF red > 255 THEN red := 255 END;
+				green := 255 * green DIV alpha; IF green > 255 THEN green := 255 END;
+				blue := 255 * blue DIV alpha; IF blue > 255 THEN blue := 255 END
+			END
+		END
+	END GetRGBA;
+
+
+	(**--- Palettes ---**)
+
+	(** return index of color in palette which approximates the requested color reasonably well **)
+	PROCEDURE PaletteIndex* (pal: Palette; red, green, blue: LONGINT): LONGINT;
+	BEGIN
+		IF pal.used>256 THEN
+			RETURN CLUTs.Match(pal.clut, ASH(red, 16) + ASH(green, 8) +  blue) *pal.used DIV 256 (*PH090122*)
+		ELSE RETURN CLUTs.Match(pal.clut, ASH(red, 16) + ASH(green, 8) +  blue)
+		END;
+	END PaletteIndex;
+
+	(** compute internal palette structures whenever palette colors have been modified **)
+	PROCEDURE InitPalette* (pal: Palette; used, bits: LONGINT);
+		VAR n, red, green, blue, alpha: LONGINT;
+	BEGIN
+		n := 0;
+		IF used>pal.used THEN NEW(pal.col,used);END;
+		pal.used:=used;
+		WHILE n < used DO
+			GetRGBA(pal.col[n], red, green, blue, alpha);
+			CLUTs.Set(pal.clut, (*n*) n*255 DIV used, ASH(red, 16) + ASH(green, 8) + blue);
+			INC(n)
+		END;
+		CLUTs.Init(pal.clut, MIN (used,256), bits);
+	END InitPalette;
+
+	(** (re)compute grayscale palette, typically used for pictures with >256 gray scale values**)
+	PROCEDURE InitGrayPalette* (pal: Palette; used, bits: LONGINT);
+		VAR n, gray: LONGINT;
+	BEGIN
+		n := 0;
+		IF used>pal.used THEN NEW(pal.col,used); END;
+		pal.used := used;
+		WHILE n < used DO
+			gray:= n*255 DIV used;
+			SetRGBA(pal.col[n],gray,gray,gray,255); (*PH 090122*)
+			CLUTs.Set(pal.clut, gray, ASH(gray, 16) + ASH(gray, 8) + gray);
+			INC(n)
+		END;
+		CLUTs.Init(pal.clut, MIN(used,256), bits);
+	END InitGrayPalette;
+
+	(** copy palette contents **)
+	PROCEDURE CopyPalette* (from, to: Palette);
+		VAR n: LONGINT;
+	BEGIN
+		n := 0;
+		IF to.used<from.used THEN NEW(to.col,from.used) END;
+		to.used:=from.used;
+		WHILE n < from.used DO
+			to.col[n] := from.col[n]; INC(n)
+		END;
+		CLUTs.Copy(from.clut, to.clut)
+	END CopyPalette;
+
+	(** compute and initialize a pseudo-optimal palette for an image (in hi-color or true-color format) **)
+	PROCEDURE ComputePalette* (img: Image; pal: Palette; reservedcols, maxcols, bits: LONGINT);
+		(*
+			uses octree-quantization
+		*)
+
+		TYPE
+			Node = POINTER TO RECORD	(* octree node *)
+				dsc: ARRAY 8 OF Node;	(* descendants *)
+				link: Node;	(* next reducible node on same level *)
+				leaf: BOOLEAN;
+				weight: LONGINT;	(* accumulated number of pixels represented by this node *)
+				r, g, b: LONGINT;	(* accumulated color values *)
+			END;
+
+		VAR
+			sent, root: Node; reducible: ARRAY 8 OF Node; colors, maxDepth, y, x, bb, used: LONGINT; adr, aa: ADDRESS; pix: Pixel;
+
+		PROCEDURE insert (VAR node: Node; depth: LONGINT; pix: Pixel);
+			VAR idx, bit: LONGINT;
+		BEGIN
+			IF node = NIL THEN
+				NEW(node);
+				IF depth = maxDepth THEN
+					node.leaf := TRUE;
+					INC(colors)
+				ELSE
+					node.leaf := FALSE;
+					node.link := reducible[depth]; reducible[depth] := node
+				END
+			END;
+			INC(node.weight);	(* node represents more pixels *)
+			IF node.leaf THEN
+				INC(node.r, LONG(ORD(pix[r])));
+				INC(node.g, LONG(ORD(pix[g])));
+				INC(node.b, LONG(ORD(pix[b])))
+			ELSE
+				idx := 0; bit := 7-depth;
+				IF ODD(ASH(ORD(pix[r]), -bit)) THEN INC(idx, 4) END;
+				IF ODD(ASH(ORD(pix[g]), -bit)) THEN INC(idx, 2) END;
+				IF ODD(ASH(ORD(pix[b]), -bit)) THEN INC(idx) END;
+				insert(node.dsc[idx], depth+1, pix)
+			END
+		END insert;
+
+		PROCEDURE reduce;
+			VAR d, min, n, i: LONGINT; node, prev, dsc: Node;
+		BEGIN
+			d := maxDepth-1;
+			WHILE reducible[d] = NIL DO
+				DEC(d); DEC(maxDepth)
+			END;
+			sent.link := reducible[d];
+			node := sent; min := MAX(LONGINT);
+			WHILE node.link # NIL DO
+				IF node.link.weight < min THEN
+					min := node.link.weight; prev := node
+				END;
+				node := node.link
+			END;
+			node := prev.link; prev.link := node.link;
+			reducible[d] := sent.link;
+			n := 1;	(* number of colors is initially one for the node itself *)
+			FOR i := 0 TO 7 DO
+				dsc := node.dsc[i];
+				IF dsc # NIL THEN
+					DEC(n);	(* reducing one color *)
+					INC(node.r, dsc.r); INC(node.g, dsc.g); INC(node.b, dsc.b);
+					node.dsc[i] := NIL
+				END
+			END;
+			node.leaf := TRUE;
+			INC(colors, n)
+		END reduce;
+
+		PROCEDURE traverse (node: Node);
+			VAR i: LONGINT;
+		BEGIN
+			IF node # NIL THEN
+				IF node.leaf THEN
+					pal.col[used, r] := CHR(node.r DIV node.weight);
+					pal.col[used, g] := CHR(node.g DIV node.weight);
+					pal.col[used, b] := CHR(node.b DIV node.weight);
+					pal.col[used, a] := 0FFX;
+					INC(used)
+				ELSE
+					FOR i := 0 TO 7 DO
+						traverse(node.dsc[i])
+					END
+				END
+			END
+		END traverse;
+
+	BEGIN	(* ComputePalette *)
+		ASSERT(reservedcols + maxcols <= MAXCOL, 100);
+		NEW(sent);
+		root := NIL; colors := 0; maxDepth := 8;
+		y := 0; adr := img.adr;
+		WHILE y < img.height DO
+			IF img.fmt.bpp < 8 THEN
+				x := 0; aa := adr; bb := 0;
+				WHILE x < img.width DO
+					img.fmt.unpack(img.fmt, aa, bb, pix);
+					insert(root, 0, pix);
+					WHILE colors > maxcols DO
+						reduce()
+					END;
+					INC(x); bb := bb + img.fmt.bpp; INC(aa, bb DIV 8); bb := bb MOD 8
+				END
+			ELSE
+				x := 0; aa := adr; bb := img.fmt.bpp DIV 8;
+				WHILE x < img.width DO
+					img.fmt.unpack(img.fmt, aa, 0, pix);
+					insert(root, 0, pix);
+					WHILE colors > maxcols DO
+						reduce()
+					END;
+					INC(x); INC(aa, bb)
+				END
+			END;
+			INC(y); INC(adr, img.bpr)
+		END;
+		used := reservedcols;
+		traverse(root);
+		InitPalette(pal, used, bits)
+	END ComputePalette;
+
+
+	(**--- Formats ---**)
+
+	(* A1 - one bit alpha, MSB leftmost *)
+	PROCEDURE PackA1 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR b: CHAR;
+	BEGIN
+		SYSTEM.GET(adr, b);
+		IF pix[a] >= 80X THEN SYSTEM.PUT(adr, Set[ORD(b), bit])
+		ELSE SYSTEM.PUT(adr, Clr[ORD(b), bit])
+		END
+	END PackA1;
+
+	PROCEDURE UnpackA1 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR b: CHAR;
+	BEGIN
+		SYSTEM.GET(adr, b);
+		IF Bit[ORD(b), bit] THEN pix[a] := 0FFX
+		ELSE pix := Zero
+		END
+	END UnpackA1;
+
+	(* A8 - 8 bit alpha *)
+	PROCEDURE PackA8 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	BEGIN
+		SYSTEM.PUT(adr, pix[a])
+	END PackA8;
+
+	PROCEDURE UnpackA8 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR alpha: LONGINT;
+	BEGIN
+		SYSTEM.GET(adr, pix[a]);
+		IF pix[a] = 0X THEN
+			pix := Zero
+		ELSIF pix[a] # 0FFX THEN
+			alpha := ORD(pix[a]);
+			pix[r] := CHR(ORD(pix[r]) * alpha DIV 255);
+			pix[g] := CHR(ORD(pix[g]) * alpha DIV 255);
+			pix[b] := CHR(ORD(pix[b]) * alpha DIV 255)
+		END
+	END UnpackA8;
+
+	(* P8 - 8 bit indexed format with custom palette *)
+	PROCEDURE PackP8 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	BEGIN
+		IF pix[a] # 0X THEN	(* model alpha as brightness *)
+			SYSTEM.PUT(adr, CHR(CLUTs.Match(fmt.pal.clut, ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))))
+		END
+	END PackP8;
+
+	PROCEDURE UnpackP8 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR idx: CHAR;
+	BEGIN
+		SYSTEM.GET(adr, idx); pix := fmt.pal.col[ORD(idx)]
+	END UnpackP8;
+
+	(* D8 - 8 bit indexed format with display palette *)
+	PROCEDURE PackD8 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	BEGIN
+		IF pix[a] # 0X THEN	(* model alpha as brightness *)
+			SYSTEM.PUT(adr, CHR(ColorToIndex(ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))))
+		END
+	END PackD8;
+
+	PROCEDURE UnpackD8 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR idx: CHAR; col: LONGINT;
+	BEGIN
+		SYSTEM.GET(adr, idx); col := IndexToColor(ORD(idx));
+		pix[b] := CHR(col MOD 100H); pix[g] := CHR(ASH(col, -8) MOD 100H); pix[r] := CHR(ASH(col, -16) MOD 100H)
+	END UnpackD8;
+
+		(* P16 - 16 bit indexed format with custom palette *)
+	PROCEDURE PackP16 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	VAR val:LONGINT;
+	BEGIN
+		IF pix[a] # 0X THEN	(* model alpha as brightness *)
+			(*SYSTEM.PUT16(adr, PaletteIndex(fmt.pal, ORD(pix[r]), ORD(pix[g]), ORD(pix[b])))*)
+			val:=CLUTs.Match(fmt.pal.clut, ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]),16));
+			IF fmt.pal.used>256 THEN val:=val*fmt.pal.used DIV 256 END;
+			SYSTEM.PUT16(adr, SHORT(val))
+		END
+	END PackP16;
+
+	PROCEDURE UnpackP16 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	BEGIN
+		pix := fmt.pal.col[SYSTEM.GET16(adr) MOD 1000H] 	(*unsigned 16 bit entity*)
+	END UnpackP16;
+
+
+	(* BGR555 - 16 hi-color with 5 bit blue, 5 bit green and 5 bit red in ascending order *)
+	PROCEDURE PackBGR555 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR int: LONGINT;
+	BEGIN
+		IF pix[a] # 0X THEN	(* model alpha as brightness *)
+			int := ASH(ORD(pix[b]), -3) + ASH(ASH(ORD(pix[g]), -3), 5) + ASH(ASH(ORD(pix[r]), -3), 10);
+			SYSTEM.PUT(adr, CHR(int)); SYSTEM.PUT(adr+1, CHR(ASH(int, -8)))
+		END
+	END PackBGR555;
+
+	PROCEDURE UnpackBGR555 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR lo, hi: CHAR; int: LONGINT;
+	BEGIN
+		SYSTEM.GET(adr, lo); SYSTEM.GET(adr+1, hi); int := ASH(ORD(hi), 8) + ORD(lo);
+		pix[b] := CHR(ASH(int MOD 20H, 3) + 4);
+		pix[g] := CHR(ASH(ASH(int, -5) MOD 20H, 3) + 4);
+		pix[r] := CHR(ASH(ASH(int, -10) MOD 20H, 3) + 4);
+		pix[a] := 0FFX
+	END UnpackBGR555;
+
+	(* BGR565 - 16 hi-color with 5 bit blue, 6 bit green and 5 bit red in ascending order *)
+	PROCEDURE PackBGR565 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR int: LONGINT;
+	BEGIN
+		IF pix[a] # 0X THEN	(* model alpha as brightness *)
+			int := ASH(ORD(pix[b]), -3) + ASH(ASH(ORD(pix[g]), -2), 5) + ASH(ASH(ORD(pix[r]), -3), 11);
+			SYSTEM.PUT(adr, CHR(int)); SYSTEM.PUT(adr+1, CHR(ASH(int, -8)))
+		END
+	END PackBGR565;
+
+	PROCEDURE UnpackBGR565 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR lo, hi: CHAR; int: LONGINT;
+	BEGIN
+		SYSTEM.GET(adr, lo); SYSTEM.GET(adr+1, hi); int := ASH(ORD(hi), 8) + ORD(lo);
+		pix[b] := CHR(ASH(int MOD 20H, 3) + 4);
+		pix[g] := CHR(ASH(ASH(int, -5) MOD 40H, 2) + 2);
+		pix[r] := CHR(ASH(ASH(int, -11) MOD 20H, 3) + 4);
+		pix[a] := 0FFX
+	END UnpackBGR565;
+
+	(* BGR466 - 16 hi-color with 4 bit blue, 6 bit green and 6 bit red in ascending order *)
+	PROCEDURE PackBGR466 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR int: LONGINT;
+	BEGIN
+		IF pix[a] # 0X THEN	(* model alpha as brightness *)
+			int := ASH(ORD(pix[b]), -4) + ASH(ASH(ORD(pix[g]), -2), 4) + ASH(ASH(ORD(pix[r]), -2), 10);
+			SYSTEM.PUT(adr, CHR(int)); SYSTEM.PUT(adr+1, CHR(ASH(int, -8)))
+		END
+	END PackBGR466;
+
+	PROCEDURE UnpackBGR466 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+		VAR lo, hi: CHAR; int: LONGINT;
+	BEGIN
+		SYSTEM.GET(adr, lo); SYSTEM.GET(adr+1, hi); int := ASH(ORD(hi), 8) + ORD(lo);
+		pix[b] := CHR(ASH(int MOD 10H, 4) + 8);
+		pix[g] := CHR(ASH(ASH(int, -4) MOD 40H, 2) + 2);
+		pix[r] := CHR(ASH(ASH(int, -10) MOD 40H, 2) + 2);
+		pix[a] := 0FFX
+	END UnpackBGR466;
+
+	(* BGR888 - 24 bit true color with blue in lower, green in middle, and red in upper byte *)
+	PROCEDURE PackBGR888 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	BEGIN
+		IF pix[a] # 0X THEN	(* model alpha as brightness *)
+			SYSTEM.MOVE(ADDRESSOF(pix[0]), adr, 3)
+		END
+	END PackBGR888;
+
+	PROCEDURE UnpackBGR888 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	BEGIN
+		SYSTEM.MOVE(adr, ADDRESSOF(pix[0]), 3); pix[a] := 0FFX
+	END UnpackBGR888;
+
+	(* BGRA8888 - 32 bit true color with blue in lowest, green in lower middle, red in upper middle, and alpha in top byte *)
+	PROCEDURE PackBGRA8888 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	BEGIN
+		SYSTEM.MOVE(ADDRESSOF(pix[0]), adr, 4)
+	END PackBGRA8888;
+
+	PROCEDURE UnpackBGRA8888 (VAR fmt: Format0; adr: ADDRESS; bit: LONGINT; VAR pix: Pixel);
+	BEGIN
+		SYSTEM.MOVE(adr, ADDRESSOF(pix[0]), 4)
+	END UnpackBGRA8888;
+
+	(** return image format for given Displays transfer format **)
+	PROCEDURE DisplayFormat* (format: LONGINT): Format;
+	BEGIN
+		CASE format OF
+		| Displays.index8: RETURN D8
+		| Displays.color565: RETURN BGR565
+		| Displays.color888: RETURN BGR888
+		| Displays.color8888: RETURN BGRA8888
+		END
+	END DisplayFormat;
+
+	(** initialize format **)
+	PROCEDURE InitFormat* (VAR fmt: Format; code, bpp, align: SHORTINT; comps: SET; pal: Palette; pack, unpack: PackProc);
+	BEGIN
+		fmt.code := code; fmt.bpp := bpp; fmt.align := align; fmt.components := comps; fmt.pal := pal;
+		fmt.pack := pack; fmt.unpack := unpack
+	END InitFormat;
+
+	(** initialize index formats with custom palette **)
+	PROCEDURE InitPaletteFormat* (VAR fmt: Format; pal: Palette);
+	BEGIN
+		fmt.components := {index}; fmt.pal := pal;
+		IF pal.used<=256 THEN
+			fmt.align := 1;
+			fmt.code := p8; fmt.bpp := 8;
+			fmt.pack := PackP8; fmt.unpack := UnpackP8
+		ELSIF pal.used <= 10000H THEN
+			fmt.align := 2;
+			fmt.code := p16; fmt.bpp := 16;
+			fmt.pack := PackP16; fmt.unpack := UnpackP16
+		ELSE HALT(199)
+		END
+	END InitPaletteFormat;
+
+	(** return if two formats are the same **)
+	PROCEDURE Same* (VAR fmt0, fmt1: Format): BOOLEAN;
+	BEGIN
+		RETURN
+			(fmt0.pack = fmt1.pack) & (fmt0.unpack = fmt1.unpack) &
+			(~(index IN fmt0.components) OR (fmt0.pal = fmt1.pal))	(* doesn't work if palette has been re-initialized *)
+	END Same;
+
+
+	(**--- Images ---**)
+
+	(** initialize custom image **)
+	PROCEDURE Init* (img: Image; width, height: LONGINT; VAR fmt: Format; bpr: LONGINT; adr: ADDRESS);
+	BEGIN
+		ASSERT((width > 0) & (height > 0), 100);
+		img.width := width; img.height := height; img.fmt := fmt; img.bpr := bpr; img.adr := adr
+	END Init;
+
+	(** initialize custom image on byte buffer **)
+	PROCEDURE InitBuf* (img: Image; width, height: LONGINT; VAR fmt: Format; bpr, offset: LONGINT; VAR buf: ARRAY OF CHAR);
+	BEGIN
+		ASSERT((0 <= offset) & (offset + height * ABS(bpr) <= LEN(buf)), 100);
+		IF bpr >= 0 THEN Init(img, width, height, fmt, bpr, ADDRESSOF(buf[0]))
+		ELSE Init(img, width, height, fmt, bpr, ADDRESSOF(buf[offset]) + LEN(buf) - bpr)
+		END
+	END InitBuf;
+
+	(** initialize image on rectangular area within existing image (lower left corner must fall on byte boundary) **)
+	PROCEDURE InitRect* (img, base: Image; x, y, w, h: LONGINT);
+	BEGIN
+		ASSERT((0 <= x) & (x + w <= base.width) & (0 <= y) & (y + h <= base.height), 100);
+		ASSERT(x * base.fmt.bpp MOD 8 = 0, 101);
+		Init(img, w, h, base.fmt, base.bpr, base.adr + y * base.bpr + x * base.fmt.bpp DIV 8)
+	END InitRect;
+
+	(** create image in requested format (allocating or reusing necessary memory) **)
+	PROCEDURE Create* (img: Image; width, height: LONGINT; fmt: Format);
+	VAR size: LONGINT; a0, a1: ADDRESS;
+	BEGIN
+		ASSERT((width > 0) & (height > 0), 100);
+		img.width := width; img.height := height;
+		img.fmt := fmt;
+		img.bpr := (width * fmt.bpp + 7) DIV 8;
+		IF fmt.align > 1 THEN
+			img.bpr := (img.bpr + fmt.align - 1) DIV fmt.align * fmt.align
+		END;
+		size := height * img.bpr; INC(size, (-size) MOD 4);
+		IF (img.mem = NIL) OR (size < LEN(img.mem^) DIV 2) OR (LEN(img.mem^) < size) THEN
+			NEW(img.mem, size)
+		ELSE
+			a0 := ADDRESSOF(img.mem[0]); a1 := a0 + size;
+			WHILE a0 # a1 DO
+				SYSTEM.PUT32(a0,0); INC(a0, SIZEOF(LONGINT))
+			END
+		END;
+		img.adr := ADDRESSOF(img.mem[0])
+	END Create;
+
+	PROCEDURE CreateWithBuffer*(img: Image; width, height: LONGINT; fmt: Format; mem: POINTER TO ARRAY OF CHAR; VAR adr: ADDRESS);
+	VAR size: LONGINT; a0, a1: ADDRESS;
+	BEGIN
+		ASSERT((width > 0) & (height > 0), 100);
+		img.width := width; img.height := height;
+		img.fmt := fmt;
+		img.bpr := (width * fmt.bpp + 7) DIV 8;
+		IF fmt.align > 1 THEN
+			img.bpr := (img.bpr + fmt.align - 1) DIV fmt.align * fmt.align
+		END;
+		size := height * img.bpr; INC(size, (-size) MOD 4);
+		a0 := adr; a1 := adr + size;
+		ASSERT(ADDRESSOF(mem[0]) <= a0);
+		ASSERT(a1 <= ADDRESSOF(mem[LEN(mem)-1]));
+		WHILE a0 # a1 DO
+			SYSTEM.PUT32(a0,0); INC(a0, SIZEOF(LONGINT))
+		END;
+		img.adr := adr;
+		img.mem := mem;
+		adr := a1;
+	END CreateWithBuffer;
+
+	(**--- Transfer Modes ---**)
+
+	(** initialize transfer mode **)
+	PROCEDURE InitMode* (VAR mode: Mode; op: SHORTINT);
+	BEGIN
+		mode.op := op;
+		IF (mode.src.pal # NIL) & ((mode.map = NIL) OR (LEN(mode.map^) # mode.src.pal.used)) THEN
+			NEW(mode.map, mode.src.pal.used)
+		END;
+		SetRGB(mode.col, 255, 255, 255);
+		(*
+		mode.col := SYSTEM.VAL(Pixel, -1);
+		*)
+		mode.src.pack := NIL; mode.dst.pack := NIL	(* force re-evaluation of transfer procedure *)
+	END InitMode;
+
+	(** initialize transfer mode with color components for pure alpha sources **)
+	PROCEDURE InitModeColor* (VAR mode: Mode; op: SHORTINT; red, green, blue: LONGINT);
+	BEGIN
+		mode.op := op;
+		IF (mode.src.pal # NIL) & ((mode.map = NIL) OR (LEN(mode.map^) # mode.src.pal.used)) THEN
+			NEW(mode.map, mode.src.pal.used)
+		END;
+		SetRGB(mode.col, red, green, blue);
+		mode.src.pack := NIL; mode.dst.pack := NIL
+	END InitModeColor;
+
+	(** set new source color for transfer mode **)
+	PROCEDURE SetModeColor* (VAR mode: Mode; red, green, blue: LONGINT);
+	BEGIN
+		SetRGB(mode.col, red, green, blue);
+		IF (mode.src.pal # NIL) & ((mode.map = NIL) OR (LEN(mode.map^) # mode.src.pal.used)) THEN
+			NEW(mode.map, mode.src.pal.used)
+		END;
+		mode.src.pack := NIL; mode.dst.pack := NIL
+	END SetModeColor;
+
+	(** blend source pixel into destination pixel according to compositing operation **)
+	PROCEDURE Blend* (op: LONGINT; VAR src, dst: Pixel);
+		VAR fs, fd, i: LONGINT;
+	BEGIN
+		CASE op OF
+		| clear: fs := 0; fd := 0
+		| srcCopy: fs := 255; fd := 0
+		| dstCopy: fs := 0; fd := 255
+		| srcOverDst: fs := 255; fd := 255-ORD(src[a])
+		| dstOverSrc: fs := 255-ORD(dst[a]); fd := 255
+		| srcInDst: fs := ORD(dst[a]); fd := 0
+		| dstInSrc: fs := 0; fd := ORD(src[a])
+		| srcWithoutDst: fs := 255-ORD(dst[a]); fd := 0
+		| dstWithoutSrc: fs := 0; fd := 255-ORD(src[a])
+		| srcAtopDst: fs := ORD(dst[a]); fd := 255-ORD(src[a])
+		| dstAtopSrc: fs := 255-ORD(dst[a]); fd := ORD(src[a])
+		| srcXorDst: fs := 255-ORD(dst[a]); fd := 255-ORD(src[a])
+		END;
+
+		IF fs + fd = 0 THEN
+			FOR i := 0 TO 3 DO dst[i] := 0X END
+		ELSIF fs = 0 THEN
+			IF fd # 255 THEN
+				FOR i := 0 TO 3 DO
+					dst[i] := Clamp[200H + fd * ORD(dst[i]) DIV 255]
+				END
+			END
+		ELSIF fd = 0 THEN
+			IF fs = 255 THEN
+				dst := src
+			ELSE
+				FOR i := 0 TO 3 DO
+					dst[i] := Clamp[200H + fs * ORD(src[i]) DIV 255]
+				END
+			END
+		ELSE
+			dst[0] := Clamp[200H + (fs * ORD(src[0]) + fd * ORD(dst[0])) DIV 255];
+			dst[1] := Clamp[200H + (fs * ORD(src[1]) + fd * ORD(dst[1])) DIV 255];
+			dst[2] := Clamp[200H + (fs * ORD(src[2]) + fd * ORD(dst[2])) DIV 255];
+			dst[3] := Clamp[200H + (fs * ORD(src[3]) + fd * ORD(dst[3])) DIV 255]
+		END
+	END Blend;
+
+
+	(*--- General Transfer ---*)
+
+	PROCEDURE AnyBlendAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR spix, dpix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			spix := mode.col; dpix := mode.col;
+			mode.src.unpack(mode.src, sadr, sbit, spix);
+			mode.dst.unpack(mode.dst, dadr, dbit, dpix);
+			Blend(mode.op, spix, dpix);
+			mode.dst.pack(mode.dst, dadr, dbit, dpix);
+			sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8;
+			dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8;
+			DEC(len)
+		END
+	END AnyBlendAny;
+
+
+	(* --- invert --- *)
+
+	PROCEDURE InvAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		WHILE len > 0 DO
+			mode.dst.unpack(mode.dst, dadr, dbit, mode.buf);
+			mode.buf[r]:=CHR(255-ORD(mode.buf[r]));
+			mode.buf[g]:=CHR(255-ORD(mode.buf[g]));
+			mode.buf[b]:=CHR(255-ORD(mode.buf[b]));
+			mode.dst.pack(mode.dst, dadr, dbit, mode.buf);
+			sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8;
+			dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8;
+			DEC(len)
+		END
+	END InvAny;
+
+	(* --- alpha invert --- *)
+
+	PROCEDURE InvOverAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	VAR pix:Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			IF pix[a]>=80X THEN
+				mode.dst.unpack(mode.dst, dadr, dbit, mode.buf);
+				mode.buf[r]:=CHR(255-ORD(mode.buf[r]));
+				mode.buf[g]:=CHR(255-ORD(mode.buf[g]));
+				mode.buf[b]:=CHR(255-ORD(mode.buf[b]));
+				mode.dst.pack(mode.dst, dadr, dbit, mode.buf)
+			END;
+			sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8;
+			dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8;
+			DEC(len)
+		END
+	END InvOverAny;
+
+	(*--- clear ---*)
+
+	PROCEDURE ClearAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR inc: LONGINT;
+	BEGIN
+		IF mode.dst.bpp MOD 8 = 0 THEN
+			inc := mode.dst.bpp DIV 8;
+			WHILE len > 0 DO
+				mode.dst.pack(mode.dst, dadr, 0, Zero);
+				INC(dadr, inc); DEC(len)
+			END
+		ELSE
+			WHILE len > 0 DO
+				mode.dst.pack(mode.dst, dadr, dbit, Zero);
+				dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8;
+				DEC(len)
+			END
+		END
+	END ClearAny;
+
+	PROCEDURE Clear1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR out: CHAR;
+	BEGIN
+		IF (dbit > 0) OR (len < 8) THEN
+			SYSTEM.GET(dadr, out);
+			WHILE (dbit < 8) & (len > 0) DO
+				out := Clr[ORD(out), dbit];
+				INC(dbit); DEC(len)
+			END;
+			SYSTEM.PUT(dadr, out)
+		END;
+		WHILE len >= 32 DO
+			SYSTEM.PUT(dadr, LONG(LONG(0))); INC(dadr, 4); DEC(len, 32)
+		END;
+		WHILE len >= 8 DO
+			SYSTEM.PUT(dadr, 0X); INC(dadr); DEC(len, 8)
+		END;
+		IF len > 0 THEN
+			SYSTEM.GET(dadr, out); dbit := 0;
+			REPEAT
+				out := Clr[ORD(out), dbit];
+				INC(dbit); DEC(len)
+			UNTIL len = 0;
+			SYSTEM.PUT(dadr, out)
+		END
+	END Clear1;
+	
+	(*
+	PROCEDURE ClearBytes (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		len := len * mode.dst.bpp DIV 8;
+		WHILE len >= 4 DO
+			SYSTEM.PUT(dadr, LONG(LONG(0))); INC(dadr, 4); DEC(len)
+		END;
+		WHILE len > 0 DO
+			SYSTEM.PUT(dadr, 0X); INC(dadr); DEC(len)
+		END
+	END ClearBytes;
+	*)
+
+	PROCEDURE ClearBytes(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE
+		LDR R0, [FP, #dadr]
+		LDR R1, [FP, #len]
+		MOV R2, #0
+	loop:
+		CMP R1, #0
+		BLE end ; exit if len <= 0
+		
+		STRB R2, [R0, #0]
+		ADD R0, R0, #1 ; INC(dadr);
+		SUB R1, R1, #1 ; DEC(len);
+		B loop
+	end:
+	END ClearBytes;
+	
+	(* len is nr of DWords*)
+	PROCEDURE Clear32(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE
+		LDR R0, [FP, #dadr]
+		LDR R1, [FP, #len]
+		MOV R2, #0
+	loop:
+		CMP R1, #0
+		BLE end ; exit if len <= 0
+		
+		STR R2, [R0, #0]
+		ADD R0, R0, #4 ; INC(dadr,4);
+		SUB R1, R1, #1 ; DEC(len);
+		B loop
+	end:
+	END Clear32;
+	
+	(*---- fill --------------*)
+	PROCEDURE Fill8(destAdr: ADDRESS; size: SIZE; filler: LONGINT);
+	CODE
+		LDR R0, [FP, #destAdr]
+		LDR R1, [FP, #size]
+		LDR R2, [FP, #filler]
+	loop:
+		CMP R1, #0
+		BEQ end ; exit if size = 0
+		
+		STRB R2, [R0, #0]
+		ADD R0, R0, #1 ; INC(destAdr);
+		SUB R1, R1, #1 ; DEC(size);
+		B loop
+	end:
+	END Fill8;
+	
+	(* size in 16bit words*)
+	PROCEDURE Fill16(destAdr: ADDRESS; size: SIZE; filler: LONGINT);
+	CODE
+		LDR R0, [FP, #destAdr]
+		LDR R1, [FP, #size]
+		LDR R2, [FP, #filler]
+	loop:
+		CMP R1, #0
+		BEQ end ; exit if size = 0
+		
+		STRH R2, [R0, #0]
+		ADD R0, R0, #2 ; INC(destAdr,2);
+		SUB R1, R1, #1 ; DEC(size);
+		B loop
+	end:
+	END Fill16;
+	
+	(* size in 24bit entities*)
+	PROCEDURE Fill24(destAdr: ADDRESS; size: SIZE; filler: LONGINT);
+	CODE
+		LDR R0, [FP, #destAdr]
+		LDR R1, [FP, #size]
+		LDR R2, [FP, #filler]
+		LSR R4, R2, #8
+		LSR R5, R2, #16
+	loop:
+		CMP R1, #0
+		BEQ end ; exit if size = 0
+		
+		STRB R2, [R0, #0]
+		STRB R4, [R0, #1]
+		STRB R5, [R0, #2]
+		ADD R0, R0, #3 ; INC(destAdr,3);
+		SUB R1, R1, #1 ; DEC(size);
+		B loop
+	end:
+	END Fill24;
+	
+	(* size in DWords*)
+	PROCEDURE Fill32(destAdr: ADDRESS; size: SIZE; filler: LONGINT); (*! to do: change interface, introduce in Blend()*)
+	CODE
+		LDR R0, [FP, #destAdr]
+		LDR R1, [FP, #size]
+		LDR R2, [FP, #filler]
+	loop:
+		CMP R1, #0
+		BEQ end ; exit if size = 0
+		
+		STR R2, [R0, #0]
+		ADD R0, R0, #4 ; INC(destAdr,4);
+		SUB R1, R1, #1 ; DEC(size);
+		B loop
+	end:
+	END Fill32;
+
+	(* len in DWords*)
+	PROCEDURE Fill32A(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT); (*! to do: change interface, introduce in Blend()*)
+	CODE
+		LDR R0, [FP, #dadr]
+		LDR R1, [FP, #len]
+		LDR R2, [FP, #sadr]
+		LDR R2, [R2, #0]
+	loop:
+		CMP R1, #0
+		BEQ end ; exit if size = 0
+		
+		STR R2, [R0, #0]
+		ADD R0, R0, #4 ; INC(destAdr,4);
+		SUB R1, R1, #1 ; DEC(size);
+		B loop
+	end:
+	END Fill32A;
+
+	(*--- srcCopy Transfer ---*)
+
+	(* constant values *)
+	PROCEDURE Set1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR out: CHAR;
+	BEGIN
+		IF (dbit > 0) OR (len < 8) THEN
+			SYSTEM.GET(dadr, out);
+			WHILE (dbit < 8) & (len > 0) DO
+				out := Set[ORD(out), dbit];
+				INC(dbit); DEC(len)
+			END;
+			SYSTEM.PUT(dadr, out)
+		END;
+		WHILE len >= 8 DO
+			SYSTEM.PUT(dadr, 0FFX);
+			INC(dadr); DEC(len, 8)
+		END;
+		IF len > 0 THEN
+			SYSTEM.GET(dadr, out); dbit := 0;
+			REPEAT
+				out := Set[ORD(out), dbit];
+				INC(dbit); DEC(len)
+			UNTIL len = 0;
+			SYSTEM.PUT(dadr, out)
+		END
+	END Set1;
+
+	PROCEDURE ConstCopy8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		Fill8(dadr, len, ORD(mode.buf[0]))
+		(*WHILE len > 0 DO	SYSTEM.PUT(dadr, mode.buf[0]); INC(dadr); DEC(len) END*)
+	END ConstCopy8;
+
+	PROCEDURE ConstCopy16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		Fill16(dadr, len, ORD(mode.buf[0])+ASH(ORD(mode.buf[1]),8))
+		(*WHILE len > 0 DO SYSTEM.MOVE(ADDRESSOF(mode.buf[0]), dadr, 2); INC(dadr, 2); DEC(len) END*)
+	END ConstCopy16;
+
+	PROCEDURE ConstCopy24 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		Fill24(dadr, len, ORD(mode.buf[0])+ASH(ORD(mode.buf[1]),8)+ASH(ORD(mode.buf[2]),16));
+		(*WHILE len > 0 DO	SYSTEM.MOVE(ADDRESSOF(mode.buf[0]), dadr, 3); INC(dadr, 3); DEC(len) END*)
+	END ConstCopy24;
+
+	(* identical formats *)
+	PROCEDURE Copy1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in, out: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in);
+		WHILE (sbit = 0) & (dbit = 0) & (len >= 8) DO
+			SYSTEM.PUT(dadr, in);
+			INC(sadr); INC(dadr); DEC(len, 8);
+			SYSTEM.GET(sadr, in)
+		END;
+		IF (dbit > 0) OR (len < 8) THEN
+			SYSTEM.GET(dadr, out)
+		END;
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN out := Set[ORD(out), dbit]
+			ELSE out := Clr[ORD(out), dbit]
+			END;
+			INC(sbit); INC(dbit); DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END;
+			IF dbit = 8 THEN
+				SYSTEM.PUT(dadr, out);
+				INC(dadr); dbit := 0;
+				IF len < 8 THEN
+					SYSTEM.GET(dadr, out)
+				END
+			END
+		END;
+		IF dbit > 0 THEN
+			SYSTEM.PUT(dadr, out)
+		END
+	END Copy1;
+
+	PROCEDURE Copy8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		SYSTEM.MOVE(sadr, dadr, len)
+	END Copy8;
+
+	PROCEDURE I8CopyI8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR byte: CHAR;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, byte); SYSTEM.PUT(dadr, mode.map[ORD(byte)]);
+			INC(sadr); INC(dadr); DEC(len)
+		END
+	END I8CopyI8;
+
+	PROCEDURE Copy16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		SYSTEM.MOVE(sadr, dadr, 2*len)
+	END Copy16;
+
+	PROCEDURE I16CopyI16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR val: INTEGER;
+	BEGIN
+		WHILE len > 0 DO
+			val:=SYSTEM.GET16(sadr); SYSTEM.PUT16(dadr, mode.map[val MOD 10000H]);
+			INC(sadr); INC(dadr); DEC(len)
+		END
+	END I16CopyI16;
+
+
+	PROCEDURE Copy24 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		SYSTEM.MOVE(sadr, dadr, 3*len)
+	END Copy24;
+
+	PROCEDURE Copy32(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE
+		LDR R0, [FP, #dadr]
+		LDR R1, [FP, #len]
+		LDR R2, [FP, #sadr]
+	loop:
+		CMP R1, #0
+		BLE end ; exit if len <= 0
+		
+		LDR R4, [R2, #0] ; R4 := src
+		STR R4, [R0, #0] ; dst := src
+		ADD R0, R0, #4 ; INC(dadr,4);
+		ADD R2, R2, #4 ; INC(sadr,4);
+		SUB R1, R1, #1 ; DEC(len);
+		B loop
+	end:
+	(*BEGIN
+		SYSTEM.MOVE(sadr, dadr, 4*len)*)
+	END Copy32;
+
+	(* general methods *)
+	PROCEDURE AnyCopyAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			pix := mode.buf;
+			mode.src.unpack(mode.src, sadr, sbit, mode.buf);
+			mode.dst.pack(mode.dst, dadr, dbit, mode.buf);
+			sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8;
+			dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8;
+			DEC(len)
+		END
+	END AnyCopyAny;
+
+	PROCEDURE AnyBytesCopyAnyBytes (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR sinc, dinc: LONGINT; pix: Pixel;
+	BEGIN
+		sinc := mode.src.bpp DIV 8; dinc := mode.dst.bpp DIV 8;
+		WHILE len > 0 DO
+			pix := mode.buf;
+			mode.src.unpack(mode.src, sadr, sbit, mode.buf);
+			mode.dst.pack(mode.dst, dadr, dbit, mode.buf);
+			INC(sadr, sinc); INC(dadr, dinc); DEC(len)
+		END
+	END AnyBytesCopyAnyBytes;
+
+
+	(* A1 *)
+	PROCEDURE AnyCopyA1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR out: CHAR; pix: Pixel;
+	BEGIN
+		SYSTEM.GET(dadr, out); pix[a] := 0FFX;
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			sbit := sbit + mode.src.bpp; INC(sadr, sbit MOD 8); sbit := sbit MOD 8;
+			IF pix[a] >= 80X THEN out := Set[ORD(out), dbit]
+			ELSE out := Clr[ORD(out), dbit]
+			END;
+			INC(dbit); DEC(len);
+			IF dbit = 8 THEN
+				SYSTEM.PUT(dadr, out); INC(dadr); SYSTEM.GET(dadr, out); dbit := 0
+			END
+		END;
+		SYSTEM.PUT(dadr, out)
+	END AnyCopyA1;
+
+	PROCEDURE A8CopyA1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR out, in: CHAR;
+	BEGIN
+		IF (dbit > 0) OR (len < 8) THEN
+			SYSTEM.GET(dadr, out)
+		END;
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, in);
+			IF in >= 80X THEN out := Set[ORD(out), dbit]
+			ELSE out := Clr[ORD(out), dbit]
+			END;
+			INC(sadr); INC(dbit); DEC(len);
+			IF dbit = 8 THEN
+				SYSTEM.PUT(dadr, out);
+				INC(dadr); dbit := 0;
+				IF len < 8 THEN
+					SYSTEM.GET(dadr, out)
+				END
+			END
+		END;
+		IF dbit > 0 THEN
+			SYSTEM.PUT(dadr, out)
+		END
+	END A8CopyA1;
+
+	PROCEDURE BGRA8888CopyA1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR out, in: CHAR;
+	BEGIN
+		INC(sadr, a);	(* only look at alpha component *)
+		IF (dbit > 0) OR (len < 8) THEN
+			SYSTEM.GET(dadr, out)
+		END;
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, in);
+			IF in >= 80X THEN out := Set[ORD(out), dbit]
+			ELSE out := Clr[ORD(out), dbit]
+			END;
+			INC(sadr, 4); INC(dbit); DEC(len);
+			IF dbit = 8 THEN
+				SYSTEM.PUT(dadr, out);
+				INC(dadr); dbit := 0;
+				IF len < 8 THEN
+					SYSTEM.GET(dadr, out)
+				END
+			END
+		END;
+		IF dbit > 0 THEN
+			SYSTEM.PUT(dadr, out)
+		END
+	END BGRA8888CopyA1;
+
+	PROCEDURE A1CopyAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN mode.dst.pack(mode.dst, dadr, dbit, mode.buf)
+			ELSE mode.dst.pack(mode.dst, dadr, dbit, Zero)
+			END;
+			INC(sbit); dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8; DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END
+		END
+	END A1CopyAny;
+
+	PROCEDURE A1CopyA8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN SYSTEM.PUT(dadr, 0FFX)
+			ELSE SYSTEM.PUT(dadr, 0X)
+			END;
+			INC(sbit); INC(dadr); DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END
+		END
+	END A1CopyA8;
+
+	PROCEDURE A1CopyBGRA8888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel; in: CHAR;
+	BEGIN
+		pix := mode.buf;
+		SYSTEM.GET(sadr, in);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN pix[a] := 0FFX
+			ELSE pix[a] := 0X
+			END;
+			SYSTEM.MOVE(ADDRESSOF(pix), dadr, 4);
+			INC(sbit); INC(dadr, 4); DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END
+		END
+	END A1CopyBGRA8888;
+
+
+	(* A8 *)
+	PROCEDURE AnyCopyA8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		pix[a] := 0FFX;
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			SYSTEM.PUT(dadr, pix[a]);
+			INC(dadr); sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; DEC(len)
+		END
+	END AnyCopyA8;
+
+	PROCEDURE BGRA8888CopyA8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR byte: CHAR;
+	BEGIN
+		INC(sadr, 3);
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, byte);
+			SYSTEM.PUT(dadr, byte);
+			INC(sadr, 4); INC(dadr); DEC(len)
+		END
+	END BGRA8888CopyA8;
+
+	PROCEDURE A8CopyBGRA8888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		pix := mode.buf;
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, pix[a]);
+			SYSTEM.MOVE(ADDRESSOF(pix), dadr, 4);
+			INC(sadr); INC(dadr, 4); DEC(len)
+		END
+	END A8CopyBGRA8888;
+
+
+	(* P8 *)
+	PROCEDURE AnyCopyP8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			pix := mode.buf;
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			SYSTEM.PUT(dadr, CHR(CLUTs.Match(mode.dst.pal.clut, ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))));
+			INC(dadr); sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; DEC(len)
+		END
+	END AnyCopyP8;
+
+	PROCEDURE Any16CopyP8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, 0, pix);
+			SYSTEM.PUT(dadr, CHR(CLUTs.Match(mode.dst.pal.clut, ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))));
+			INC(sadr, 2); INC(dadr); DEC(len)
+		END
+	END Any16CopyP8;
+
+	PROCEDURE BGR888CopyP8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 3);
+			SYSTEM.PUT(dadr, CHR(CLUTs.Match(mode.dst.pal.clut, ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))));
+			INC(sadr, 3); INC(dadr); DEC(len)
+		END
+	END BGR888CopyP8;
+
+	PROCEDURE BGRA8888CopyP8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 4);
+			SYSTEM.PUT(dadr, CHR(CLUTs.Match(mode.dst.pal.clut, ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))));
+			INC(sadr, 4); INC(dadr); DEC(len)
+		END
+	END BGRA8888CopyP8;
+
+	PROCEDURE P8CopyAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR b: CHAR;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, b);
+			mode.dst.pack(mode.dst, dadr, dbit, mode.src.pal.col[ORD(b)]);
+			INC(sadr); dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8; DEC(len)
+		END
+	END P8CopyAny;
+
+	PROCEDURE P8CopyAny16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR b: CHAR;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, b);
+			mode.dst.pack(mode.dst, dadr, 0, mode.src.pal.col[ORD(b)]);
+			INC(sadr); INC(dadr, 2); DEC(len)
+		END
+	END P8CopyAny16;
+
+	PROCEDURE P8CopyBGR888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR b: CHAR;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, b);
+			SYSTEM.MOVE(ADDRESSOF(mode.src.pal.col[ORD(b)]), dadr, 3);
+			INC(sadr); INC(dadr, 3); DEC(len)
+		END
+	END P8CopyBGR888;
+
+	PROCEDURE P8CopyBGRA8888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR b: CHAR;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, b);
+			SYSTEM.MOVE(ADDRESSOF(mode.src.pal.col[ORD(b)]), dadr, 4);
+			INC(sadr); INC(dadr, 4); DEC(len)
+		END
+	END P8CopyBGRA8888;
+
+
+	(* D8 *)
+	PROCEDURE AnyCopyD8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			pix := mode.buf;
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			SYSTEM.PUT(dadr, CHR(ColorToIndex(ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))));
+			INC(dadr); sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; DEC(len)
+		END
+	END AnyCopyD8;
+
+	PROCEDURE Any16CopyD8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, 0, pix);
+			SYSTEM.PUT(dadr, CHR(ColorToIndex(ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))));
+			INC(sadr, 2); INC(dadr); DEC(len)
+		END
+	END Any16CopyD8;
+
+	PROCEDURE BGR888CopyD8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 3);
+			SYSTEM.PUT(dadr, CHR(ColorToIndex(ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))));
+			INC(sadr, 3); INC(dadr); DEC(len)
+		END
+	END BGR888CopyD8;
+
+	PROCEDURE BGRA8888CopyD8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 4);
+			SYSTEM.PUT(dadr, CHR(ColorToIndex(ORD(pix[b]) + ASH(ORD(pix[g]), 8) + ASH(ORD(pix[r]), 16))));
+			INC(sadr, 4); INC(dadr); DEC(len)
+		END
+	END BGRA8888CopyD8;
+
+	PROCEDURE D8CopyAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel; byte: CHAR; col: LONGINT;
+	BEGIN
+		pix[a] := 0FFX;
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, byte); col := IndexToColor(ORD(byte));
+			pix[b] := CHR(col MOD 100H); pix[g] := CHR(ASH(col, -8) MOD 100H); pix[r] := CHR(ASH(col, -16) MOD 100H);
+			mode.dst.pack(mode.dst, dadr, dbit, pix);
+			INC(sadr); dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8; DEC(len)
+		END
+	END D8CopyAny;
+
+	PROCEDURE D8CopyAny16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel; byte: CHAR; col: LONGINT;
+	BEGIN
+		pix[a] := 0FFX;
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, byte); col := IndexToColor(ORD(byte));
+			pix[b] := CHR(col MOD 100H); pix[g] := CHR(ASH(col, -8) MOD 100H); pix[r] := CHR(ASH(col, -16) MOD 100H);
+			mode.dst.pack(mode.dst, dadr, 0, pix);
+			INC(sadr); INC(dadr, 2); DEC(len)
+		END
+	END D8CopyAny16;
+
+	PROCEDURE D8CopyBGR888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR byte: CHAR; col: LONGINT; pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, byte); col := IndexToColor(ORD(byte));
+			pix[b] := CHR(col MOD 100H); pix[g] := CHR(ASH(col, -8) MOD 100H); pix[r] := CHR(ASH(col, -16) MOD 100H);
+			SYSTEM.MOVE(ADDRESSOF(pix[0]), dadr, 3);
+			INC(sadr); INC(dadr, 3); DEC(len)
+		END
+	END D8CopyBGR888;
+
+	PROCEDURE D8CopyBGRA8888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel; byte: CHAR; col: LONGINT;
+	BEGIN
+		pix[a] := 0FFX;
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, byte); col := IndexToColor(ORD(byte));
+			pix[b] := CHR(col MOD 100H); pix[g] := CHR(ASH(col, -8) MOD 100H); pix[r] := CHR(ASH(col, -16) MOD 100H);
+			SYSTEM.MOVE(ADDRESSOF(pix[0]), dadr, 4);
+			INC(sadr); INC(dadr, 4); DEC(len)
+		END
+	END D8CopyBGRA8888;
+
+	(*
+	(* P816*)
+	PROCEDURE AnyCopyP16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			pix := mode.buf;
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			SYSTEM.PUT16(dadr, PaletteIndex(mode.dst.pal, ORD(pix[r]), ORD(pix[g]), ORD(pix[b])));
+			INC(dadr,2); sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; DEC(len)
+		END
+	END AnyCopyP16;
+
+	PROCEDURE Any16CopyP16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, 0, pix);
+			SYSTEM.PUT16(dadr, PaletteIndex(mode.dst.pal, ORD(pix[r]), ORD(pix[g]), ORD(pix[b])));
+			INC(sadr, 2); INC(dadr,2); DEC(len)
+		END
+	END Any16CopyP16;
+
+	PROCEDURE BGR888CopyP16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 3);
+			SYSTEM.PUT16(dadr, PaletteIndex(mode.dst.pal, ORD(pix[r]), ORD(pix[g]), ORD(pix[b])));
+			INC(sadr, 3); INC(dadr,2); DEC(len)
+		END
+	END BGR888CopyP16;
+
+	PROCEDURE BGRA8888CopyP16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 4);
+			SYSTEM.PUT16(dadr, PaletteIndex(mode.dst.pal, ORD(pix[r]), ORD(pix[g]), ORD(pix[b])));
+			INC(sadr, 4); INC(dadr,2); DEC(len)
+		END
+	END BGRA8888CopyP16;
+
+	PROCEDURE P16CopyAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		WHILE len > 0 DO
+			mode.dst.pack(mode.dst, dadr, dbit, mode.src.pal.col[LONG(SYSTEM.GET16(sadr)) MOD 10000H]);
+			INC(sadr,2); dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8; DEC(len)
+		END
+	END P16CopyAny;
+
+	PROCEDURE P16CopyAny16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		WHILE len > 0 DO
+			mode.dst.pack(mode.dst, dadr, 0, mode.src.pal.col[LONG(SYSTEM.GET16(sadr)) MOD 10000H]);
+			INC(sadr,2); INC(dadr, 2); DEC(len)
+		END
+	END P16CopyAny16;
+	*)
+
+	PROCEDURE P16CopyBGR888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(ADDRESSOF(mode.src.pal.col[LONG(SYSTEM.GET16(sadr)) MOD 10000H]), dadr, 3);
+			INC(sadr,2); INC(dadr, 3); DEC(len)
+		END
+	END P16CopyBGR888;
+
+	PROCEDURE P16CopyBGRA8888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	VAR val:LONGINT;
+	BEGIN
+		WHILE len > 0 DO
+			val:=LONG(SYSTEM.GET16(sadr)) MOD 10000H;
+			SYSTEM.MOVE(ADDRESSOF(mode.src.pal.col[val]), dadr, 4);
+			INC(sadr,2); INC(dadr, 4); DEC(len)
+		END
+	END P16CopyBGRA8888;
+
+
+
+	(* BGR555, BGR565, BGR466 *)
+	PROCEDURE AnyCopyAny16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			pix := mode.buf;
+			mode.src.unpack(mode.src, sadr, 0, pix);
+			mode.dst.pack(mode.dst, dadr, 0, pix);
+			INC(dadr, 2); sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; DEC(len)
+		END
+	END AnyCopyAny16;
+
+	PROCEDURE Any16CopyAny16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, 0, pix);
+			mode.dst.pack(mode.dst, dadr, 0, pix);
+			INC(sadr, 2); INC(dadr, 2); DEC(len)
+		END
+	END Any16CopyAny16;
+
+	PROCEDURE BGR888CopyAny16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		pix[a] := 0FFX;
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 3);
+			mode.dst.pack(mode.dst, dadr, 0, pix);
+			INC(sadr, 3); INC(dadr, 2); DEC(len)
+		END
+	END BGR888CopyAny16;
+
+	PROCEDURE BGRA8888CopyAny16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 4);
+			mode.dst.pack(mode.dst, dadr, 0, pix);
+			INC(dadr, 2); sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; DEC(len)
+		END
+	END BGRA8888CopyAny16;
+
+	PROCEDURE Any16CopyAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, 0, pix);
+			mode.dst.pack(mode.dst, dadr, 0, pix);
+			INC(sadr, 2); dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8; DEC(len)
+		END
+	END Any16CopyAny;
+
+	PROCEDURE Any16CopyBGR888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, 0, pix);
+			SYSTEM.MOVE(ADDRESSOF(pix[0]), dadr, 3);
+			INC(sadr, 2); INC(dadr, 3); DEC(len)
+		END
+	END Any16CopyBGR888;
+
+	PROCEDURE Any16CopyBGRA8888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, 0, pix);
+			SYSTEM.MOVE(ADDRESSOF(pix[0]), dadr, 4);
+			INC(sadr, 2); INC(dadr, 4); DEC(len)
+		END
+	END Any16CopyBGRA8888;
+
+
+	(* BGR888 *)
+	PROCEDURE AnyCopyBGR888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			pix := mode.buf;
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			SYSTEM.MOVE(ADDRESSOF(pix[0]), dadr, 3);
+			INC(dadr, 3); sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; DEC(len)
+		END
+	END AnyCopyBGR888;
+
+	PROCEDURE BGRA8888CopyBGR888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, dadr, 3);
+			INC(sadr, 4); INC(dadr, 3); DEC(len)
+		END
+	END BGRA8888CopyBGR888;
+
+	(*PROCEDURE SSE2BGRA8888CopyBGR888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE {SYSTEM.i386, SYSTEM.MMX, SYSTEM.SSE, SYSTEM.SSE2, SYSTEM.Pentium}
+		PUSHFD
+		PUSH 		EBX
+		; CLI
+
+		MOV		ESI, [EBP+sadr]	; source adr
+		MOV 		EDI, [EBP+dadr]  ; source adr
+
+		MOV		ECX, [EBP+len]
+
+	loop:
+		CMP		ECX, 0
+		JLE			end
+   		CMP 		ECX, 4
+		 JL			singlepixel
+
+	fourpixel:
+		; 4pixels at the time
+		MOV 		EAX,      [ESI] ; pixel 0
+		MOV 		EBX,    [ESI+4] ; pixel 1
+
+		AND 		EAX, 0FFFFFFH
+		AND 		EBX, 0FFFFFFH
+		MOV 		EDX, EBX
+		SHL			EDX, 24
+		OR			EAX, EDX ; 1000
+		MOV		[EDI], EAX ; write back to mem
+
+		MOV 		EAX,    [ESI+8] ; pixel 2
+		AND 		EAX, 0FFFFFFH
+		SHR		EBX,8
+		MOV		EDX, EAX
+		SHL			EDX, 16
+		OR			EBX, EDX ; 2211
+		MOV		[EDI+4], EBX
+
+		MOV 		EDX, [ESI+12] ; pixel 3
+		SHL			EDX, 8
+		SHR		EAX, 16
+		OR			EAX, EDX ; 3332
+
+		MOV		[EDI], EAX
+
+		ADD 		ESI, 16
+		ADD 		EDI, 12
+		SUB 		ECX, 4
+		JG			loop
+		JMP 		end
+
+	singlepixel:
+		MOV 		EAX, [ESI]
+
+		MOV		[EDI], AX
+		SHR		EAX, 16
+		MOV		[EDI+2], AL
+
+		ADD 		ESI, 4
+		ADD		EDI, 3
+		SUB 		ECX, 1
+		JG			loop
+
+	end:
+		EMMS ; declare FPU registers free
+		POP 		EBX
+		POPFD
+	END SSE2BGRA8888CopyBGR888;*)
+
+	PROCEDURE BGR888CopyAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		pix[a] := 0FFX;
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 3);
+			mode.dst.pack(mode.dst, dadr, dbit, pix);
+			INC(sadr, 3); dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8; DEC(len)
+		END
+	END BGR888CopyAny;
+
+	PROCEDURE BGR888CopyBGRA8888(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, dadr, 3); SYSTEM.PUT(dadr+3, 0FFX);
+			INC(sadr, 3); INC(dadr, 4); DEC(len)
+		END
+
+	END BGR888CopyBGRA8888;
+
+(*	PROCEDURE SSE2BGR888CopyBGRA8888(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE {SYSTEM.i386, SYSTEM.MMX, SYSTEM.SSE, SYSTEM.SSE2,SYSTEM.Pentium}
+		PUSHFD
+		PUSH 		EBX
+		; CLI
+
+		PXOR 		XMM0, XMM0
+
+		MOV 		EAX, 0FF00H
+		PINSRW 	XMM0, EAX, 1
+		PINSRW 	XMM0, EAX, 3
+		PINSRW 	XMM0, EAX, 5
+		PINSRW 	XMM0, EAX, 7 ; prepare for fourpixel
+
+		SHL 		EAX, 16 ; prepare for singlepixel
+
+		MOV		ESI, [EBP+sadr]	; source adr
+		MOV 		EDI, [EBP+dadr]  ; source adr
+
+		MOV		ECX, [EBP+len]
+
+	loop:
+		CMP		ECX, 0
+		JLE			end
+   		CMP 		ECX, 4
+		 JL			singlepixel
+
+	fourpixel:
+	; 4pixels at the time
+		PXOR 		XMM2,XMM2
+		PXOR 		XMM1,XMM1
+
+		MOV		EBX, [ESI+9] ; read 1st source pixel
+		MOVD 		XMM2, EBX
+
+		PSLLDQ		XMM2, 4
+		MOV		EBX, [ESI+6] ; read 2nd source pixel
+		MOVD 		XMM1, EBX
+		POR 		XMM2, XMM1
+
+		PSLLDQ		XMM2, 4
+		MOV		EBX, [ESI+3] ; read 3rd source pixel
+		MOVD 		XMM1, EBX
+		POR 		XMM2, XMM1
+
+		PSLLDQ		XMM2, 4
+		MOV		EBX, [ESI] ; read 4th source pixel
+		MOVD 		XMM1, EBX
+		POR 		XMM2, XMM1
+		ADD 		ESI, 12
+
+		POR 		XMM2, XMM0
+
+		MOVDQU 	[EDI], XMM2 ; set the pixels
+		ADD		EDI, 16	; inc adr
+		SUB 		ECX, 4
+		JG			loop
+		JMP 		end
+
+	singlepixel:
+		MOV		EBX, [ESI] ; read source pixel
+		OR 			EBX, EAX
+		ADD 		ESI, 3
+		MOV 		[EDI], EBX
+
+		ADD		EDI, 4	; inc adr
+		SUB 		ECX, 1
+		JG			loop
+	end:
+		EMMS ; declare FPU registers free
+		POP 		EBX
+		POPFD
+	END SSE2BGR888CopyBGRA8888;*)
+
+	(* BGRA8888 *)
+	PROCEDURE AnyCopyBGRA8888 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			pix := mode.buf;
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			SYSTEM.MOVE(ADDRESSOF(pix[0]), dadr, 4);
+			INC(dadr, 4); sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; DEC(len)
+		END
+	END AnyCopyBGRA8888;
+
+	PROCEDURE BGRA8888CopyAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(pix[0]), 4);
+			mode.dst.pack(mode.dst, dadr, dbit, pix);
+			INC(sadr, 4); dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8; DEC(len)
+		END
+	END BGRA8888CopyAny;
+
+
+	(*--- dstCopy Transfer ---*)
+
+	PROCEDURE EmptyTransfer (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	END EmptyTransfer;
+
+
+	(*--- srcOverDst Transfer ---*)
+
+	(* A1 *)
+	PROCEDURE AnyOverA1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR out: CHAR; pix: Pixel;
+	BEGIN
+		SYSTEM.GET(dadr, out);
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			IF pix[a] >= 80X THEN
+				out := Set[ORD(out), dbit]
+			END;
+			sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; INC(dbit); DEC(len);
+			IF dbit = 8 THEN
+				SYSTEM.PUT(dadr, out);
+				INC(dadr); dbit := 0;
+				SYSTEM.GET(dadr, out)
+			END
+		END;
+		SYSTEM.PUT(dadr, out)
+	END AnyOverA1;
+
+	PROCEDURE A1OverA1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in, out: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in); SYSTEM.GET(dadr, out);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN
+				out := Set[ORD(out), dbit]
+			END;
+			INC(sbit); INC(dbit); DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END;
+			IF dbit = 8 THEN
+				SYSTEM.PUT(dadr, out);
+				INC(dadr); dbit := 0;
+				SYSTEM.GET(dadr, out)
+			END
+		END;
+		SYSTEM.PUT(dadr, out)
+	END A1OverA1;
+
+	PROCEDURE A8OverA1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in, out: CHAR;
+	BEGIN
+		SYSTEM.GET(dadr, out);
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, in);
+			IF in >= 80X THEN
+				out := Set[ORD(out), dbit]
+			END;
+			INC(sadr); INC(dbit); DEC(len);
+			IF dbit = 8 THEN
+				SYSTEM.PUT(dadr, out);
+				INC(dadr); dbit := 0;
+				SYSTEM.GET(dadr, out)
+			END
+		END;
+		SYSTEM.PUT(dadr, out)
+	END A8OverA1;
+
+	PROCEDURE BGRA8888OverA1 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in, out: CHAR;
+	BEGIN
+		SYSTEM.GET(dadr, out);
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr + a, in);
+			IF in >= 80X THEN
+				out := Set[ORD(out), dbit]
+			END;
+			INC(sadr, 4); INC(dbit); DEC(len);
+			IF dbit = 8 THEN
+				SYSTEM.PUT(dadr, out);
+				INC(dadr); dbit := 0;
+				SYSTEM.GET(dadr, out)
+			END
+		END;
+		SYSTEM.PUT(dadr, out)
+	END BGRA8888OverA1;
+
+	PROCEDURE A1OverAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN
+				mode.dst.pack(mode.dst, dadr, dbit, mode.buf)
+			END;
+			INC(sbit); dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8; DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END
+		END
+	END A1OverAny;
+
+	PROCEDURE A1OverConst8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN
+				SYSTEM.PUT(dadr, mode.buf[0])
+			END;
+			INC(sbit); INC(dadr); DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END
+		END
+	END A1OverConst8;
+
+	PROCEDURE A1OverConst16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN
+				SYSTEM.MOVE(ADDRESSOF(mode.buf[0]), dadr, 2)
+			END;
+			INC(sbit); INC(dadr, 2); DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END
+		END
+	END A1OverConst16;
+
+	PROCEDURE A1OverConst24 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN
+				SYSTEM.MOVE(ADDRESSOF(mode.buf[0]), dadr, 3)
+			END;
+			INC(sbit); INC(dadr, 3); DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END
+		END
+	END A1OverConst24;
+
+	PROCEDURE A1OverConst32 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in: CHAR;
+	BEGIN
+		SYSTEM.GET(sadr, in);
+		WHILE len > 0 DO
+			IF Bit[ORD(in), sbit] THEN
+				SYSTEM.MOVE(ADDRESSOF(mode.buf[0]), dadr, 4)
+			END;
+			INC(sbit); INC(dadr, 4); DEC(len);
+			IF sbit = 8 THEN
+				INC(sadr); sbit := 0;
+				SYSTEM.GET(sadr, in)
+			END
+		END
+	END A1OverConst32;
+
+
+	(* A8 *)
+	PROCEDURE AnyOverA8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR pix: Pixel; b: CHAR;
+	BEGIN
+		WHILE len > 0 DO
+			mode.src.unpack(mode.src, sadr, sbit, pix);
+			IF pix[a] = 0FFX THEN
+				SYSTEM.PUT(dadr, 0FFX)
+			ELSIF pix[a] # 0X THEN
+				SYSTEM.GET(dadr, b);
+				SYSTEM.PUT(dadr, CHR(ORD(pix[a]) + ORD(b) * LONG(255-ORD(pix[a])) DIV 255))
+			END;
+			sbit := sbit + mode.src.bpp; INC(sadr, sbit DIV 8); sbit := sbit MOD 8; INC(dadr); DEC(len)
+		END
+	END AnyOverA8;
+
+	PROCEDURE A8OverAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	VAR spix, dpix: Pixel; alpha, rc,gc,bc,ac: CHAR;
+	BEGIN
+		ac := mode.col[a];
+		rc := mode.col[r];
+		gc := mode.col[g];
+		bc := mode.col[b];
+		
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, alpha);
+			alpha := CHR(ORD(ac) * ORD(alpha) DIV 255);
+			IF alpha = 0FFX THEN
+				mode.dst.pack(mode.dst, dadr, dbit, mode.buf)
+			ELSIF alpha # 0X THEN
+				spix[a] := alpha;
+				(* the following computation of the colors has to be done because the blending method seems to assume this *)
+				spix[r] := CHR(ORD(rc) * ORD(alpha) DIV 255);
+				spix[g] := CHR(ORD(gc) * ORD(alpha) DIV 255);
+				spix[b] := CHR(ORD(bc) * ORD(alpha) DIV 255);
+				mode.dst.unpack(mode.dst, dadr, dbit, dpix);
+				Blend(mode.op, spix, dpix);
+				mode.dst.pack(mode.dst, dadr, dbit, dpix);
+			END;
+			INC(sadr); DEC(len);
+			dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8
+		END
+	END A8OverAny;
+	
+	
+	PROCEDURE A8OverBGRA8888Asm(CONST col: Pixel; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE
+		LDR R0, [FP, #sadr]
+		LDR R1, [FP, #dadr]
+		LDR R2, [FP, #len]
+		
+		LDR R6, [FP, #col]
+		LDRB R3, [R6, #0] ; col[b]
+		LDRB R4, [R6, #1] ; col[g] 
+		LDRB R5, [R6, #2] ; col[r]
+		LDRB R10, [R6, #3] ; col[a]
+		ADD R10, R10, #1; to account for DIV 256 i/o DIV 255 below
+		
+	loop:
+		CMP R2, #0
+		BLE end ; exit if len <= 0
+		
+		LDRB R6, [R0, #0] ; src[a]
+		MUL R6, R10, R6 ; alpha := src0[a]*col[a]
+		LSR R6, R6, #8 ; alpha := alpha DIV 256
+
+		CMP R6, #255
+		BNE compute ; src[a] # 255 -> compute blended colors
+		
+		; src[a] = 255 -> just move src to dst
+		MOV R8, R3 ; dst[b] := src0[b]
+		ORR R8, R8, R4, LSL #8 ; dst[g] := src0[g]
+		ORR R8, R8, R5, LSL #16 ; dst[r] := src0[r]
+		ORR R8, R8, R6, LSL #24 ; dst[a] := src[a]
+		B store ; go to the actual data move
+		
+	compute:
+		RSB R7, R6, #255 ; fd := 255 - src[a]
+		
+		LDR R8, [R1, #0] ; R8 := dst
+		
+		; compute src[b]
+		MUL R9, R3, R6 ; src0[b]*src[a]
+		LSR R9, R9, #8 ; src[b] := (src0[b]*src[a]) DIV 256
+		; compute dst[b]
+		AND R11, R8, #0FFH ; dst[b]
+		MUL R11, R11, R7 ; dst[b]*fd
+		ADD R11, R11, R9, LSL #8 ; dst[b]*fd + src[b]*256
+		LSR R11, R11, #8 ; (dst[b]*fd + src[b]*256) DIV 256
+		CMP R11, #255
+		BLT put_b
+		MOV R11, #255
+	
+	put_b:
+		BIC R8, R8, #0FFH ; clear dst[b]
+		ORR R8, R8, R11
+		
+		; compute src[g]
+		MUL R9, R4, R6 ; src0[g]*src[a]
+		LSR R9, R9, #8 ; src[g] := (src0[g]*src[a]) DIV 256
+		; compute dst[g]
+		AND R11, R8, #0FF00H ; dst[g]*256
+		LSR R11, R11, #8 ; dst[g]
+		MUL R11, R11, R7 ; dst[g]*fd
+		ADD R11, R11, R9, LSL #8 ; dst[g]*fd + src[g]*256
+		LSR R11, R11, #8 ; (dst[g]*fd + src[g]*256) DIV 256
+		CMP R11, #255
+		BLT put_g
+		MOV R11, #255
+	
+	put_g:
+		BIC R8, R8, #0FF00H ; clear dst[g]
+		ORR R8, R8, R11, LSL #8
+		
+		; compute src[r]
+		MUL R9, R5, R6 ; src0[r]*src[a]
+		LSR R9, R9, #8 ; src[r] := (src0[r]*src[a]) DIV 256
+		; compute dst[r]
+		AND R11, R8, #0FF0000H ; dst[r]*65536
+		LSR R11, R11, #16 ; dst[r]
+		MUL R11, R11, R7 ; dst[r]*fd
+		ADD R11, R11, R9, LSL #8 ; dst[r]*fd + src[r]*256
+		LSR R11, R11, #8 ; (dst[r]*fd + src[r]*256) DIV 256
+		CMP R11, #255
+		BLT put_r
+		MOV R11, #255
+	
+	put_r:
+		BIC R8, R8, #0FF0000H ; clear dst[r]
+		ORR R8, R8, R11, LSL #16
+		
+		; compute dst[a]
+		AND R11, R8, #0FF000000H ; dst[a]*16777216
+		LSR R11, R11, #24 ; dst[a]
+		MUL R11, R11, R7 ; dst[a]*fd
+		ADD R11, R11, R6, LSL #8 ; dst[a]*fd + src[a]*256
+		LSR R11, R11, #8 ; (dstfdc[a] + src[a]*256) DIV 256
+		CMP R11, #255
+		BLT put_a
+		MOV R11, #255
+	
+	put_a:
+		BIC R8, R8, #0FF000000H ; clear dst[a]
+		ORR R8, R8, R11, LSL #24
+		
+	store:
+		STR R8, [R1, #0]
+		ADD R0, R0, #1 ; INC(sadr);
+		ADD R1, R1, #4 ; INC(dadr,4);
+		SUB R2, R2, #1 ; DEC(len);
+		B loop
+	end:
+	END A8OverBGRA8888Asm;
+	
+	PROCEDURE A8OverBGRA8888(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	BEGIN
+		A8OverBGRA8888Asm(mode.col,sadr,sbit,dadr,dbit,len);
+	END A8OverBGRA8888;
+	
+	(*PROCEDURE A8OverBGRA8888(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	VAR src , dst: Pixel; rc,gc,bc, fd, fs: LONGINT;
+	BEGIN
+		rc := ORD(mode.col[r]);
+		gc := ORD(mode.col[g]);
+		bc := ORD(mode.col[b]);
+		fs := 255;
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, src[a]);
+			SYSTEM.MOVE(dadr, ADDRESSOF(dst), 4);
+				(* the following computation of the colors has to be done because the blending method seems to assume this *)
+				src[r] := CHR(rc * ORD(src[a]) DIV 255);
+				src[g] := CHR(gc * ORD(src[a]) DIV 255);
+				src[b] := CHR(bc * ORD(src[a]) DIV 255);
+
+				fd := 255-ORD(src[a]);
+				
+				dst[0] := Clamp[200H + (fs * ORD(src[0]) + fd * ORD(dst[0])) DIV 255];
+				dst[1] := Clamp[200H + (fs * ORD(src[1]) + fd * ORD(dst[1])) DIV 255];
+				dst[2] := Clamp[200H + (fs * ORD(src[2]) + fd * ORD(dst[2])) DIV 255];
+				dst[3] := Clamp[200H + (fs * ORD(src[3]) + fd * ORD(dst[3])) DIV 255];
+				
+				SYSTEM.MOVE(ADDRESSOF(dst),dadr,4);
+			INC(sadr); INC(dadr,4); DEC(len);
+		END
+	END A8OverBGRA8888;*)
+
+	PROCEDURE A8OverA8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in, out: CHAR;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr, in);
+			IF in = 0FFX THEN
+				SYSTEM.PUT(dadr, 0FFX)
+			ELSIF in # 0X THEN
+				SYSTEM.GET(dadr, out);
+				SYSTEM.PUT(dadr, CHR(ORD(in) + ORD(out) * LONG(255-ORD(in)) DIV 255))
+			END;
+			INC(sadr); INC(dadr); DEC(len)
+		END
+	END A8OverA8;
+	
+	PROCEDURE BGRA8888OverBGRA8888(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE
+		LDR R0, [FP, #sadr]
+		LDR R1, [FP, #dadr]
+		LDR R2, [FP, #len]
+
+	loop:
+		CMP R2, #0
+		BLE end ; exit if len <= 0
+		
+		LDR R8, [R0, #0] ; R8 := src
+		LSR R6, R8, #24 ; R6 := src[a]
+		
+		CMP R6, #255
+		BEQ store ; src[a] = 255 -> just move src to dst
+		
+		MOV R9, #0FFH ; LSB mask
+		AND R3, R9, R8 ; R3 := src[b]
+		AND R4, R9, R8, LSR #8 ; R4 := src[g]
+		AND R5, R9, R8, LSR #16 ; R5 := src[r]
+
+	compute:
+		RSB R7, R6, #255 ; fd := 255 - src[a]
+		
+		LDR R8, [R1, #0] ; R8 := dst
+		
+		; compute dst[b]
+		AND R11, R8, #0FFH ; dst[b]
+		MUL R11, R11, R7 ; dst[b]*fd
+		ADD R11, R11, R3, LSL #8 ; dst[b]*fd + src[b]*256
+		LSR R11, R11, #8 ; (dst[b]*fd + src[b]*256) DIV 256
+		CMP R11, #255
+		BLT put_b
+		MOV R11, #255
+	
+	put_b:
+		BIC R8, R8, #0FFH ; clear dst[b]
+		ORR R8, R8, R11
+		
+		; compute dst[g]
+		AND R11, R8, #0FF00H ; dst[g]*256
+		LSR R11, R11, #8 ; dst[g]
+		MUL R11, R11, R7 ; dst[g]*fd
+		ADD R11, R11, R4, LSL #8 ; dst[g]*fd + src[g]*256
+		LSR R11, R11, #8 ; (dst[g]*fd + src[g]*256) DIV 256
+		CMP R11, #255
+		BLT put_g
+		MOV R11, #255
+	
+	put_g:
+		BIC R8, R8, #0FF00H ; clear dst[g]
+		ORR R8, R8, R11, LSL #8
+		
+		; compute dst[r]
+		AND R11, R8, #0FF0000H ; dst[r]*65536
+		LSR R11, R11, #16 ; dst[r]
+		MUL R11, R11, R7 ; dst[r]*fd
+		ADD R11, R11, R5, LSL #8 ; dst[r]*fd + src[r]*256
+		LSR R11, R11, #8 ; (dst[r]*fd + src[r]*256) DIV 256
+		CMP R11, #255
+		BLT put_r
+		MOV R11, #255
+	
+	put_r:
+		BIC R8, R8, #0FF0000H ; clear dst[r]
+		ORR R8, R8, R11, LSL #16
+		
+		; compute dst[a]
+		AND R11, R8, #0FF000000H ; dst[a]*16777216
+		LSR R11, R11, #24 ; dst[a]
+		MUL R11, R11, R7 ; dst[a]*fd
+		ADD R11, R11, R6, LSL #8 ; dst[a]*fd + src[a]*256
+		LSR R11, R11, #8 ; (dstfdc[a] + src[a]*256) DIV 256
+		CMP R11, #255
+		BLT put_a
+		MOV R11, #255
+	
+	put_a:
+		BIC R8, R8, #0FF000000H ; clear dst[a]
+		ORR R8, R8, R11, LSL #24
+		
+	store:
+		STR R8, [R1, #0]
+		ADD R0, R0, #4 ; INC(sadr,4);
+		ADD R1, R1, #4 ; INC(dadr,4);
+		SUB R2, R2, #1 ; DEC(len);
+		B loop
+	end:
+	END BGRA8888OverBGRA8888;
+
+	(*PROCEDURE BGRA8888OverBGRA8888(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	VAR src, dst: Pixel; fd,t: LONGINT;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(src), 4);
+			IF src[a] = 0FFX THEN
+				SYSTEM.MOVE(sadr,dadr,4);
+			ELSIF src[a] # 0X THEN
+				SYSTEM.MOVE(dadr, ADDRESSOF(dst), 4);
+				fd := 256-ORD(src[a]);
+				dst[0] := CHR(MIN( (256 * ORD(src[0]) + fd * ORD(dst[0])) DIV 256, 255));
+				(*
+				IF t < 255 THEN dst[0] := CHR(t) ELSE dst[0] := CHR(255) END;
+				*)
+				dst[1] := CHR(MIN((256 * ORD(src[1]) + fd * ORD(dst[1])) DIV 256, 255));
+				(*
+				IF t < 255 THEN dst[1] := CHR(t) ELSE dst[1] := CHR(255) END;
+				*)
+				dst[2] := CHR(MIN( (256 * ORD(src[2]) + fd * ORD(dst[2])) DIV 256, 255));
+				(*
+				IF t < 255 THEN dst[2] := CHR(t) ELSE dst[2] := CHR(255) END;
+				*)
+				dst[3] := CHR(MIN( (256 * ORD(src[3]) + fd * ORD(dst[3])) DIV 256, 255));
+				(*
+				IF t < 255 THEN dst[3] := CHR(t) ELSE dst[3] := CHR(255) END;
+				*)
+				SYSTEM.MOVE(ADDRESSOF(dst),dadr,4);
+			END;
+			INC(sadr,4); INC(dadr,4); DEC(len);
+		END
+
+	END BGRA8888OverBGRA8888;*)
+
+
+	(* BGRA8888 *)
+	PROCEDURE BGRA8888OverAny (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR spix, dpix: Pixel;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr, ADDRESSOF(spix),4);
+			(*
+			SYSTEM.GET (sadr, spix);
+			*)
+			IF spix[a] = 0FFX THEN
+				mode.dst.pack(mode.dst, dadr, dbit, spix)
+			ELSIF spix[a] # 0X THEN
+				mode.dst.unpack(mode.dst, dadr, dbit, dpix);
+				Blend(mode.op, spix, dpix);
+				mode.dst.pack(mode.dst, dadr, dbit, dpix)
+			END;
+			INC(sadr, SIZEOF (Pixel)); DEC(len);
+			dbit := dbit + mode.dst.bpp; INC(dadr, dbit DIV 8); dbit := dbit MOD 8
+		END
+	END BGRA8888OverAny;
+
+	PROCEDURE BGRA8888OverA8 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR in, out: CHAR;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.GET(sadr + a, in);
+			IF in = 0FFX THEN
+				SYSTEM.PUT(dadr, 0FFX)
+			ELSIF in # 0X THEN
+				SYSTEM.GET(dadr, out);
+				SYSTEM.PUT(dadr, CHR(ORD(in) + ORD(out) * (255 - ORD(in)) DIV 255))
+			END;
+			INC(sadr, 4); INC(dadr); DEC(len)
+		END
+	END BGRA8888OverA8;
+
+	PROCEDURE BGRA8888OverAny16 (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR src, dst: Pixel; fd, t: LONGINT;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr,ADDRESSOF(src),4);
+			(* SYSTEM.GET (sadr, src);*)
+			IF src[a] = 0FFX THEN
+				mode.dst.pack(mode.dst, dadr, dbit, src)
+			ELSIF src[a] # 0X THEN
+				mode.dst.unpack(mode.dst, dadr, 0, dst);
+				fd := 255-ORD(src[a]);
+				t := (256 * ORD(src[0]) + fd * ORD(dst[0])) DIV 256;
+				IF t < 255 THEN dst[0] := CHR(t) ELSE dst[0] := CHR(255) END;
+
+				t := (256 * ORD(src[1]) + fd * ORD(dst[1])) DIV 256;
+				IF t < 255 THEN dst[1] := CHR(t) ELSE dst[1] := CHR(255) END;
+				t := (256 * ORD(src[2]) + fd * ORD(dst[2])) DIV 256;
+				IF t < 255 THEN dst[2] := CHR(t) ELSE dst[2] := CHR(255) END;
+				t := (256 * ORD(src[3]) + fd * ORD(dst[3])) DIV 256;
+				IF t < 255 THEN dst[3] := CHR(t) ELSE dst[3] := CHR(255) END;
+				mode.dst.pack(mode.dst, dadr, 0, dst);
+			END;
+			INC(dadr, 2); INC(sadr, SIZEOF (Pixel)); DEC(len)
+		END
+	END BGRA8888OverAny16;
+
+	PROCEDURE BGRA8888Over565* (VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+		VAR src, dst: Pixel; fd, t: LONGINT;
+	BEGIN
+		WHILE len > 0 DO
+			SYSTEM.MOVE(sadr,ADDRESSOF(src),4);
+			(*
+			SYSTEM.GET (sadr, src);
+			*)
+			IF src[a] = 0FFX THEN
+				SYSTEM.PUT16(dadr, ASH(ORD(src[b]), -3) + ASH(ASH(ORD(src[g]), -2), 5) + ASH(ASH(ORD(src[r]), -3), 11));
+			ELSIF src[a] # 0X THEN
+				t := SYSTEM.GET16(dadr);
+				dst[b] := CHR((t MOD 32) * 8); dst[g] := CHR((t DIV 32 MOD 64) * 4); dst[r] := CHR((t DIV 2048 MOD 32) * 8);
+
+				fd := 256-ORD(src[a]);
+				t := (256 * ORD(src[0]) + fd * ORD(dst[0])) DIV 256;
+				IF t < 255 THEN dst[0] := CHR(t) ELSE dst[0] := CHR(255) END;
+
+				t := (256 * ORD(src[1]) + fd * ORD(dst[1])) DIV 256;
+				IF t < 255 THEN dst[1] := CHR(t) ELSE dst[1] := CHR(255) END;
+				t := (256 * ORD(src[2]) + fd * ORD(dst[2])) DIV 256;
+				IF t < 255 THEN dst[2] := CHR(t) ELSE dst[2] := CHR(255) END;
+				t := (256 * ORD(src[3]) + fd * ORD(dst[3])) DIV 256;
+				IF t < 255 THEN dst[3] := CHR(t) ELSE dst[3] := CHR(255) END;
+				SYSTEM.PUT16(dadr, ASH(ORD(dst[b]), -3) + ASH(ASH(ORD(dst[g]), -2), 5) + ASH(ASH(ORD(dst[r]), -3), 11));
+			END;
+			INC(dadr, 2); INC(sadr, 4); DEC(len)
+		END
+	END BGRA8888Over565;
+
+
+(*	PROCEDURE MMXBGRA8888Over565(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE {SYSTEM.i386, SYSTEM.MMX}
+		PUSHFD
+		; CLI
+		MOV 		ESI, [EBP+sadr]
+		MOV 		EDI, [EBP+dadr]
+		PXOR 		MMX0, MMX0
+		PXOR 		MMX1, MMX1
+
+		MOV		EAX, 0FFFFFFFFH
+		MOVD		MMX7, EAX
+		PUNPCKLBW MMX7, MMX0  ; 00FF00FF00FF00FF
+
+		MOV		ECX, [EBP+len]
+	loop:
+		CMP		ECX, 0
+		JE 			end
+
+		MOV		EAX, [ESI]
+		XOR		EBX, EBX
+		MOV		BX, [EDI]
+
+		; 255 - alpha
+		MOV		EDX, EAX
+		SHR		EDX, 24
+		CMP		EDX, 0
+		JE			empty
+		CMP		EDX, 255
+		JE			full
+
+	alpha:
+		NEG		EDX
+		ADD		EDX, 255
+		MOVD 		MMX6, EDX
+		PUNPCKLWD MMX6, MMX6
+		PUNPCKLDQ MMX6, MMX6
+
+		MOVD 		MMX1, EAX
+
+		; unpack dst
+		MOV		EDX, EBX ; b
+		SHL			EDX, 3
+		AND		EDX, 0F8H
+		MOV		EAX, EDX
+
+		MOV		EDX, EBX ; g
+		SHL			EDX, 5
+		AND		EDX, 0FC00H
+		OR			EAX, EDX
+
+		MOV		EDX, EBX ; r
+		SHL			EDX, 8
+		AND		EDX, 0F80000H
+		OR			EAX, EDX
+
+		MOVD		MMX2, EAX
+
+		PUNPCKLBW  MMX1, MMX0  ; 0000ARGB --> 0A0R0G0B
+		PMULLW  	MMX1, MMX7
+
+		PUNPCKLBW  MMX2, MMX0  ; 0000ARGB --> 0A0R0G0B
+		PMULLW 	MMX2, MMX6
+
+		PADDUSW 	MMX1, MMX2
+
+		;	PSRLW	MMX1, 8 ; normalize
+		DB 			0FH, 71H, 0D1H, 08H
+		PACKUSWB MMX1, MMX0
+
+		; HUGA BIMBO Muell
+		MOVD		EAX, MMX1
+
+	full:
+;			XOR EDX, EDX
+;			SHR EAX, 3
+;			MOV EDX, EAX
+;			AND EDX, 1FH
+;			SHR EAX, 2
+;			AND EAX, 0FFFFFFE0H
+;			OR EDX, EAX
+;			AND EDX, 7FFH
+;
+;			SHR EAX, 3
+;			AND EAX,
+;
+;
+;			SHR AL, 3
+;			SHR AH, 2
+;			MOV EDX, EAX
+;			SHR EAX, 3
+;			AND EAX, 01F0000H
+;			OR EDX, EAX
+;			AND EDX, 01F3F1FH
+
+
+		MOV		EBX, EAX
+		AND		EBX, 0FFH
+		SHR		EBX, 3
+		MOV		EDX, EBX
+
+		MOV		EBX, EAX
+		SHR		EBX, 8
+		AND		EBX, 0FFH
+		SHR		EBX, 2
+		SHL			EBX, 5
+		OR			EDX, EBX
+
+		MOV		EBX, EAX
+		SHR		EBX, 16
+		AND		EBX, 0FFH
+		SHR		EBX, 3
+		SHL			EBX, 11
+		OR			EDX, EBX
+
+		MOV 		[EDI], DX
+	empty:
+		ADD 		ESI, 4;
+		ADD 		EDI, 2;
+		DEC		ECX
+		JMP 		loop
+	end:
+		EMMS ; declare FPU registers free
+		POPFD
+	END MMXBGRA8888Over565;*)
+
+
+(*	PROCEDURE SSE2BGRA8888Over565(VAR mode: Mode0; sadr: ADDRESS; sbit: LONGINT; dadr: ADDRESS; dbit, len: LONGINT);
+	CODE {SYSTEM.i386, SYSTEM.MMX, SYSTEM.SSE, SYSTEM.SSE2}
+		PUSHFD
+		PUSH 		EBX
+		; CLI
+
+		PXOR 		MMX0, MMX0
+		PXOR 		MMX1, MMX1
+		PXOR 		MMX2, MMX2
+		PXOR 		MMX3, MMX3
+		PXOR 		MMX4, MMX4
+		PXOR 		MMX5, MMX5
+		PXOR 		MMX6, MMX6
+		PXOR 		MMX7, MMX7
+
+		PXOR 		XMM3, XMM3
+		PXOR 		XMM4, XMM4
+
+		MOV 		ESI, [EBP+sadr]
+		MOV 		EDI, [EBP+dadr]
+		MOV 		ECX, [EBP+len]
+
+		;  create masks
+
+		; src only FF which is rotated -> MMX3
+		MOV		EAX, 0000000FFH
+		MOVD		MMX3, EAX
+
+		; dest red -> MMX4
+		MOV		EAX, 0F800F800H
+		MOVD		MMX4, EAX
+
+		; dest green -> MMX5
+		MOV		EAX, 07E007E0H
+		MOVD		MMX5, EAX
+
+		; dest blue -> MMX6 ; moved as MMX6 is used in singlepixel
+		; MOV	 	EAX, 001F001FH
+		; MOVD		MMX6, EAX
+
+		; BEGIN
+		; 		WHILE len > 0 DO
+
+	loop:
+		CMP 		ECX,0
+		JE 			end ; jump to end if ECX = 0
+
+		; if len < 8 then do one pixel at the time
+		CMP		ECX, 8
+		JL 			singlepixel
+		; else
+		; take 8 at the time
+
+		MOV		EBX, ESI
+		AND 		EBX, 0FH
+		CMP		EBX, 0
+		JNE 		singlepixel
+
+	alleightpixels:
+		; dest blue -> MMX6
+		MOV		EAX, 001F001FH
+		MOVD		MMX6, EAX
+
+		; src := SYSTEM.VAL(Pixel, SYSTEM.GET32(sadr));
+		; Load data into memory
+		;MOV 	XMM4, 0FF000000FF000000FF000000FF000000H
+
+		MOVDQA 	XMM2, [ESI] ;src 5-8
+		MOVQ2DQ	XMM4,  MMX3     ; 000000000000000000000000000000FFH
+		MOVDQA 	XMM1, [ESI+16]  ;src 1-4
+		PREFETCHNTA		[ESI+32] ; prepare src 9-15
+
+		; get alphas
+		MOVDQU 	XMM6, XMM2
+		PSHUFD		XMM4, XMM4, 0
+		MOVDQU 	XMM5, XMM1
+		PSLLD 		XMM4, 24
+		PAND 		XMM6, XMM4 ; alpha 5-8 in XMM6
+		PAND 		XMM5, XMM4  ; alpha 1-4 in XMM5
+		PSRLD 		XMM5, 24
+		PSHUFHW 	XMM5, XMM5, 85H
+		PSRLD 		XMM6, 24
+
+		; put both alphas into 1 register
+		PSHUFHW 	XMM6, XMM6, 85H
+		PSHUFLW 	XMM5, XMM5, 85H
+		PSHUFLW 	XMM6, XMM6, 58H
+		PSHUFD		XMM5, XMM5, 0D0H  ; 0102030400000000
+		PSHUFD 	XMM6, XMM6, 5CH ; 0000000005060708
+		PXOR 		XMM0,XMM0
+		POR		XMM5, XMM6            ; XMM5 = alphas 0102030405060708
+
+		PCMPEQD 	XMM0, XMM5
+		PMOVMSKB EAX, XMM0
+		CMP 		EAX, 0FFFFH ; all alphas = zero; TEST not possible, because only 8 bits compared
+		JE      		endloop
+
+		; mask out alpha = zero
+
+		; fd := 255-ORD(src[a]); fd = XMM4
+		; MOV 	XMM4, 00FF00FF00FF00FF00FF00FF00FF00FFH
+		PXOR 		XMM4, XMM4
+		MOV	 	EAX, 00FFH
+		PINSRW	XMM4, EAX ,0
+		PSHUFLW 	XMM4, XMM4, 0
+		PSHUFD 	XMM4, XMM4, 0
+		PSUBW 		XMM4, XMM5
+		MOV 		EAX,1H
+		PINSRW	XMM3, EAX ,0
+		PSHUFLW 	XMM3, XMM3, 0
+		PSHUFD 	XMM3, XMM3, 0
+		PADDUSW 	XMM4, XMM3
+
+		; new red
+		; calculate red 2
+
+		; get source
+
+		; sred14 = src14 && (srcMask <<16)
+		; srcMask << 16
+		MOVQ2DQ 	XMM3, MMX3
+		PSHUFD 	XMM3, XMM3, 0
+		MOVDQU 	XMM5, XMM1
+		MOVDQU 	XMM6, XMM2
+		PSLLD 		XMM3, 16
+
+		; sred14 = src14 && (srcMask << 24)
+		; src14 must be copied because it mustn't be changed
+		PAND 		XMM5, XMM3 ; sred14
+		PSRLD 		XMM5, 16
+
+		; sred14s = shuffled sred14
+		PSHUFHW 	XMM5, XMM5,85H
+		PAND 		XMM6, XMM3 ; sred58
+		PSRLD 		XMM6, 16
+
+		PSHUFLW 	XMM5, XMM5,85H
+		PSHUFHW 	XMM6, XMM6,85H
+		PSHUFD  	XMM5, XMM5,0D0H ; sred14s
+		PSHUFLW 	XMM6, XMM6,58H
+		PSHUFD  	XMM6, XMM6,5CH ; sred58s
+		POR 		XMM5, XMM6 ; sred18
+
+		; sred18255 = sred18 * 256- sred18
+		MOVDQU 	XMM7, XMM5
+		PSLLW 		XMM5, 8
+		PSUBUSW 	XMM5, XMM7 ; sred18255
+
+		; src is now ready
+
+		; destination
+		; dest18 must be copied because it mustn't be changed
+		; Load data into memory
+		MOVDQU 	XMM3, [EDI]  ;dest 1-8
+		MOVQ2DQ 	XMM6, MMX4
+		PSHUFD 	XMM6, XMM6, 0
+		MOVDQU 	XMM7, XMM3
+
+		PAND 		XMM7, XMM6 ; dred18
+		PSRLW 		XMM7, 8
+		;  dred18alpha = dred18 * negalpha
+		PMULLW XMM7, XMM4 ; dred18alpha
+
+		; dest is prepared
+		; combining dest and src
+
+		; dred18big = sred18255 + dred18alpha
+		PADDUSW 	XMM7, XMM5 ; dred18big
+		; dred18f = dred18big && destMaskred128  because >> 11 and << 11 is && mask
+		PAND 		XMM7, XMM6 ; dred18f
+
+ 		; dest18nr0 = dest18 && (~destMaskred128)
+ 		PANDN 	XMM6, XMM3  ; dest18nr0
+
+ 		; dest18nrf = dest18nr0 || dred18f
+ 		POR 		XMM6, XMM7
+
+		MOVDQU 	XMM3, XMM6
+
+		; red is calculated
+
+		; calculate green:
+		; get source
+
+		; sgreen14 = src14 && (srcMask <<8)
+		; srcMask << 8
+		MOVQ2DQ 	XMM7, MMX3
+
+		PSHUFD 	XMM7, XMM7, 0
+		MOVDQU 	XMM5, XMM1
+		PSLLD 		XMM7, 8
+		PAND 		XMM5, XMM7 ; sgreen14
+		PSRLD 		XMM5, 8
+
+		; sgreen14s = shuffled sgreen14
+		PSHUFHW 	XMM5, XMM5,85H
+		MOVDQU 	XMM6, XMM2
+		PSHUFLW 	XMM5, XMM5,85H
+		PAND 		XMM6, XMM7 ; sgreen58
+		PSRLD 		XMM6, 8
+		PSHUFD  	XMM5, XMM5,0D0H ; sgreen14s
+
+		; sgreen58 = src58&& (srcMask << 8)
+		; src58 must be copied because it mustn't be changed
+
+		; sgreen58s = shuffled sgreen58
+		PSHUFHW 	XMM6, XMM6,85H
+		PSHUFLW 	XMM6, XMM6,58H
+		PSHUFD  	XMM6, XMM6,5CH ; sgreen58s
+
+		; sgreen18 = sgreen14s || sgreen58s
+		POR 		XMM5, XMM6 ; sgreen18
+
+		; sgreen18255 = sgreen18 * 256- sgreen18
+		MOVDQU 	XMM7, XMM5
+		MOVQ2DQ	XMM6, MMX5
+
+		PSLLW 		XMM5, 8
+		PSUBUSW 	XMM5, XMM7 ; sgreen18255
+		PSHUFD 	XMM6, XMM6, 0
+
+		MOVDQU 	XMM7, XMM3
+
+		PAND 		XMM7, XMM6 ; dgreen18
+		PSRLW 		XMM7,3
+		;  dgreen18alpha = dgreen18 * negalpha
+		PMULLW 	XMM7, XMM4 ; dgreen18alpha
+
+		; dest is prepared
+		; combining dest and src
+
+		; dgreen18big = sgreen18255 + dgreen18alpha
+
+	 	PADDUSW 	XMM7, XMM5 ; dgreen18big
+		PANDN 	XMM6, XMM3  ; dest18ng0
+
+		; dgreen18f = (dgreen18big >> 11) <<5
+
+		PSRLW 		XMM7, 10 ; dgreen18f
+		PSLLW 		XMM7, 5
+
+ 		; dest18ng0 = dest18 && (~destMaskgreen128)
+
+  		; dest18ngf = dest18ng0 || dred18f
+ 		POR 		XMM6, XMM7
+		MOVDQU 	XMM3, XMM6
+
+		; green is calculated
+
+		; calculate blue
+		; get source
+
+		; sblue14 = src14 && (srcMask)
+		; srcMask
+		MOVQ2DQ 	XMM7, MMX3
+		MOVDQU 	XMM5, XMM1
+
+		PSHUFD 	XMM7, XMM7, 0
+		MOVDQU 	XMM6, XMM2
+
+		; sblue14 = src14 && (srcMask)
+		; src14 must be copied because it mustn't be changed
+		PAND 		XMM5, XMM7 ; sblue14
+
+		; sblue14s = shuffled sblue14
+		PSHUFHW 	XMM5, XMM5,85H
+		PAND 		XMM6, XMM7 ; sblue58
+		PSHUFHW 	XMM6, XMM6,85H
+
+		PSHUFLW 	XMM5, XMM5,85H
+		PSHUFLW 	XMM6, XMM6,58H
+
+		PSHUFD  	XMM5, XMM5,0D0H ; sblue14s
+		PSHUFD  	XMM6, XMM6,5CH ; sblue58s
+
+		POR 		XMM5, XMM6 ; sblue18
+
+		; sblue18255 = sblue18 * 256- sblue18
+		MOVDQU 	XMM7, XMM5
+		PSLLW 		XMM5, 8
+		PSUBUSW 	XMM5, XMM7 ; sblue18255
+		MOVQ2DQ	XMM6, MMX6
+		PSHUFD 	XMM6, XMM6, 0
+		MOVDQU 	XMM7, XMM3
+		PAND 		XMM7, XMM6 ; dblue18
+		PSLLW 		XMM7, 3
+
+		PMULLW 	XMM7, XMM4 ; dblue18alpha
+
+		; dest is prepared
+		; combining dest and src
+
+		; dblue18big = sblue18255 + dblue18alpha
+
+	 	PADDUSW 	XMM7, XMM5 ; dblue18big
+		; dblue18f = (dblue18big >> 11)
+		PANDN 	XMM6, XMM3  ; dest18nr0
+ 		PSRLW 		XMM7, 11 ; dblue18f
+
+  		; dest18nr0 = dest18 && (~destMaskblue128)
+
+  		; dest18nbf = dest18nb0 || dblue18f
+ 		POR 		XMM6, XMM7
+		MOVDQU 	XMM3, XMM6
+
+		; blue is calculated
+
+		; now dest is calculated, store it
+		; get 0 stuff
+		MOVDQU	XMM5, [EDI]
+		PAND		XMM5,XMM0
+		PANDN		XMM0, XMM3
+		POR		XMM0, XMM5
+
+		MOVDQU 	[EDI],XMM0
+		PREFETCHNTA		[EDI+16] ; prepare dest 9-15
+	endloop:
+		ADD 		ESI, 32 ; num of bytes
+		ADD 		EDI, 16
+		SUB 		ECX, 8
+		JMP 		loop
+
+	singlepixel: ; original code from MMXBGRA8888Over565, adjusted to fit this procedure
+		MOV	 	EAX, 0FFFFFFFFH
+		MOVD		MMX7, EAX
+		PUNPCKLBW  MMX7, MMX0  ; 00FF00FF00FF00FF
+
+		MOV 		EAX,[ESI]
+		XOR		EBX, EBX
+		MOV 		BX,	[EDI]
+
+		; 255 - alpha
+		MOV		EDX, EAX
+		SHR		EDX, 24
+
+		CMP		EDX, 0
+		JE			empty
+		CMP		EDX, 255
+		JE			full
+
+	alpha:
+		NEG		EDX
+		ADD		EDX, 255
+
+		MOVD 		MMX6, EDX
+		PUNPCKLWD MMX6, MMX6
+		PUNPCKLDQ MMX6, MMX6
+
+		MOVD 		MMX1, EAX
+		; unpack dst
+		MOV		EDX, EBX ; b
+		SHL			EDX, 3
+		AND		EDX, 0F8H
+		MOV		EAX, EDX
+
+		MOV		EDX, EBX ; g
+		SHL			EDX, 5
+		AND		EDX, 0FC00H
+		OR			EAX, EDX
+
+		MOV		EDX, EBX ; r
+		SHL			EDX, 8
+		AND		EDX, 0F80000H
+		OR			EAX, EDX
+
+		MOVD		MMX2, EAX
+		PUNPCKLBW MMX1, MMX0  ; 0000ARGB --> 0A0R0G0B
+		PMULLW 	MMX1, MMX7
+		PUNPCKLBW MMX2, MMX0  ; 0000ARGB --> 0A0R0G0B
+		PMULLW 	MMX2, MMX6
+		PADDUSW 	MMX1, MMX2
+
+	;	PSRLW	MMX1, 8 ; normalize
+		DB 			0FH, 71H, 0D1H, 08H
+		PACKUSWB 	MMX1, MMX0
+
+		; HUGA BIMBO Muell
+		MOVD		EAX, MMX1
+
+	full:
+		MOV		EBX, EAX
+		AND		EBX, 0FFH
+		SHR		EBX, 3
+		MOV		EDX, EBX
+
+		MOV		EBX, EAX
+		SHR		EBX, 8
+		AND		EBX, 0FFH
+		SHR		EBX, 2
+		SHL			EBX, 5
+		OR			EDX, EBX
+
+		MOV		EBX, EAX
+		SHR		EBX, 16
+		AND		EBX, 0FFH
+		SHR		EBX, 3
+		SHL			EBX, 11
+		OR			EDX, EBX
+
+		MOV 		[EDI], DX
+
+	empty:
+		ADD 		ESI, 4;
+		ADD 		EDI, 2;
+		DEC		ECX
+		JMP 		loop
+
+	end:
+		EMMS ; declare FPU registers free
+		POP 		EBX
+		POPFD
+
+	END SSE2BGRA8888Over565;*)
+
+
+	(** find (optimized) pixel transfer procedure for transfer mode and given source and destination formats **)
+	PROCEDURE Bind* (VAR mode: Mode; VAR src, dst: Format);
+		VAR op: LONGINT; val,i: LONGINT;
+	BEGIN
+		IF Same(src, mode.src) & Same(dst, mode.dst) THEN
+			ASSERT(mode.transfer # NIL, 120);
+			RETURN	(* assume transfer procedure is still valid *)
+		END;
+		mode.src := src; mode.dst := dst; mode.buf := mode.col;
+		IF  (src.pal # NIL) & ((mode.map = NIL) OR (LEN(mode.map^) # src.pal.used) ) THEN
+			NEW(mode.map, src.pal.used)
+		END;
+
+
+		(* try to convert complex composite operations into simpler ones *)
+		IF alpha IN src.components * dst.components THEN	(* source and destination contain alpha information *)
+			op := mode.op
+		ELSIF alpha IN src.components THEN	(* only source contains alpha *)
+			CASE mode.op OF
+			| dstOverSrc: op := dstCopy
+			| srcInDst: op := srcCopy
+			| srcWithoutDst: op := clear
+			| srcAtopDst: op := srcOverDst
+			| dstAtopSrc: op := dstInSrc
+			| srcXorDst: op := dstWithoutSrc
+			ELSE op := mode.op
+			END
+		ELSIF alpha IN dst.components THEN	(* only destination contains alpha *)
+			CASE mode.op OF
+			| srcOverDst: op := srcCopy
+			| dstInSrc: op := dstCopy
+			| dstWithoutSrc: op := clear
+			| srcAtopDst: op := srcInDst
+			| dstAtopSrc: op := dstOverSrc
+			| srcXorDst: op := srcWithoutDst
+			ELSE op := mode.op
+			END
+		ELSE	(* no alpha in either source or destination *)
+			CASE mode.op OF
+			| srcOverDst, srcInDst, srcAtopDst: op := srcCopy
+			| dstOverSrc, dstInSrc, dstAtopSrc: op := dstCopy
+			| srcWithoutDst, dstWithoutSrc, srcXorDst: op := clear
+			ELSE op := mode.op
+			END
+		END;
+
+		IF op = InvDst THEN
+			mode.transfer:=InvAny;
+		ELSIF op = InvOverDst THEN
+			mode.transfer:=InvOverAny;
+		ELSIF op = clear THEN
+			CASE dst.code OF
+			| a1: mode.transfer := Clear1
+			| a8, bgr555, bgr565, bgr466, bgr888(*, bgra8888*): mode.transfer := ClearBytes
+			| bgra8888: mode.transfer:=Clear32;
+			| p8:
+				mode.buf[0] := CHR(CLUTs.Match(dst.pal.clut, 0));
+				IF mode.buf[0] = 0X THEN mode.transfer := ClearBytes
+				ELSE mode.transfer := ConstCopy8
+				END
+			| d8:
+				mode.buf[0] := CHR(ColorToIndex(0));
+				IF mode.buf[0] = 0X THEN mode.transfer := ClearBytes
+				ELSE mode.transfer := ConstCopy8
+				END
+			| p16:
+				val:=CLUTs.Match(dst.pal.clut, 0);
+				IF dst.pal.used>256 THEN val:=val*dst.pal.used DIV 256 END;
+				SYSTEM.PUT16(ADDRESSOF(mode.buf[0]),SHORT(val)); (*PH090122*)
+				(*mode.buf[0] := CHR(PaletteIndex(dst.pal, 0, 0, 0));*)
+				IF val = 0 THEN mode.transfer := ClearBytes
+				ELSE mode.transfer := ConstCopy16
+				END
+			ELSE mode.transfer := ClearAny
+			END
+
+		ELSIF op = srcCopy THEN
+			CASE dst.code OF
+			| a1:
+				CASE src.code OF
+				| a1: mode.transfer := Copy1
+				| a8: mode.transfer := A8CopyA1
+				| bgra8888: mode.transfer := BGRA8888CopyA1
+				ELSE
+					IF alpha IN src.components THEN mode.transfer := AnyCopyA1
+					ELSE mode.transfer := Set1
+					END
+				END
+			| a8:
+				CASE src.code OF
+				| a1: mode.transfer := A1CopyA8
+				| a8: mode.transfer := Copy8
+				| bgra8888: mode.transfer := BGRA8888CopyA8
+				ELSE
+					IF alpha IN src.components THEN mode.transfer := AnyCopyA8
+					ELSE mode.buf[0] := 0FFX; mode.transfer := ConstCopy8
+					END
+				END
+			| p8:
+				CASE src.code OF
+				| a1, a8:
+					mode.buf[0] := CHR(CLUTs.Match(dst.pal.clut, ORD(mode.col[b]) + ASH(ORD(mode.col[g]), 8) +
+						ASH(ORD(mode.col[r]), 16)));
+					mode.transfer := ConstCopy8
+				| p8:
+					IF src.pal = dst.pal THEN mode.transfer := Copy8
+					ELSE
+						FOR i := 0 TO src.pal.used-1 DO
+							mode.map[i] := SHORT(CLUTs.Match(dst.pal.clut, ORD(src.pal.col[i, b]) + ASH(ORD(src.pal.col[i, g]), 8) +
+								ASH(ORD(src.pal.col[i, r]), 16)))
+						END;
+						mode.transfer := I8CopyI8
+					END
+				| d8:
+					FOR i := 0 TO 255 DO
+						mode.map[i] := SHORT(CLUTs.Match(dst.pal.clut, IndexToColor(i) MOD 1000000H))
+					END;
+					mode.transfer := I8CopyI8
+				| bgr555, bgr565, bgr466,p16: mode.transfer := Any16CopyP8
+				| bgr888: mode.transfer := BGR888CopyP8
+				| bgra8888: mode.transfer := BGRA8888CopyP8
+				ELSE mode.transfer := AnyCopyP8
+				END
+			| d8:
+				CASE src.code OF
+				| a1, a8:
+					mode.buf[0] := CHR(ColorToIndex(
+						ORD(mode.col[b]) + ASH(ORD(mode.col[g]), 8) + ASH(ORD(mode.col[r]), 16)));
+					mode.transfer := ConstCopy8
+				| p8:
+					FOR i := 0 TO src.pal.used-1 DO
+						mode.map[i] := SHORT(ColorToIndex(
+							ORD(src.pal.col[i, b]) + ASH(ORD(src.pal.col[i, g]), 8) + ASH(ORD(src.pal.col[i, r]), 16)))
+					END;
+					mode.transfer := I8CopyI8
+				| d8: mode.transfer := Copy8
+				| bgr555, bgr565, bgr466, p16: mode.transfer := Any16CopyD8
+				| bgr888: mode.transfer := BGR888CopyD8
+				| bgra8888: mode.transfer := BGRA8888CopyD8
+				ELSE mode.transfer := AnyCopyD8
+				END
+			| p16:
+				CASE src.code OF
+				| a1, a8: dst.pack(dst, ADDRESSOF(mode.buf[0]), 0, mode.col); mode.transfer := ConstCopy16
+				| p8: mode.transfer := P8CopyAny16
+				| d8: mode.transfer := D8CopyAny16
+				| p16:
+					IF src.pal = dst.pal THEN mode.transfer := Copy16
+					ELSE
+						FOR i := 0 TO src.pal.used-1 DO
+							val:=CLUTs.Match(dst.pal.clut, ORD(src.pal.col[i, b]) + ASH(ORD(src.pal.col[i, g]), 8) +
+								ASH(ORD(src.pal.col[i, r]), 16));
+							IF dst.pal.used>256 THEN val := val * dst.pal.used DIV 256 END;
+							mode.map[i] := SHORT(val)
+						END;
+						mode.transfer := I16CopyI16
+					END
+				| bgr555, bgr565, bgr466:  mode.transfer := Any16CopyAny16
+				| bgr888: mode.transfer := BGR888CopyAny16
+				| bgra8888: mode.transfer := BGRA8888CopyAny16
+				ELSE mode.transfer := AnyCopyAny16
+				END;
+			| bgr555, bgr565, bgr466:
+				CASE src.code OF
+				| a1, a8: dst.pack(dst, ADDRESSOF(mode.buf[0]), 0, mode.col); mode.transfer := ConstCopy16
+				| p8: mode.transfer := P8CopyAny16
+				| d8: mode.transfer := D8CopyAny16
+				| bgr555, bgr565, bgr466,p16:
+					IF src.code = dst.code THEN mode.transfer := Copy16
+					ELSE mode.transfer := Any16CopyAny16
+					END
+				| bgr888: mode.transfer := BGR888CopyAny16
+				| bgra8888: mode.transfer := BGRA8888CopyAny16
+				ELSE mode.transfer := AnyCopyAny16
+				END;
+			| bgr888:
+				CASE src.code OF
+				| a1, a8: mode.buf := mode.col; mode.transfer := ConstCopy24
+				| p8: mode.transfer := P8CopyBGR888
+				| d8: mode.transfer := D8CopyBGR888
+				| p16: mode.transfer := P16CopyBGR888	(*PH090122*)
+				| bgr555, bgr565, bgr466: mode.transfer := Any16CopyBGR888
+				| bgr888: mode.transfer := Copy24
+				| bgra8888: 
+				(*	IF SSE2enabled THEN mode.transfer := SSE2BGRA8888CopyBGR888
+					ELSE *) mode.transfer := BGRA8888CopyBGR888
+				(*	END*);
+				ELSE mode.transfer := AnyCopyBGR888
+				END
+			| bgra8888:
+				CASE src.code OF
+				| a1: mode.transfer := A1CopyBGRA8888
+				| a8: mode.transfer := A8CopyBGRA8888
+				| p8: mode.transfer := P8CopyBGRA8888
+				| d8: mode.transfer := D8CopyBGRA8888
+				| p16: mode.transfer := P16CopyBGRA8888	(*PH090122*)
+				| bgr555, bgr565, bgr466: mode.transfer := Any16CopyBGRA8888
+				| bgr888:  	(*IF SSE2enabled THEN mode.transfer := SSE2BGR888CopyBGRA8888*)
+							(*ELSE*) mode.transfer :=BGR888CopyBGRA8888;
+							(*END;*)
+				| bgra8888: mode.transfer := Copy32
+				ELSE mode.transfer := AnyCopyBGRA8888
+				END
+			ELSE
+				CASE src.code OF
+				| a1: mode.transfer := A1CopyAny
+				| p8: mode.transfer := P8CopyAny
+				| d8: mode.transfer := D8CopyAny
+				| bgr555, bgr565, bgr466,p16: mode.transfer := Any16CopyAny
+				| bgr888: mode.transfer := BGR888CopyAny
+				| bgra8888: mode.transfer := BGRA8888CopyAny
+				ELSE
+					IF (src.bpp MOD 8 = 0) & (dst.bpp MOD 8 = 0) THEN mode.transfer := AnyBytesCopyAnyBytes
+					ELSE mode.transfer := AnyCopyAny
+					END
+				END
+			END
+
+		ELSIF op = dstOverSrc THEN
+			mode.transfer := EmptyTransfer
+
+		ELSIF op = srcOverDst THEN
+			CASE dst.code OF
+			| a1:
+				CASE src.code OF
+				| a1: mode.transfer := A1OverA1
+				| a8: mode.transfer := A8OverA1
+				| bgra8888: mode.transfer := BGRA8888OverA1
+				ELSE mode.transfer := AnyOverA1
+				END
+			| a8:
+				CASE src.code OF
+				| a1: mode.buf[0] := 0FFX; mode.transfer := A1OverConst8
+				| a8: mode.transfer := A8OverA8
+				| bgra8888: mode.transfer := BGRA8888OverA8
+				ELSE mode.transfer := AnyOverA8
+				END
+			| bgra8888:
+
+				CASE src.code OF
+				| a1: mode.buf := mode.col; mode.transfer := A1OverConst32
+				| a8: mode.buf := mode.col; mode.transfer := A8OverAny;
+					IF mode.op = srcOverDst THEN mode.transfer := A8OverBGRA8888 END;
+				| bgra8888: mode.transfer := BGRA8888OverBGRA8888
+				ELSE mode.transfer := BGRA8888OverAny; (* ? *)
+				END
+			ELSE
+				CASE src.code OF
+				| a1:
+					CASE dst.code OF
+					| p8:
+						mode.buf[0] := CHR(CLUTs.Match(dst.pal.clut, ORD(mode.col[b]) + ASH(ORD(mode.col[g]), 8) +
+							ASH(ORD(mode.col[r]), 16)));
+						mode.transfer := A1OverConst8
+					| d8:
+						mode.buf[0] := CHR(ColorToIndex(ORD(mode.col[b]) + ASH(ORD(mode.col[g]), 8) +
+							ASH(ORD(mode.col[r]), 16)));
+						mode.transfer := A1OverConst8
+					| p16: (* this is probably not correct ... *)
+						mode.buf[0] := CHR(PaletteIndex(dst.pal, ORD(mode.col[r]), ORD(mode.col[g]), ORD(mode.col[b])));
+						mode.transfer := A1OverConst16
+					| bgr555, bgr565, bgr466: dst.pack(dst, ADDRESSOF(mode.buf[0]), 0, mode.col); mode.transfer := A1OverConst16
+					| bgr888: mode.buf := mode.col; mode.transfer := A1OverConst24
+					ELSE mode.transfer := A1OverAny
+					END
+				| a8: mode.buf := mode.col; mode.transfer := A8OverAny
+				| bgra8888:
+					CASE dst.code OF
+					| bgr555, bgr466, p16: mode.transfer := BGRA8888OverAny16
+					| bgr565 : (*IF MMXenabled THEN
+							mode.transfer := MMXBGRA8888Over565;
+							IF SSE2enabled THEN mode.transfer := SSE2BGRA8888Over565; END;
+						ELSE*) mode.transfer := BGRA8888Over565
+						(*END*)
+					ELSE mode.transfer := BGRA8888OverAny
+					END
+				ELSE
+					mode.transfer := AnyBlendAny
+				END
+			END
+		ELSE
+			mode.transfer := AnyBlendAny
+		END;
+
+		ASSERT(mode.transfer # NIL, 120)
+	END Bind;
+
+
+	(**--- Image Operations ---**)
+
+	(** get pixel from image **)
+	PROCEDURE Get* (img: Image; x, y: LONGINT; VAR pix: Pixel; VAR mode: Mode);
+		VAR bit: LONGINT; adr: ADDRESS;
+	BEGIN
+		ASSERT((0 <= x) & (x < img.width) & (0 <= y) & (y < img.height), 100);
+		bit := x * img.fmt.bpp; adr := img.adr + y * img.bpr + bit DIV 8; bit := bit MOD 8;
+		Bind(mode, img.fmt, PixelFormat);
+		(*mode.transfer(mode, adr, bit, ADDRESSOF(pix), 0, 1)*)
+
+		mode.transfer(mode, adr, bit, ADDRESSOF(pix[0]), 0, 1) (*PH090122*)
+	END Get;
+
+	(** put pixel into image **)
+	PROCEDURE Put* (img: Image; x, y: LONGINT; CONST pix: Pixel; VAR mode: Mode);
+		VAR bit: LONGINT; adr: ADDRESS;
+	BEGIN
+		(*ASSERT((0 <= x) & (x < img.width) & (0 <= y) & (y < img.height), 100);*) (*avoid a HALT if roundoff errors from higher levels occur here*)
+		IF (0 > x) OR (x >= img.width) OR (0 > y) OR (y >= img.height) THEN RETURN END;
+		bit := x * img.fmt.bpp; adr := img.adr + y * img.bpr + bit DIV 8; bit := bit MOD 8;
+		Bind(mode, PixelFormat, img.fmt);
+		mode.transfer(mode, ADDRESSOF(pix[0]), 0, adr, bit, 1)
+	END Put;
+
+	(** fill rectangular area **)
+	PROCEDURE Fill* (img: Image; llx, lly, urx, ury: LONGINT; CONST pix: Pixel; VAR mode: Mode);
+		VAR bit, bb, x, c, t: LONGINT; m: Mode; adr, aa: ADDRESS;
+	BEGIN
+		ASSERT((0 <= llx) & (llx < urx) & (urx <= img.width) & (0 <= lly) & (lly < ury) & (ury <= img.height), 100);
+		bit := llx * img.fmt.bpp; adr := img.adr + lly * img.bpr + bit DIV 8; bit := bit MOD 8;
+
+		IF (mode.op = srcCopy) & (img.fmt.code IN {bgr565}) THEN (* shortcut for speed in important cases *)
+			c := ASH(ORD(pix[b]), -3) + ASH(ASH(ORD(pix[g]), -2), 5) + ASH(ASH(ORD(pix[r]), -3), 11);
+			t := urx - llx;
+			WHILE lly < ury DO
+				Fill16(adr, t, c);
+				INC(lly); INC(adr, img.bpr)
+			END
+		ELSIF (mode.op = srcCopy) & (img.fmt.code IN {}) THEN
+			c := ASH(ORD(pix[r]), 24) + ASH(ORD(pix[g]), 16) + ASH(ORD(pix[b]), 8) + ORD(pix[a]);
+			t := urx - llx;
+			WHILE lly < ury DO
+				Fill32(adr, t, c);
+				INC(lly); INC(adr, img.bpr)
+			END
+		ELSE
+			Bind(mode, PixelFormat, img.fmt);
+			IF (mode.op IN {clear, srcCopy}) OR (pix[a] = 0FFX) & (mode.op IN {srcOverDst, dstWithoutSrc}) THEN	(* dst is replaced *)
+				(* copy one pixel to lower left corner of rect *)
+				mode.transfer(mode, ADDRESSOF(pix[0]), 0, adr, bit, 1);
+
+				(* copy pixel to rest of bottom row *)
+				InitMode(m, srcCopy); Bind(m, img.fmt, img.fmt);
+				IF (bit = 0) & (img.fmt.bpp MOD 8 = 0) THEN	(* use simple address calculation *)
+					bb := img.fmt.bpp DIV 8; aa := adr + bb; x := llx+1;
+					WHILE x < urx DO
+						m.transfer(m, adr, 0, aa, 0, 1);
+						INC(aa, bb); INC(x)
+					END
+				ELSE
+					bb := bit + img.fmt.bpp; aa := adr + bb DIV 8; bb := bb MOD 8; x := llx+1;
+					WHILE x < urx DO
+						m.transfer(m, adr, bit, aa, bb, 1);
+						bb := bb + img.fmt.bpp; aa := aa + bb DIV 8; bb := bb MOD 8; INC(x)
+					END
+				END;
+
+				(* now copy bottom row to others *)
+				INC(lly); aa := adr + img.bpr;
+				WHILE lly < ury DO
+					m.transfer(m, adr, bit, aa, bit, urx - llx);
+					INC(lly); INC(aa, img.bpr)
+				END
+
+			ELSE	(* fill pixel by pixel *)
+				WHILE lly < ury DO
+					x := llx; aa := adr; bb := bit;
+					WHILE x < urx DO
+						mode.transfer(mode, ADDRESSOF(pix[0]), 0, aa, bb, 1);
+						bb := bb + img.fmt.bpp; aa := aa + bb DIV 8; bb := bb MOD 8; INC(x)
+					END;
+					INC(lly); INC(adr, img.bpr)
+				END
+			END
+		END
+	END Fill;
+
+	(** clear image **)
+	PROCEDURE Clear* (img: Image);
+		VAR mode: Mode;
+	BEGIN
+		InitMode(mode, clear);
+		Bind(mode, PixelFormat, img.fmt);
+		Fill(img, 0, 0, img.width, img.height, Zero, mode)
+	END Clear;
+
+	(** get several pixels and store them in array in requested format **)
+	PROCEDURE GetPixels* (img: Image; x, y, w: LONGINT; VAR fmt: Format; VAR buf: ARRAY OF CHAR; ofs : LONGINT; VAR mode: Mode);
+		VAR sbit: LONGINT; sadr: ADDRESS;
+	BEGIN
+		ASSERT((0 <= x) & (x + w <= img.width) & (0 <= y) & (y <= img.height), 100);
+		ASSERT(ofs + w * fmt.bpp DIV 8 <= LEN(buf), 101);
+		Bind(mode, img.fmt, fmt);
+		sbit := x * img.fmt.bpp; sadr := img.adr + y * img.bpr + sbit DIV 8; sbit := sbit MOD 8;
+		mode.transfer(mode, sadr, sbit, ADDRESSOF(buf[ofs]), 0, w)
+	END GetPixels;
+
+	(** put several pixels from array in given format into image **)
+	PROCEDURE PutPixels* (img: Image; x, y, w: LONGINT; VAR fmt: Format; VAR buf: ARRAY OF CHAR; ofs : LONGINT; VAR mode: Mode);
+		VAR dbit: LONGINT; dadr: ADDRESS;
+	BEGIN
+		ASSERT((0 <= x) & (x + w <= img.width) & (0 <= y) & (y <= img.height), 100);
+		ASSERT(ofs + w * fmt.bpp DIV 8 <= LEN(buf), 101);
+		dbit := x * img.fmt.bpp; dadr := img.adr + y * img.bpr + dbit DIV 8; dbit := dbit MOD 8;
+		Bind(mode, fmt, img.fmt);
+		mode.transfer(mode, ADDRESSOF(buf[ofs]), 0, dadr, dbit, w)
+	END PutPixels;
+
+	(** copy rectangular area to the same or another image in specified mode **)
+	PROCEDURE Copy* (src, dst: Image; llx, lly, urx, ury, dx, dy: LONGINT; VAR mode: Mode);
+		VAR w, h, sbit, dbit, slen, sb, db, len, l: LONGINT; sadr, dadr, sa, da: ADDRESS;
+	BEGIN
+		ASSERT((0 <= llx) & (llx <= urx) & (urx <= src.width) & (0 <= lly) & (lly <= ury) & (ury <= src.height), 100);
+		ASSERT((0 <= dx) & (dx + urx - llx <= dst.width) & (0 <= dy) & (dy + ury - lly <= dst.height), 101);
+		Bind(mode, src.fmt, dst.fmt);
+		w := urx - llx; h := ury - lly;
+		IF (src # dst) OR (lly > dy) OR (lly = dy) & ((llx > dx) OR (urx <= dx)) THEN	(* copy lines bottom-up *)
+			sbit := llx * src.fmt.bpp;  sadr := src.adr + lly * src.bpr + sbit DIV 8; sbit := sbit MOD 8;
+			dbit := dx * dst.fmt.bpp; dadr := dst.adr + dy * dst.bpr + dbit DIV 8; dbit := dbit MOD 8;
+			WHILE h > 0 DO
+				mode.transfer(mode, sadr, sbit, dadr, dbit, w);
+				INC(sadr, src.bpr); INC(dadr, dst.bpr); DEC(h)
+			END
+		ELSIF lly < dy THEN	(* copy lines top-down *)
+			sbit := llx * src.fmt.bpp; sadr := src.adr + ury * src.bpr + sbit DIV 8;  sbit := sbit MOD 8;
+			dbit := dx * dst.fmt.bpp; dadr := dst.adr + (dy + h) * dst.bpr + dbit DIV 8; dbit := dbit MOD 8;
+			WHILE h > 0 DO
+				DEC(sadr, src.bpr); DEC(dadr, dst.bpr); DEC(h);
+				mode.transfer(mode, sadr, sbit, dadr, dbit, w)
+			END
+		ELSIF llx # dx THEN	(* uh oh! overlapping spans *)
+			slen := dx + w - urx;	(* maximal span length guaranteeing non-overlapping spans *)
+			sbit := urx * src.fmt.bpp; sadr := src.adr + lly * src.bpr + sbit DIV 8; sbit := sbit MOD 8;
+			dbit := (dx + w) * dst.fmt.bpp; dadr := dst.adr + dy * dst.bpr + dbit DIV 8; dbit := dbit MOD 8;
+			WHILE h > 0 DO
+				sa := sadr; sb := sbit; da := dadr; db := dbit; len := w;
+				WHILE len > 0 DO
+					l := slen;
+					IF l > len THEN l := len END;
+					DEC(sb, l * src.fmt.bpp); INC(sa, sb DIV 8); sb := sb MOD 8;
+					DEC(db, l * dst.fmt.bpp); INC(da, db DIV 8); db := db MOD 8;
+					mode.transfer(mode, sa, sb, da, db, l);
+					DEC(len, l)
+				END;
+				INC(sadr, src.bpr); INC(dadr, dst.bpr); DEC(h)
+			END
+		END
+	END Copy;
+
+	(** replicate pattern within rectangular area of image using given mode **)
+	PROCEDURE FillPattern* (pat, dst: Image; llx, lly, urx, ury, px, py: LONGINT; VAR mode: Mode);
+		VAR pw, ph, olx, oby, ilx, olw, irw, dy, sy, dx, sx, ty: LONGINT;
+	BEGIN
+		ASSERT((0 <= llx) & (llx <= urx) & (urx <= dst.width) & (0 <= lly) & (lly <= ury) & (ury <= dst.height), 100);
+		pw := pat.width; ph := pat.height;
+		olx := px + (llx - px) DIV pw * pw;
+		oby := py + (lly - py) DIV ph * ph;
+		ilx := olx + pw; olw := llx - olx;
+		irw := (urx - px) MOD pw;
+		IF urx - irw < ilx THEN irw := olw + urx - llx END;
+		dy := lly; sy := lly - oby;
+		IF (oby < lly) & (oby + ph <= ury) THEN
+			dx := llx; sx := olw;
+			IF (olx < llx) & (ilx <= urx) THEN
+				Copy(pat, dst, sx, sy, pw, ph, llx, lly, mode);
+				dx := ilx; sx := 0
+			END;
+			WHILE dx + pw <= urx DO
+				Copy(pat, dst, 0, sy, pw, ph, dx, lly, mode);
+				INC(dx, pw)
+			END;
+			IF dx < urx THEN
+				Copy(pat, dst, sx, sy, irw, ph, dx, lly, mode)
+			END;
+			dy := oby + ph; sy := 0
+		END;
+		WHILE dy + ph <= ury DO
+			dx := llx; sx := olw;
+			IF (olx < llx) & (ilx <= urx) THEN
+				Copy(pat, dst, sx, 0, pw, ph, llx, dy, mode);
+				dx := ilx; sx := 0
+			END;
+			WHILE dx + pw <= urx DO
+				Copy(pat, dst, 0, 0, pw, ph, dx, dy, mode);
+				INC(dx, pw)
+			END;
+			IF dx < urx THEN
+				Copy(pat, dst, sx, 0, irw, ph, dx, dy, mode)
+			END;
+			INC(dy, ph)
+		END;
+		IF dy < ury THEN
+			ty := sy + ury - dy;
+			dx := llx; sx := olw;
+			IF (olx < llx) & (ilx <= urx) THEN
+				Copy(pat, dst, sx, sy, pw, ty, llx, dy, mode);
+				dx := ilx; sx := 0
+			END;
+			WHILE dx + pw <= urx DO
+				Copy(pat, dst, 0, sy, pw, ty, dx, dy, mode);
+				INC(dx, pw)
+			END;
+			IF dx < urx THEN
+				Copy(pat, dst, sx, sy, irw, ty, dx, dy, mode)
+			END
+		END
+	END FillPattern;
+
+	(** darken image while maintaining coverage **)
+	PROCEDURE Darken* (img: Image; factor: REAL);
+		VAR s, i, j, k, y, x, bit: LONGINT; adr: ADDRESS; clamp: ARRAY 256 OF CHAR; pix: Pixel;
+	BEGIN
+		s := ABS(ENTIER(255*factor + 0.5));
+		IF (s # 255) & (img.fmt.components # {alpha}) THEN
+			i := 256; j := 256*s;
+			REPEAT
+				DEC(i); DEC(j, s); k := j DIV 255;
+				IF k <= 255 THEN clamp[i] := CHR(k) ELSE clamp[i] := 0FFX END
+			UNTIL i = 0;
+			y := 0;
+			WHILE y < img.height DO
+				x := 0; adr := img.adr + y * img.bpr; bit := 0;
+				WHILE x < img.width DO
+					img.fmt.unpack(img.fmt, adr, bit, pix);
+					pix[r] := clamp[ORD(pix[r])]; pix[g] := clamp[ORD(pix[g])]; pix[b] := clamp[ORD(pix[b])];
+					img.fmt.pack(img.fmt, adr, bit, pix);
+					bit := bit + img.fmt.bpp; INC(adr, bit); bit := bit MOD 8;
+					INC(x)
+				END;
+				INC(y)
+			END
+		END
+	END Darken;
+
+	(** fade image **)
+	PROCEDURE Fade* (img: Image; factor: REAL);
+		VAR s, i, j, k, y, x, bit: LONGINT; adr: ADDRESS; clamp: ARRAY 256 OF CHAR; pix: Pixel;
+	BEGIN
+		s := ABS(ENTIER(255*factor + 0.5));
+		IF s = 0 THEN
+			Clear(img)
+		ELSIF s # 255 THEN
+			i := 256; j := 256*s;
+			REPEAT
+				DEC(i); DEC(j, s); k := j DIV 255;
+				IF k <= 255 THEN clamp[i] := CHR(k) ELSE clamp[i] := 0FFX END
+			UNTIL i = 0;
+			y := 0;
+			WHILE y < img.height DO
+				x := 0; adr := img.adr + y * img.bpr; bit := 0;
+				WHILE x < img.width DO
+					img.fmt.unpack(img.fmt, adr, bit, pix);
+					pix[r] := clamp[ORD(pix[r])]; pix[g] := clamp[ORD(pix[g])];
+					pix[b] := clamp[ORD(pix[b])]; pix[a] := clamp[ORD(pix[a])];
+					img.fmt.pack(img.fmt, adr, bit, pix);
+					bit := bit + img.fmt.bpp; INC(adr, bit); bit := bit MOD 8;
+					INC(x)
+				END;
+				INC(y)
+			END
+		END
+	END Fade;
+
+	(** make image brighter and more transparent; Opaque(I, f) = Darken(Fade(I, f), 1/f) **)
+	PROCEDURE Opaque* (img: Image; factor: REAL);
+		VAR s, i, j, k, y, x, bit: LONGINT; adr: ADDRESS; clamp: ARRAY 256 OF CHAR; pix: Pixel;
+	BEGIN
+		s := ABS(ENTIER(255*factor + 0.5));
+		IF s = 0 THEN
+			Clear(img)
+		ELSIF s # 255 THEN
+			i := 256; j := 256*s;
+			REPEAT
+				DEC(i); DEC(j, s); k := j DIV 255;
+				IF k <= 255 THEN clamp[i] := CHR(k) ELSE clamp[i] := 0FFX END
+			UNTIL i = 0;
+			y := 0;
+			WHILE y < img.height DO
+				x := 0; adr := img.adr + y * img.bpr; bit := 0;
+				WHILE x < img.width DO
+					img.fmt.unpack(img.fmt, adr, bit, pix);
+					pix[a] := clamp[ORD(pix[a])];
+					img.fmt.pack(img.fmt, adr, bit, pix);
+					bit := bit + img.fmt.bpp; INC(adr, bit); bit := bit MOD 8;
+					INC(x)
+				END;
+				INC(y)
+			END
+		END
+	END Opaque;
+
+	(** add components of two (faded) images **)
+	PROCEDURE Add* (i, j, res: Image);
+		VAR y, x, ibit, jbit, rbit, k: LONGINT; iadr, jadr, radr: ADDRESS; ipix, jpix, rpix: Pixel;
+	BEGIN
+		ASSERT((i.width = j.width) & (i.height = j.height) & (i.width <= res.width) & (i.height <= res.height), 100);
+		y := 0;
+		WHILE y < i.height DO
+			x := 0; iadr := i.adr + y * i.bpr; ibit := 0; jadr := j.adr + y * j.bpr; jbit := 0; radr := res.adr + y * res.bpr; rbit := 0;
+			WHILE x < i.width DO
+				i.fmt.unpack(i.fmt, iadr, ibit, ipix); j.fmt.unpack(j.fmt, jadr, jbit, jpix);
+				FOR k := 0 TO 3 DO
+					rpix[k] := Clamp[ORD(ipix[k]) + ORD(jpix[k])]
+				END;
+				res.fmt.pack(res.fmt, radr, rbit, rpix);
+				ibit := ibit + i.fmt.bpp; INC(iadr, ibit); ibit := ibit MOD 8;
+				jbit := jbit + j.fmt.bpp; INC(jadr, jbit); jbit := jbit MOD 8;
+				rbit := rbit + res.fmt.bpp; INC(radr, rbit); rbit := rbit MOD 8;
+				INC(x)
+			END;
+			INC(y)
+		END
+	END Add;
+
+	(** copy image to another using error diffusion dithering (Floyd-Steinberg) **)
+	PROCEDURE Dither* (src, dst: Image);
+		TYPE
+			error = RECORD r, g, b: LONGINT END;
+		VAR
+			e351: POINTER TO ARRAY OF error;
+			y, x, sb, db, ex, e, e3, e5: LONGINT;
+			sadr, dadr, sa, da: ADDRESS;
+			e7, e51, e1: error;
+			spix, dpix: Pixel;
+	BEGIN
+		ASSERT((src.width <= dst.width) & (src.height <= dst.height), 100);
+		NEW(e351, src.width+2);	(* accumulated error for next row *)
+		y := 0; sadr := src.adr; dadr := dst.adr;
+		WHILE y < src.height DO 	(* scan from left to right *)
+			e7.r := 0; e7.g := 0; e7.b := 0;
+			e51.r := 0; e51.g := 0; e51.b := 0;
+			e1.r := 0; e1.g := 0; e1.b := 0;
+			x := 0; sa := sadr; sb := 0; da := dadr; db := 0;
+			WHILE x < src.width DO
+				ex := x+1;
+				src.fmt.unpack(src.fmt, sa, sb, spix);
+				spix[r] := Clamp[200H + ORD(spix[r]) + e351[ex].r + e7.r];
+				spix[g] := Clamp[200H + ORD(spix[g]) + e351[ex].g + e7.g];
+				spix[b] := Clamp[200H + ORD(spix[b]) + e351[ex].b + e7.b];
+				dst.fmt.pack(dst.fmt, da, db, spix);
+				dst.fmt.unpack(dst.fmt, da, db, dpix);
+				e := ORD(spix[r]) - ORD(dpix[r]); e3 := 3*e DIV 16; e5 := 5*e DIV 16;
+				e7.r := 7*e DIV 16; e351[x].r := e3 + e51.r; e51.r := e5 + e1.r; e1.r := e - e3 - e5 - e7.r;
+				e := ORD(spix[g]) - ORD(dpix[g]); e3 := 3*e DIV 16; e5 := 5*e DIV 16;
+				e7.g := 7*e DIV 16; e351[x].g := e3 + e51.g; e51.g := e5 + e1.g; e1.g := e - e3 - e5 - e7.g;
+				e := ORD(spix[b]) - ORD(dpix[b]); e3 := 3*e DIV 16; e5 := 5*e DIV 16;
+				e7.b := 7*e DIV 16; e351[x].b := e3 + e51.b; e51.b := e5 + e1.b; e1.b := e - e3 - e5 - e7.b;
+				sb := sb + src.fmt.bpp; INC(sa, sb DIV 8); sb := sb MOD 8;
+				db := db + dst.fmt.bpp; INC(da, db DIV 8); db := db MOD 8;
+				x := ex
+			END;
+			INC(y); INC(sadr, src.bpr); INC(dadr, dst.bpr);
+			IF y < src.height THEN	(* scan from right to left *)
+				e351[x] := e51;
+				e7.r := 0; e7.g := 0; e7.b := 0;
+				e51.r := 0; e51.g := 0; e51.b := 0;
+				e1.r := 0; e1.g := 0; e1.b := 0;
+				INC(sa, src.bpr); INC(da, dst.bpr);
+				WHILE x > 0 DO
+					ex := x; DEC(x);
+					sb := sb - src.fmt.bpp; INC(sa, sb DIV 8); sb := sb MOD 8;
+					db := db - dst.fmt.bpp; INC(da, db DIV 8); db := db MOD 8;
+					src.fmt.unpack(src.fmt, sa, sb, spix);
+					spix[r] := Clamp[200H + ORD(spix[r]) + e351[ex].r + e7.r];
+					spix[g] := Clamp[200H + ORD(spix[g]) + e351[ex].g + e7.g];
+					spix[b] := Clamp[200H + ORD(spix[b]) + e351[ex].b + e7.b];
+					dst.fmt.pack(dst.fmt, da, db, spix);
+					dst.fmt.unpack(dst.fmt, da, db, dpix);
+					INC(ex);
+					e := ORD(spix[r]) - ORD(dpix[r]); e3 := 3*e DIV 16; e5 := 5*e DIV 16;
+					e7.r := 7*e DIV 16; e351[x].r := e3 + e51.r; e51.r := e5 + e1.r; e1.r := e - e3 - e5 - e7.r;
+					e := ORD(spix[g]) - ORD(dpix[g]); e3 := 3*e DIV 16; e5 := 5*e DIV 16;
+					e7.g := 7*e DIV 16; e351[x].g := e3 + e51.g; e51.g := e5 + e1.g; e1.g := e - e3 - e5 - e7.g;
+					e := ORD(spix[b]) - ORD(dpix[b]); e3 := 3*e DIV 16; e5 := 5*e DIV 16;
+					e7.b := 7*e DIV 16; e351[x].b := e3 + e51.b; e51.b := e5 + e1.b; e1.b := e - e3 - e5 - e7.b
+				END;
+				e351[1] := e51;
+				INC(y); INC(sadr, src.bpr); INC(dadr, dst.bpr)
+			END
+		END
+	END Dither;
+
+
+	(**--- File I/O ---**)
+
+	(** write image to file rider **)
+	PROCEDURE Write* (VAR fr: Streams.Writer; img: Image);
+	VAR m: Image; h, w, len: LONGINT; adr, aa: ADDRESS; buf: ARRAY 256 OF CHAR;
+			SrcCopy:Mode;
+	BEGIN
+		InitMode(SrcCopy, srcCopy);
+		IF ~(img.fmt.code IN {a1..bgra8888,p16}) THEN
+			NEW(m);
+			IF img.fmt.components = {color} THEN Create(m, img.width, img.height, BGR888)
+			ELSIF img.fmt.components = {alpha} THEN Create(m, img.width, img.height, A8)
+			ELSIF img.fmt.components = {index} THEN Create(m, img.width, img.height, D8)
+			ELSE Create(m, img.width, img.height, BGRA8888)
+			END;
+			Copy(img, m, 0, 0, img.width, img.height, 0, 0, SrcCopy);
+			img := m
+		END;
+		fr.RawNum(2);	(* version *)
+		fr.RawNum(img.fmt.code);
+		fr.RawNum(img.width); fr.RawNum(img.height);
+		fr.RawNum(ABS(img.bpr));
+		h := img.height; adr := img.adr;
+		WHILE h > 0 DO
+			w := ABS(img.bpr); aa := adr;
+			WHILE w > 0 DO
+				len := 256;
+				IF len > w THEN len := w END;
+				SYSTEM.MOVE(aa, ADDRESSOF(buf[0]), len);
+				fr.Bytes(buf, 0, len);
+				DEC(w, len); INC(aa, len)
+			END;
+			DEC(h); INC(adr, img.bpr)
+		END;
+		IF img.fmt.code IN {p8,p16} THEN
+			fr.RawNum(img.fmt.pal.used);
+			len := 0;
+			WHILE len < img.fmt.pal.used DO
+				fr.Char(img.fmt.pal.col[len, r]);
+				fr.Char(img.fmt.pal.col[len, g]);
+				fr.Char(img.fmt.pal.col[len, b]);
+				INC(len)
+			END
+		END;
+		fr.Update	(* optional *)
+	END Write;
+
+	(** read image from file rider **)
+	PROCEDURE Read* (VAR fr: Streams.Reader; img: Image);
+		VAR ver, code, w, h, bpr, len, bytesRead: LONGINT; adr, aa: ADDRESS; fmt: Format; buf: ARRAY 256 OF CHAR; used: LONGINT;
+	BEGIN
+		ASSERT(img#NIL,100);
+		fr.RawNum(ver);	(* know version 1&2*)
+		ASSERT(ver IN {1,2},101);
+		fr.RawNum(code);
+		CASE code OF
+		| a1: fmt := A1
+		| a8: fmt := A8
+		| p8: InitFormat(fmt, p8, 8, 1, {index}, NIL, PackP8, UnpackP8)
+		| d8: fmt := D8
+		| p16: InitFormat(fmt, p16, 16, 2, {index}, NIL, PackP16, UnpackP16);
+		| bgr555: fmt := BGR555
+		| bgr565: fmt := BGR565
+		| bgr466: fmt := BGR466
+		| bgr888: fmt := BGR888
+		| bgra8888: fmt := BGRA8888
+		END;
+		fr.RawNum(w); fr.RawNum(h);
+		Create(img, SHORT(w), SHORT(h), fmt);
+		fr.RawNum(bpr);
+		ASSERT(bpr <= img.bpr);
+		adr := img.adr;
+		WHILE h > 0 DO
+			w := bpr; aa := adr;
+			WHILE w > 0 DO
+				len := 256;
+				IF len > w THEN len := w END;
+				fr.Bytes(buf, 0, len, bytesRead);	(* ignore bytesRead *)
+				SYSTEM.MOVE(ADDRESSOF(buf[0]), aa, len);
+				DEC(w, len); INC(aa, len)
+			END;
+			DEC(h); INC(adr, img.bpr)
+		END;
+		IF code IN {p8,p16} THEN
+			fr.RawNum(used);
+			len := 0;
+			NEW(img.fmt.pal); img.fmt.pal.Init(used);
+			WHILE len < used DO
+				fr.Char(img.fmt.pal.col[len, r]);
+				fr.Char(img.fmt.pal.col[len, g]);
+				fr.Char(img.fmt.pal.col[len, b]);
+				img.fmt.pal.col[len, a] := 0FFX;
+				INC(len)
+			END;
+			InitPalette(img.fmt.pal, used, 4)
+		END
+	END Read;
+
+	(*--- Initialization ---*)
+
+	PROCEDURE InitBitTables;
+		VAR b, i: LONGINT;
+	BEGIN
+		FOR b := 0 TO 0FFH DO
+			FOR i := 0 TO 7 DO
+				IF ODD(ASH(b, -i)) THEN
+					Bit[b, i] := TRUE; Set[b, i] := CHR(b); Clr[b, i] := CHR(b - ASH(1, i))
+				ELSE
+					Bit[b, i] := FALSE; Set[b, i] := CHR(b + ASH(1, i)); Clr[b, i] := CHR(b)
+				END
+			END
+		END
+	END InitBitTables;
+
+	PROCEDURE InitClamp;
+		VAR i: LONGINT;
+	BEGIN
+		FOR i := 0 TO 1FFH DO Clamp[i] := 0X END;
+		FOR i := 0 TO 0FFH DO Clamp[200H+i] := CHR(i) END;
+		FOR i := 300H TO 4FFH DO Clamp[i] := 0FFX END
+	END InitClamp;
+
+	PROCEDURE ToggleMMX*;
+	BEGIN
+		MMXenabled := ~MMXenabled
+	END ToggleMMX;
+
+	PROCEDURE ToggleSSE2*;
+	BEGIN
+		SSE2enabled := ~SSE2enabled;
+		KernelLog.String("SSE2 toggled! Is now: ");  KernelLog.Boolean(SSE2enabled);KernelLog.Ln;
+	END ToggleSSE2;
+
+	(** Map a color value to an 8-bit CLUT index.  Only used if format = index8. *)
+	PROCEDURE ColorToIndex*(col: LONGINT): LONGINT;
+	BEGIN
+			(* default implementation is not very useful and should be overridden. *)
+		RETURN SYSTEM.VAL(LONGINT,
+				SYSTEM.VAL(SET, ASH(col, 7-23)) * {5..7} +
+				SYSTEM.VAL(SET, ASH(col, 4-15)) * {2..4} +
+				SYSTEM.VAL(SET, ASH(col, 1-7)) * {0..1})
+	END ColorToIndex;
+
+	(** Map an 8-bit CLUT index to a color value.  Only used if format = index8. *)
+	PROCEDURE IndexToColor*(index: LONGINT): LONGINT;
+	BEGIN
+			(* default implementation is not very useful and should be overridden. *)
+		RETURN
+				ASH(SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, index) * {5..7}), 23-7) +
+				ASH(SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, index) * {2..4}), 15-4) +
+				ASH(SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, index) * {0..1}), 7-1)
+	END IndexToColor;
+
+BEGIN
+	(*MMXenabled := 23 IN Machine.features;
+	SSE2enabled := Machine.SSE2Support;*)
+(*	plugin := Displays.registry.Await("");	(* assume only one system-wide D8 display driver *)
+	d8display := plugin(Displays.Display); *)
+	InitFormat(A1, a1, 1, 1, {alpha}, NIL, PackA1, UnpackA1);
+	InitFormat(A8, a8, 8, 1, {alpha}, NIL, PackA8, UnpackA8);
+	InitFormat(D8, d8, 8, 1, {index}, NIL, PackD8, UnpackD8);
+	InitFormat(BGR555, bgr555, 16, 2, {color}, NIL, PackBGR555, UnpackBGR555);
+	InitFormat(BGR565, bgr565, 16, 2, {color}, NIL, PackBGR565, UnpackBGR565);
+	InitFormat(BGR466, bgr466, 16, 2, {color}, NIL, PackBGR466, UnpackBGR466);
+	InitFormat(BGR888, bgr888, 24, 4, {color}, NIL, PackBGR888, UnpackBGR888);
+	InitFormat(BGRA8888, bgra8888, 32, 4, {color, alpha}, NIL, PackBGRA8888, UnpackBGRA8888);
+	PixelFormat := BGRA8888;
+	Zero[0] := 0X; Zero[1] := 0X; Zero[2] := 0X; Zero[3] := 0X;
+	InitBitTables; InitClamp
+END Raster.
+
+(**
+Remarks
+
+1. Images
+While many applications wish to handle images of any kind without having to care about details, other applications need low-level access to image interna for maximum effiency. With this in mind, the Images module provides an abstract procedural interface but also discloses low-level information to those clients needing it:
+	* an image references a contiguous block of memory holding pixel data
+	* the point of reference is the address of the pixel in the lower-left corner
+	* pixels are organized in rows (either bottom-up or top-down)
+	* rows can be aligned to an arbitrary number of bytes
+	* the leftmost pixel in a row has the lowest address of all pixels in that row
+	* every pixel uses the same number of bits
+Memory for images can be automatically allocated by using Create(). Alternatively, an image can be initialized on an existing memory block (Init(), InitBuf()) or even on part of an other image (InitRect()).
+
+2. Pixels
+A general pixel pix[] contains four components (in range 0X..255X), specifying red, green, blue, and alpha value of the pixel and accessable as pix[r], pix[g], pix[b] and pix[a]. Note that in order to speed up compositing operations, the alpha value is premultiplied into the color components. Example: a red pixel with 50% coverage can be initialized with SetRGBA(pix, 255, 0, 0, 127), after which pix[r]=pix[a]=7FX and pix[g]=pix[b]=0X. Use GetRGBA() to recover the original color and alpha values.
+
+3. Palettes
+Many bitmap images and Oberon display drivers use some kind of indexed format to store colors, i.e. the value stored in the bitmap serves as an index into an array of colors. A Palette stores up to 256 colors as an array of pixels, making the mapping of an index to the corresponding color straightforward. To speed up the inverse mapping from an RGB triple to an index with PaletteIndex(), additional data is initialized when InitPalette() is called. Use ComputePalette() to compute a palette that best approximates the colors in a given image (e.g. before quantizing it to indexed format).
+
+4. Formats
+While general pixels accurately describe color and alpha information, they use a lot of memory (32 bits). Most images therefore only store part of that information. A Format record describes how pixels are represented within an image. It contains
+	* the number of bits used per pixel (must be 1, 2, 4, 8, 16, 24 or 32)
+	* the set of components stored in a pixel (color, index and/or alpha)
+	* a palette if the format uses one
+	* procedures for storing (packing) and loading (unpacking) a general pixel
+The pack and unpack procedures are given an address and a bit number specifying where the pixel is located in memory, with bit numbers ascending from left to right (although a format is free to choose any bit ordering within a pixel).
+
+5. Predefined Formats
+The following global variables contain formats which are special and have a unique code number identifying them. Besides, most operations have better performance if acting on images using them.
+	* A1 (code a1): one bit alpha, MSB leftmost (corresponds to Oberon display patterns)
+	* A8 (code a8): 8 bit alpha (mainly for anti-aliased character patterns)
+	* - (code p8): 8 bit indexed with custom palette (Oberon pictures, use InitPaletteFormat to initialize)
+	* D8 (code d8): 8 bit indexed with display palette (no palette structure attached)
+	* - (code p16): 16 bit indexed with 16bit Palette. This type is, e.g.,  often used in medical imaging (DICOM-3 standard) (* PH 2004 *)
+	* BGR555 (code bgr555), BGR565 (code bgr565), BGR466 (code bgr466): 16 bit hi-color
+	* BGR888 (code bgr888): 24 bit true-color
+	* BGRA8888 (code bgra8888), PixelFormat: 32 bit true-color with alpha channel (general pixel format)
+Procedure DisplayFormat() returns the format that best matches the supplied kind of display transfer format. The returned image format is preferably used for allocating shadow bitmaps.
+
+6. Compositing
+Most operations require a transfer mode for specifying how source and destination pixels should be combined when alpha information is present. The following compositing operations are supported:
+	* clear: destination becomes black and completely transparent
+	* srcCopy: source completely replaces destination (cf. Display.replace)
+	* dstCopy: no effect
+	* srcOverDst: source replaces destination where source is opaque (cf. Display.paint)
+	* dstOverSrc: destination replaces source where destination is opaque
+	* srcInDst: source where destination is opaque
+	* dstInSrc: destination where source is opaque
+	* srcWithoutDest*: source is cleared where destination is opaque
+	* dstWithoutSrc*: destination is cleared where source is opaque
+	* srcAtopDst*: source replaces destination where destination is opaque
+	* dstAtopSrc*: destination replaces source where source is opaque
+	* srcXorDst*: destination is cleared where both source and destination are opaque (cf. Display.invert)
+A transfer mode is initialized with a compositing operation and optionally with a color. (The color is used when the source is a pure alpha format which doesn't contain any color of its own.) An initialized mode can be bound to a source and destination format by calling Bind(), by which the mode's transfer procedure is set appropriately. A transfer procedure unpacks pixels from source and destination, blends them according to the compositing operation, and packs the resulting pixel in the destination. Bind() chooses an optimal transfer procedure for the given combination of compositing operation, source format, and destination format.
+
+7. Internalization and Externalization
+Images can be loaded from file and stored to file using one of many popular image file formats. The Load() and Store() procedures rely on a section 'ImageFormats' in the Aos registry. This section contains a list of file types that are associated with command procedures. When one of these commands is called, it should initialize the global 'LoadProc' and 'StoreProc' variables. These, when called, should read an image from or write an image to the file and set 'done' to TRUE if successful.
+**)
+

source/ARM.Usb.Mod

@@ -0,0 +1,1764 @@
+MODULE Usb;  (** AUTHOR "staubesv"; PURPOSE "USB 2.0 Bus Driver"; *)
+(**
+ * Bluebottle USB 2.0 Bus Driver based on Usb.Mod from "cplattner".
+ *
+ * Note that the hub driver (UsbHubDriver.Mod) is closely coupled with the USB driver. The USB driver maintains bus topology using
+ * UsbDevice object. Usb.rootHubArray is the root of the tiered-star topology. These are the root hubs of the USB host controllers, which
+ * are represented as EmulatedHubDevice. All other USB devices are represented using UsbDevice objects. If the hubFlag of such an object
+ * is set, it represents a USB hub. Then the hubPortDevices array is valid.
+ *
+ * References:
+ *
+ * 	Universal Serial Bus Specification, Revision 2.0
+ * 	USB Interface Association Descriptor and Device Class Code and Use Model
+ * 	USB Engineering Change Notice: Interface Association Descriptors (Applies to USB Specification 2.0)
+ *
+ *	All references can be found at http://www.usb.org
+ *
+ * History:
+ *
+ *	01.12.2005	First release (staubesv)
+ *	07.12.2005	Support for automatic device driver loading using UsbDriverLoader (staubesv)
+ *	12.12.2005 	Exception handling for Usbdi.Driver.Connect added (staubesv)
+ *	13.12.2005 	Fixed UsbDevice.InternalParseConfiguration (always ask for 8 bytes, some devices didn't like less) (staubesv)
+ *	03.12.2005	Fixed bug in DriverManager.Remove that prevented automatic USB pipe de-allocation when removing the device driver,
+ *				added exception handling for Usbdi.Driver.Disconnect (staubesv)
+ *	25.01.2006	Don't import UTF8String anymore, copied the procedure needed to this file to save space for the boot file (staubesv)
+ *	10.02.2006	Moved TrimWS & Length into this module (staubesv)
+ *	09.06.2006	DriverManager is notified when the driver lookup service is enabled (staubesv)
+ *	28.06.2006	Removed some unnecessary exports, procedure GetRootHubs now copies rootHubs array, moved utility procedures
+ *				to module UsbStrings.Mod (staubesv)
+ *	26.07.2006	Replaced DriverManager.Notify mechanism by nbrOfDriverEvents variable, clients can poll this value to be
+ *				informed about driver adds/removals (staubesv)
+ *	02.08.2006	ParseEndpointDescriptor enhanced for EndpointDescriptor.type field, adapted to Usbdi changes (staubesv)
+ *	05.01.2007	Check descriptor type in UsbDevice.GetDescriptor (staubesv)
+ *	22.03.2007	Renamed UsbDevice fields hubPortDevices, hubPortPermanentlyDisabled & HubPortErrors to deviceAtPort,
+ *				portPermanentlyDisabled & portErrors
+ *
+ * TODOs
+ * 	- more general GetStringDescriptor
+ *	- put driver manager in own module, driver manager should only return reference to appropriate driver, not call its connect/disconnect procedures
+ *)
+
+IMPORT SYSTEM, Modules, Machine, Plugins, KernelLog, UsbDriverLoader, Usbdi, UsbHcdi, Debug := UsbDebug, Lib := UsbUtilities;
+
+CONST
+
+	(** USB device states *)
+	StateDisconnected* = -1;
+	StateAttached* = 0;
+	StatePowered* = 1;
+	StateDefault* = 2;
+	StateAddress* = 3;
+	StateConfigured* = 4;
+	StateSuspended* = 5;
+
+	(* Descriptor types for GetDescriptor & SetDesciptor USB standard requests *)
+	DescriptorDevice = 1;
+	DescriptorConfiguration = 2;
+	DescriptorString = 3;
+	DescriptorInterface = 4;
+	DescriptorEndpoint = 5;
+	DescriptorDeviceQualifier = 6;
+	DescriptorOtherSpeedConfig = 7;
+	DescriptorInterfacePower = 8;
+	DescriptorOtg = 9;
+	DescriptorDebug = 10;
+	DescriptorIad = 11; (* Interface Associtation Descriptor *)
+
+	(* Standard Request Codes (USB2.0, p. 251) *)
+	SrGetStatus = 0;
+	SrClearFeature = 1;
+	SrSetFeature = 3;
+	SrSetAddress = 5;
+	SrGetDescriptor = 6;
+	SrSetDescriptor = 7;
+	SrGetConfiguration = 8;
+	SrSetConfiguration = 9;
+	SrGetInterface = 10;
+	SrSetInterface = 11;
+	SrSynchFrame = 12;
+
+	(** Standard feature selectors for GetFeature & SetFeature USB standard requests *)
+	FsDeviceRemoteWakeup* = 1;
+	FsEndpointHalt* = 0;
+	FsTestMode* = 2;
+
+	(** GetStatus bits (recipient = Device) *)
+	SelfPowered* = {0}; 		(* Current power source *)
+	RemoteWakeup* = {1}; 	(* Remote Wakeup enabled? *)
+	(** GetStatus bits (recipient = Endpoint) *)
+	Halted* = {0}; 			(* Endpoint halted? *)
+
+	LowSpeed* = 0;
+	FullSpeed* = 1;
+	HighSpeed* = 2;
+
+	(* LANGID codes used by string descriptors, see HID page on www.usb.org *)
+	IdUserDefault = 0400H;
+	IdSystemDefault = 0800H;
+	IdEnglishUS = 0409H;
+	IdEnglishUK = 0809H;
+
+	(* Timeouts *)
+	DefaultTimeout* = 5000;
+
+	(* Driver manager constants *)
+	DmMaxPriorities = 12;
+
+TYPE
+
+	Name* = Usbdi.Name;
+	Description* = Usbdi.Description;
+
+TYPE
+
+	DeviceDescriptor* = POINTER TO RECORD (Usbdi.DeviceDescriptor)
+		bMaxPacketSize0- : LONGINT;
+
+		iManufacturer- : LONGINT;
+		iProduct- : LONGINT;
+		iSerialNumber- : LONGINT;
+
+		sManufacturer- : Lib.AsciiString;
+		sProduct- : Lib.AsciiString;
+		sSerialNumber- : Lib.AsciiString;
+
+		uManufacturer- : Lib.UnicodeString;
+		uProduct- : Lib.UnicodeString;
+		uSerialNumber- : Lib.UnicodeString;
+	END;
+
+	(** As specified in the Universal Serial Bus Specification 1.1/2.0 **)
+	ConfigurationDescriptor* = POINTER TO RECORD (Usbdi.ConfigurationDescriptor)
+		bLength- : LONGINT;
+		wTotalLength- : LONGINT;
+		iConfiguration- : LONGINT;
+		bmAttributes- : SET;
+		bMaxPower- : LONGINT;
+
+		sConfiguration- : Lib.AsciiString;
+		uConfiguration- : Lib.UnicodeString;
+
+		(* Decoded bmAttributes *)
+		selfPowered- : BOOLEAN;
+		remoteWakeup- : BOOLEAN;
+	END;
+
+	(** UsbDeviceInterface: As specified in the Universal Serial Bus Specification 1.1/2.0
+		Oberon Usb addition: "Driver" points to the device driver for this interface, NIL means no driver (yet) attached
+		to this interface  **)
+	InterfaceDescriptor* = POINTER TO RECORD (Usbdi.InterfaceDescriptor)
+		bLength- : LONGINT;
+		iInterface- : LONGINT;
+		sInterface- : Lib.AsciiString;
+		uInterface- : Lib.UnicodeString;
+		driver- : Usbdi.Driver;
+	END;
+
+	(** EndpointDescriptor: As specified in the Universal Serial Bus Specification 1.1/2.0 **)
+	EndpointDescriptor* = POINTER TO RECORD (Usbdi.EndpointDescriptor)
+		bLength- : LONGINT;
+		bInterval- : LONGINT; (* Raw value; interpretation dependend on transfer speed and transfer type *)
+		mult- : LONGINT; (* Only for high-speed isochronous & interrupt transfers: How many transactions per microframe (1,2 or 3) *)
+	END;
+
+	InterfaceAssociationDescriptor* = POINTER TO RECORD (Usbdi.InterfaceAssociationDescriptor);
+		bLength- : LONGINT;
+		iFunction- : LONGINT;
+		sFunction- : Lib.AsciiString;
+		uFunction- : Lib.UnicodeString;
+	END;
+
+TYPE
+
+	UsbDevice* = OBJECT(Usbdi.UsbDevice)
+	VAR
+		(* Default control pipe (endpoint zero) *)
+		defaultpipe* : UsbHcdi.Pipe;
+
+		(* Device Qualifier; NIL if not available *)
+		qualifier- : DeviceDescriptor;
+
+		(* Other speed configurations *)
+		otherconfigurations- : Usbdi.Configurations;
+
+		address* : LONGINT;
+		speed* : LONGINT; (* Usb.LowSpeed, Usb.FullSpeed, Usb.HighSpeed *)
+
+		(* This device is connected to the port <port> of the UsbDevice <parent> *)
+		parent* : UsbDevice;
+		port* : LONGINT;
+
+		(* 	If this is a low- or fullspeed device that is connected to a high-speed bus, the device is connected to
+			the high-speed hub with the device address <ttAddress> at port <ttPort> *)
+		ttAddress*, ttPort* : LONGINT;
+
+		hubFlag* : BOOLEAN;
+		(* USB hub specific fields *)
+		nbrOfPorts* : LONGINT;
+		deviceAtPort* : POINTER TO ARRAY OF UsbDevice;
+		portPermanentDisabled* : POINTER TO ARRAY OF BOOLEAN;
+		portErrors* : POINTER TO ARRAY OF LONGINT;
+
+		(* Private, exported readonly to grant access to bytesTransfered field of the controller *)
+		controller* : UsbHcdi.Hcd;
+
+		PROCEDURE SetState*(state : LONGINT);
+		BEGIN {EXCLUSIVE}
+			IF Debug.Trace & Debug.traceDeviceStates THEN ShowStateTransition(SELF, state); END;
+			SELF.state := state;
+		END SetState;
+
+		(*
+		 * Build a pipe object for the specified endpoint.
+		 * @param interface: USB device interface to be searched
+		 * @param endpointAddr: Address of endpoint to be searched
+		 * @return: Pipe for specified endpoint; NIL if endpoint not found
+		 *)
+		PROCEDURE GetPipeByInterface(interface : InterfaceDescriptor; endpointAddr : LONGINT) : UsbHcdi.Pipe;
+		VAR pipe : UsbHcdi.Pipe; endpoint : EndpointDescriptor; endp : LONGINT;
+		BEGIN
+			WHILE (endp < interface.bNumEndpoints) DO (* Search all endpoints of the interface *)
+				IF interface.endpoints[endp].bEndpointAddress = endpointAddr THEN (* Found ! *)
+					endpoint := interface.endpoints[endp](EndpointDescriptor);
+					NEW(pipe, address, endpointAddr, controller);
+					IF (SYSTEM.VAL(SET, endpointAddr) * {7}) = {7} THEN (* device-to-host *)
+						pipe.direction := UsbHcdi.In;
+					ELSE (* host-to-device *)
+						pipe.direction := UsbHcdi.Out;
+					END;
+					pipe.status := Usbdi.InProgress;
+					pipe.mult := endpoint.mult;
+					pipe.device := SELF;
+					pipe.ttPort := ttPort;
+					pipe.ttAddress := ttAddress;
+					pipe.type := SYSTEM.VAL(INTEGER,endpoint.bmAttributes *  {0,1});
+					IF pipe.type = UsbHcdi.PipeControl THEN
+						pipe.direction := UsbHcdi.In;
+					ELSE
+						pipe.mode := Usbdi.MinCpu;
+						pipe.ioc := TRUE;
+						controller.AddCompletionHandler(pipe);
+					END;
+					pipe.maxPacketSize := endpoint.wMaxPacketSize;
+					pipe.maxRetries := 3;
+					pipe.irqInterval := endpoint.bInterval;
+					pipe.speed := speed;
+					pipe.timeout := DefaultTimeout;
+					pipe.completion.device := SELF;
+					RETURN pipe;
+				END;
+				INC(endp);
+			END;
+			RETURN pipe;
+		END GetPipeByInterface;
+
+		(** Allocate a pipe for the specified endpoint *)
+		PROCEDURE GetPipe*(endpoint : LONGINT) : Usbdi.Pipe;
+		VAR pipe : UsbHcdi.Pipe; intfc, altIntfc : LONGINT; interface : InterfaceDescriptor;
+		BEGIN
+			IF SYSTEM.VAL(SET, endpoint) * {0..3} = {} THEN (* Special case: Default control pipe is always allocated *)
+				ASSERT(defaultpipe#NIL);
+				RETURN defaultpipe;
+			ELSE
+				LOOP (* Search all interfaces of the active configuration *)
+					IF (pipe # NIL) OR (intfc >= actConfiguration.bNumInterfaces) THEN EXIT; END;
+					interface := actConfiguration.interfaces[intfc] (InterfaceDescriptor);
+					pipe := GetPipeByInterface(interface, endpoint);
+					IF pipe # NIL THEN EXIT; END;
+					LOOP (* Search all alternate interfaces *)
+						IF altIntfc >= interface.numAlternateInterfaces THEN EXIT; END;
+						pipe := GetPipeByInterface(interface.alternateInterfaces[altIntfc] (InterfaceDescriptor), endpoint);
+						IF pipe # NIL THEN EXIT; END;
+						INC(altIntfc);
+					END; (* LOOP altIntfc *)
+					INC(intfc);
+				END; (* LOOP intfc *)
+				controller.GetPipe(address, endpoint, pipe);
+				RETURN pipe; (* can be NIL *)
+			END;
+			RETURN NIL;
+		END GetPipe;
+
+		(** De-allocate the specified pipe *)
+		PROCEDURE FreePipe*(pipe : Usbdi.Pipe);
+		BEGIN
+			controller.FreePipe(pipe (UsbHcdi.Pipe));
+		END FreePipe;
+
+		(* Register this USB device at the USB hub <hub> *)
+		PROCEDURE Register*(hub: UsbDevice; portNbr: LONGINT);
+		BEGIN {EXCLUSIVE}
+			ASSERT(hub.hubFlag);
+			parent := hub; port := portNbr;
+			hub.deviceAtPort[portNbr] := SELF;
+			Machine.AtomicInc(nbrOfTopologyEvents);
+		END Register;
+
+		(*  this device from the hub it is connected to and remove its driver if installed *)
+		PROCEDURE Remove*;
+		VAR n : LONGINT;device: UsbDevice;
+		BEGIN {EXCLUSIVE}
+			IF hubFlag THEN
+				FOR n := 0 TO nbrOfPorts - 1 DO
+					IF deviceAtPort[n] # NIL THEN
+						device := deviceAtPort[n];
+						deviceAtPort[n] := NIL; (* avoid recursion *)
+						device.SetState(StateDisconnected);
+						device.Remove;
+						IF parent = SELF THEN (* Root hub: only disable ports on root hubs *) controller.DisablePort(n); END;
+					END;
+				END;
+				IF actConfiguration.interfaces[0](InterfaceDescriptor).driver # NIL THEN
+					drivers.RemoveInstance(actConfiguration.interfaces[0](InterfaceDescriptor).driver.name, SELF);
+				END;
+			ELSE
+				FOR n := 0 TO actConfiguration.bNumInterfaces - 1 DO
+					IF actConfiguration.interfaces[n](InterfaceDescriptor).driver # NIL THEN (* Remove device driver instance *)
+						drivers.RemoveInstance(actConfiguration.interfaces[n](InterfaceDescriptor).driver.name, SELF);
+						actConfiguration.interfaces[n](InterfaceDescriptor).driver := NIL;
+					END;
+				END;
+			END;
+			(* If it's not a root hub, then unregister all pipes of this device *)
+			IF ~(hubFlag & (parent = SELF)) THEN
+				controller.FreeAll(address);
+				controller.FreeAddress(address);
+			END;
+			Machine.AtomicInc(nbrOfTopologyEvents);
+		END Remove;
+
+		(** Implementation of the USB standard device requests, see USB Specification Rev 1.1, p. 185 *)
+
+		(** The ClearFeature standard request is used to clear or disable a specific feature. *)
+		PROCEDURE ClearFeature*(recipient : SET;  feature, recipientNumber : LONGINT) : BOOLEAN;
+		BEGIN
+			ASSERT((recipient = Usbdi.Device) OR (recipient =Usbdi.Interface) OR (recipient = Usbdi.Endpoint));
+			RETURN Request(recipient, SrClearFeature, feature, recipientNumber, 0, Usbdi.NoData) = Usbdi.Ok;
+		END ClearFeature;
+
+		(** This request is used to set or enable a specific feature *)
+		PROCEDURE SetFeature*(recipient : SET; feature, recipientNumber : LONGINT) : BOOLEAN;
+		BEGIN
+			ASSERT((recipient = Usbdi.Device) OR (recipient = Usbdi.Interface) OR (recipient = Usbdi.Endpoint));
+			RETURN Request(recipient, SrSetFeature, feature, recipientNumber, 0, Usbdi.NoData) = Usbdi.Ok;
+		END SetFeature;
+
+		(** Sets the address of the USB device dev to adr (should only be used by the USB driver) *)
+		PROCEDURE SetAddress*(adr : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Request(Usbdi.ToDevice, SrSetAddress, adr, 0, 0, Usbdi.NoData) = Usbdi.Ok THEN
+				address := adr; RETURN TRUE;
+			END;
+			RETURN FALSE;
+		END SetAddress;
+
+		(** This requests returns the current device configuration value. If the returned value is zero, the device is not configured. *)
+		PROCEDURE GetConfiguration*(VAR conf : LONGINT) : BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			NEW(buffer, 1);
+			IF Request(Usbdi.ToHost, SrGetConfiguration, 0, 0, 1, buffer) = Usbdi.Ok THEN
+				conf := ORD(buffer[0]); RETURN TRUE;
+			ELSE
+				conf := -1; RETURN FALSE;
+			END;
+		END GetConfiguration;
+
+		(** This requests sets the device configuration *)
+		PROCEDURE SetConfiguration*(conf : LONGINT) : BOOLEAN;
+		BEGIN
+			ASSERT(configurations[conf].bConfigurationValue <= 255);
+			IF Request(Usbdi.ToDevice + Usbdi.Standard, SrSetConfiguration, configurations[conf].bConfigurationValue, 0, 0, Usbdi.NoData) = Usbdi.Ok THEN
+				actConfiguration := configurations[conf];
+				RETURN TRUE;
+				(* need to update info for pipes *)
+			END;
+			RETURN FALSE;
+		END SetConfiguration;
+
+		(** This request returns the specified descriptor if the descriptor exists *)
+		PROCEDURE GetDescriptor*(descriptor, index, wIndex, len : LONGINT; VAR buffer : Usbdi.Buffer) : BOOLEAN;
+		VAR status : Usbdi.Status;
+		BEGIN
+			ASSERT(len >= 2);
+			status := Request(Usbdi.ToHost + Usbdi.Standard + Usbdi.Device, SrGetDescriptor, index + descriptor*100H, wIndex, len, buffer);
+			RETURN (status = Usbdi.Ok) & (ORD(buffer[1]) = descriptor);
+		END GetDescriptor;
+
+		(** This request may be used to update existing descriptors or new descriptors may be added *)
+		PROCEDURE SetDescriptor*(type : SET;  index, wIndex, len : LONGINT; VAR buffer : Usbdi.Buffer) : BOOLEAN;
+		BEGIN
+			RETURN Request(Usbdi.ToDevice, SrSetDescriptor, index + 100H*SYSTEM.VAL(LONGINT, type), wIndex, len, buffer) = Usbdi.Ok;
+		END SetDescriptor;
+
+		(** This request returns the selected alternate settings for the specified interface *)
+		PROCEDURE GetInterface*(interfaceNumber : LONGINT; VAR setting : LONGINT): BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			NEW(buffer, 1);
+			IF Request(Usbdi.ToHost + Usbdi.Interface, SrGetInterface, 0, interfaceNumber, 1, buffer) = Usbdi.Ok THEN
+				setting := ORD(buffer[0]);
+				RETURN TRUE;
+			END;
+			RETURN FALSE;
+		END GetInterface;
+
+		(** This requests allows the host to select an alternate setting for the specified interface *)
+		PROCEDURE SetInterface*(interfaceNumber, setting : LONGINT): BOOLEAN;
+		BEGIN
+			RETURN Request(Usbdi.ToDevice + Usbdi.Standard + Usbdi.Interface, SrSetInterface, setting, interfaceNumber, 0, Usbdi.NoData) = Usbdi.Ok;
+		END SetInterface;
+
+		(** This request returns status for the specified recipient *)
+		PROCEDURE GetStatus*(recipient: SET;  recipientNumber: LONGINT; VAR status : SET): BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			ASSERT((recipient = Usbdi.Device) OR (recipient = Usbdi.Interface) OR (recipient = Usbdi.Endpoint));
+			NEW(buffer, 2);
+			IF Request(Usbdi.ToHost + Usbdi.Standard + recipient, SrGetStatus , 0, recipientNumber, 2, buffer) = Usbdi.Ok THEN
+				status := SYSTEM.VAL(SET, ORD(buffer[0]) + 100H*SYSTEM.VAL(LONGINT, ORD(buffer[1])));
+				RETURN TRUE;
+			END;
+			RETURN FALSE;
+		END GetStatus;
+
+		(** This request is used to set and then report an endpoint's synchronization frame *)
+		PROCEDURE SynchFrame*(endpoint: LONGINT; VAR frameNumber : LONGINT): BOOLEAN;  (* UNTESTED *)
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			NEW(buffer, 2);
+			IF Request(Usbdi.ToDevice + Usbdi.Standard + Usbdi.Endpoint, SrSynchFrame, 0, endpoint, 2, buffer) = Usbdi.Ok THEN
+				frameNumber := ORD(buffer[0]) + 100H*SYSTEM.VAL(LONGINT, ORD(buffer[1]));
+				RETURN TRUE;
+			END;
+			RETURN FALSE;
+		END SynchFrame;
+
+		(** USB device request *)
+		PROCEDURE Request*(bmRequestType : SET;  bRequest, wValue, wIndex, wLength : LONGINT; VAR buffer : Usbdi.Buffer) : Usbdi.Status;
+		BEGIN
+			RETURN defaultpipe.Request(bmRequestType, bRequest, wValue, wIndex, wLength, buffer);
+		END Request;
+
+		(*
+		 * Get a device's configuration descriptor.
+		 * @param nbr Configuration number
+		 * @param type Configuration descriptor or Other-Speed configuration descriptor
+		 * @return Buffer containing the configuration, NIL if operation fails
+		 *)
+		PROCEDURE InternalGetConfigurations(type : LONGINT; configurations : Usbdi.Configurations) : BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr; c, length : LONGINT;
+		BEGIN
+			ASSERT(((type = DescriptorConfiguration) & (descriptor # NIL)) OR ((type = DescriptorOtherSpeedConfig) & (qualifier # NIL)));
+			FOR c := 0 TO LEN(configurations)-1 DO
+				(* Get the total size of this configuration *)
+				NEW(buffer, 8);
+				IF GetDescriptor(type, c, 0, 8, buffer) THEN
+					length := ORD(buffer[2])+ 256*SYSTEM.VAL(LONGINT, ORD(buffer[3]));
+					NEW(buffer, length);
+					IF GetDescriptor(type, c, 0, length, buffer) THEN (* Load and parse configuration *)
+						configurations[c] := ParseConfigurationDescriptor(buffer);
+						IF configurations[c] = NIL THEN RETURN FALSE; END;
+						IF (descriptor # NIL) &
+						   (descriptor.bDeviceClass = 0EFH) & (descriptor.bDeviceSubClass = 02H) & (descriptor.bDeviceProtocol = 01H) THEN
+						   (* Multi-interface function has Interface Association descriptors *)
+						    configurations[c].iads := ParseInterfaceAssociation(buffer);
+						END;
+						(* Parse non-standard descriptors *)
+						configurations[c].unknown := ParseUnknownDescriptors(configurations[c], buffer);
+					ELSE
+						IF Debug.Level >= Debug.Errors THEN KernelLog.String("Usb: Get configuration failed."); KernelLog.Ln; END;
+						RETURN FALSE;
+					END;
+				ELSE
+					IF Debug.Level >= Debug.Errors  THEN KernelLog.String("Usb: Read first 8 bytes of configuration failed"); KernelLog.Ln; END;
+					RETURN FALSE;
+				END;
+			END;
+			RETURN TRUE;
+		END InternalGetConfigurations;
+
+		PROCEDURE GetConfigurations*() : BOOLEAN;
+		BEGIN
+			ASSERT(descriptor # NIL);
+			NEW(configurations, descriptor.bNumConfigurations);
+			IF InternalGetConfigurations(DescriptorConfiguration, configurations) THEN
+				RETURN TRUE;
+			ELSE
+				configurations := NIL;
+				RETURN FALSE;
+			END;
+		END GetConfigurations;
+
+		PROCEDURE GetOtherSpeedConfigurations*() : BOOLEAN;
+		BEGIN
+			ASSERT(qualifier # NIL);
+			NEW(otherconfigurations, qualifier.bNumConfigurations);
+			IF InternalGetConfigurations(DescriptorOtherSpeedConfig, otherconfigurations) THEN
+				RETURN TRUE;
+			ELSE
+				otherconfigurations := NIL;
+				RETURN FALSE;
+			END;
+		END GetOtherSpeedConfigurations;
+
+		(**
+		 * Loads and parses the USB device qualifier. This descriptor is only available on USB 2.0 devices
+		 * which can operate as Low-/Fullspeed and Highspeed USB device.
+		 * It essentially contains the same information as the device descriptor, but the values are for
+		 * the case that the device would operate at its other operating speed.
+		 * @return TRUE, if operation succeeded, FALSE otherwise
+		 *)
+		PROCEDURE GetDeviceQualifier*() : BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			ASSERT(descriptor # NIL);
+			NEW(buffer, 10);
+			IF GetDescriptor(DescriptorDeviceQualifier, 0, 0, 10, buffer) THEN
+				qualifier := ParseDeviceQualifier(buffer);
+				(* Duplicate fields from device descriptor *)
+				qualifier.idVendor := descriptor.idVendor;
+				qualifier.idProduct := descriptor.idProduct;
+				qualifier.bcdDevice := descriptor.bcdDevice;
+				qualifier.iManufacturer := descriptor(DeviceDescriptor).iManufacturer;
+				qualifier.iProduct := descriptor(DeviceDescriptor).iProduct;
+				qualifier.iSerialNumber := descriptor(DeviceDescriptor).iSerialNumber;
+				qualifier.sManufacturer := descriptor(DeviceDescriptor).sManufacturer;
+				qualifier.sProduct := descriptor(DeviceDescriptor).sProduct;
+				qualifier.sSerialNumber := descriptor(DeviceDescriptor).sSerialNumber;
+				qualifier.uManufacturer := descriptor(DeviceDescriptor).uManufacturer;
+				qualifier.uProduct := descriptor(DeviceDescriptor).uProduct;
+				qualifier.uSerialNumber := descriptor(DeviceDescriptor).uSerialNumber;
+				RETURN TRUE;
+			ELSIF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Couldn't retrieve device qualifier."); KernelLog.Ln;
+			END;
+			RETURN FALSE;
+		END GetDeviceQualifier;
+
+		(**
+		 * Loads and parses the USB device descriptor. If parsing succeeds, the SELF.descriptor record
+		 * will be set, otherwise it's set to NIL.
+		 * @return TRUE, if opertation succeeded, FALSE otherwise
+		 *)
+		PROCEDURE GetDeviceDescriptor*() : BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			NEW(buffer, 18);
+			IF GetDescriptor(DescriptorDevice, 0, 0, 18, buffer) THEN
+				descriptor := ParseDeviceDescriptor(buffer);
+				RETURN TRUE;
+			ELSIF Debug.Level >= Debug.Errors THEN KernelLog.String("Usb: Couldn't get the device descriptor."); KernelLog.Ln;
+			END;
+			RETURN FALSE;
+		END GetDeviceDescriptor;
+
+		PROCEDURE ShowName*;
+		BEGIN
+			IF (descriptor # NIL) & (descriptor(DeviceDescriptor).sManufacturer # NIL) OR (descriptor(DeviceDescriptor).sProduct # NIL) THEN
+				IF descriptor(DeviceDescriptor).sManufacturer # NIL THEN KernelLog.String(descriptor(DeviceDescriptor).sManufacturer^); KernelLog.Char(" "); END;
+				IF descriptor(DeviceDescriptor).sProduct # NIL THEN KernelLog.String(descriptor(DeviceDescriptor).sProduct^); END;
+			ELSE
+				KernelLog.String("unknown device");
+			END;
+		END ShowName;
+
+	END UsbDevice;
+
+TYPE
+
+	RootHubArray* = POINTER TO ARRAY OF UsbDevice;
+
+TYPE
+
+	(* Root hub emulation. Emulate USB standard device requests for root hubs. Since all standard requests implemented in the
+	 * UsbDevice object use Request for the actual transfer, we simply overwrite it and emulated the results of the control transfers. *)
+	EmulatedHubDevice* = OBJECT(UsbDevice);
+
+		PROCEDURE GetPipe*(endpoint : LONGINT) : Usbdi.Pipe;
+		BEGIN
+			HALT(99); RETURN NIL; (* Root hubs don't provide pipes *)
+		END GetPipe;
+
+		PROCEDURE FreePipe*(pipe : Usbdi.Pipe);
+		BEGIN
+			HALT(99); (* Root hubs don't provide pipes *)
+		END FreePipe;
+
+		PROCEDURE Register*(hub: UsbDevice; portNbr: LONGINT);
+		BEGIN
+			HALT(99);
+		END Register;
+
+		(** Emulated USB device request *)
+		PROCEDURE Request*(bmRequestType : SET;  bRequest, wValue, wIndex, wLength : LONGINT; VAR buffer : Usbdi.Buffer) : Usbdi.Status;
+		VAR res : Usbdi.Status;
+		BEGIN
+			res := Usbdi.Stalled;
+			CASE bRequest OF
+				SrGetStatus:
+					BEGIN
+						ASSERT(wLength = 2);
+						IF (bmRequestType * Usbdi.ToHost # {}) & (bmRequestType - Usbdi.ToHost = {})  THEN
+							(* Get Device status: Indicate Selfpowered, Remote Wakeup disabled *)
+							buffer[0] := CHR(SYSTEM.VAL(LONGINT, {0})); buffer[1] := 0X; res := Usbdi.Ok;
+						ELSIF (bmRequestType * Usbdi.ToHost # {}) & (bmRequestType - Usbdi.ToHost = Usbdi.Interface) THEN
+							(* Get Interface status:  Reserved *)
+							 buffer[0] := 0X; buffer[1] := 0X; res := Usbdi.Ok;
+						ELSIF (bmRequestType * Usbdi.ToHost # {}) & (bmRequestType - Usbdi.ToHost = Usbdi.Endpoint) THEN
+							(* Get endpoint status:  Indicate endpoint not halted. *)
+							 buffer[0] := 0X; buffer[1] := 0X; res := Usbdi.Ok;
+						END;
+					END;
+				(* All following requests will fail *)
+				|SrClearFeature:
+				|SrSetFeature:
+				|SrGetDescriptor:
+				|SrSetDescriptor:
+				|SrGetConfiguration:
+				|SrSetConfiguration:
+				(* Requests unsupported by hubs / root hubs *)
+				|SrGetInterface: HALT(99);
+				|SrSetInterface: HALT(99);
+				|SrSynchFrame: HALT(99);
+				(* Requests unsupported by root hubs *)
+				|SrSetAddress: HALT(99);
+			ELSE
+				HALT(99);
+			END;
+			RETURN res;
+		END Request;
+
+		(* Emulate device, configuration, interface and endpoint descriptors of root hub *)
+		PROCEDURE EmulateDescriptors;
+		VAR
+			descriptor : DeviceDescriptor; configuration : ConfigurationDescriptor;
+			interface : InterfaceDescriptor; endpoint : EndpointDescriptor;
+			name : Lib.AsciiString;
+			i, j : LONGINT;
+		BEGIN
+			(* Emulate device descriptor *)
+			NEW(descriptor); SELF.descriptor := descriptor;
+			descriptor.bNumConfigurations := 1;
+			NEW(name, LEN(controller.name) + LEN(controller.desc) + 2);
+		 	WHILE(i < LEN(controller.name)) & (controller.name[i] # 0X) DO name[i] := controller.name[i]; INC(i); END;
+		 	name[i] := " "; name[i+1] := "(";
+		 	WHILE(j < LEN(controller.desc)) & (controller.desc[j] # 0X) DO name[j + i + 2] := controller.desc[j]; INC(j); END;
+		 	name[j + i + 2] := ")"; name[j + i + 3] := 0X;
+			descriptor.sProduct := name;
+			(* Emulate device configuration *)
+			NEW(configurations, 1); NEW(configuration); configurations[0] := configuration;
+			configuration.bNumInterfaces := 1;
+			configuration.bmAttributes := {6,7}; (* Indicate self-powered device *)
+			configuration.bMaxPower := 0; (* Root hub don't draw current from the BUS *)
+			configuration.selfPowered := TRUE;
+			configuration.remoteWakeup := FALSE;
+			NEW(configurations[0].interfaces, 1); NEW(interface);
+			configurations[0].interfaces[0] := interface;
+			configurations[0].interfaces[0].bInterfaceClass := 9; (* Hub device class *)
+			configurations[0].interfaces[0].bInterfaceSubClass := 0;
+			configurations[0].interfaces[0].bNumEndpoints := 1;
+			configurations[0].interfaces[0].bInterfaceProtocol := 0;
+			NEW(configurations[0].interfaces[0].endpoints, 1);
+			NEW(endpoint); endpoint.type := Usbdi.InterruptIn;
+			configurations[0].interfaces[0].endpoints[0] := endpoint;
+			actConfiguration := configuration;
+		END EmulateDescriptors;
+
+		PROCEDURE &New*(controller : UsbHcdi.Hcd);
+		BEGIN
+			ASSERT(controller # NIL);
+			SELF.controller := controller;
+			IF controller.isHighSpeed THEN
+				speed := HighSpeed
+			ELSE
+				(* TODO: a way to differentiate full/low speed controllers? *)
+				speed := FullSpeed
+			END;
+			parent := SELF;
+			hubFlag := TRUE;
+			nbrOfPorts := controller.portCount;
+			NEW(deviceAtPort, nbrOfPorts);
+			NEW(portPermanentDisabled, nbrOfPorts);
+			NEW(portErrors, nbrOfPorts);
+			EmulateDescriptors;
+		END New;
+
+	END EmulatedHubDevice;
+
+TYPE
+
+	RegisteredDriver* = POINTER TO RECORD
+		probe : Usbdi.ProbeProc;
+		name- : Usbdi.Name;
+		desc- : Usbdi.Description;
+		usedSuffix- : ARRAY 100 OF BOOLEAN; (* Which numbers are used for the unique names of instances *)
+		next- : RegisteredDriver;
+	END;
+
+TYPE
+	(*
+	 * This object manages USB device drivers. It will be notified by the USB hub driver when devices
+	 * are attached/detached from the bus. If a device is attached, the driver manager calls the probe procedures
+	 * of all USB device drivers which are registered at the driver manager. When a device is detached from the
+	 * bus, the associated driver (if any) will be removed from the UsbDevice object.
+	 *)
+	DriverManager* = OBJECT(Usbdi.DriverManager)
+	VAR
+		(* Driver manager internal USB device driver registy (exported for WMUsbInfo only). *)
+		drivers- : ARRAY DmMaxPriorities OF RegisteredDriver;
+
+		(* Incremented each time a driver is added or removed *)
+		nbrOfDriverEvents- : LONGINT;
+
+		(* local copy of rootHubs, since it could be modified while operating on it *)
+		rootHubs : RootHubArray;
+
+		alive, dead, probeDrivers : BOOLEAN;
+
+		(* For each interface of the USB device <dev> try to install a registered driver; called when a new USB device is found *)
+		PROCEDURE ProbeDevice*(dev : UsbDevice);
+		VAR n : LONGINT;
+		BEGIN
+			FOR n := 0 TO dev.actConfiguration.bNumInterfaces - 1 DO
+				IF dev.actConfiguration.interfaces[n](InterfaceDescriptor).driver = NIL THEN
+					(* probe all device drivers and install a driver instance if a driver for the device is registered *)
+					Install(dev, n);
+				END;
+			END;
+		END ProbeDevice;
+
+		(* Load driver using driver database services *)
+		PROCEDURE ConsultDriverDatabase(dev : UsbDevice) : BOOLEAN;
+		VAR  loaded : BOOLEAN; d : DeviceDescriptor; i : InterfaceDescriptor; intf : LONGINT;
+		BEGIN
+			IF (dev # NIL) & (dev.descriptor # NIL) THEN
+				d := dev.descriptor (DeviceDescriptor);
+				(* First look for a device-specific driver *)
+				loaded := UsbDriverLoader.LoadDeviceDriver(d.idVendor, d.idProduct, d.bcdDevice);
+				(* Look for class-specific driver *)
+				IF ~((d.bDeviceClass = 0EFH) & (d.bDeviceSubClass = 02H) & (d.bDeviceProtocol = 01H)) & (* IAD -> Search interfaces *)
+					((d.bDeviceClass # 0) OR (d.bDeviceSubClass # 0) OR (d.bDeviceProtocol # 0)) THEN (* Class description at device level *)
+					IF UsbDriverLoader.LoadClassDriver(d.bDeviceClass, d.bDeviceSubClass, d.bDeviceProtocol, d.bcdDevice) THEN
+						loaded := TRUE;
+					END;
+				ELSE (* Class description at interface level *)
+					IF (dev.actConfiguration # NIL) & (dev.actConfiguration.interfaces # NIL) THEN
+						intf := 0;
+						LOOP
+							i := dev.actConfiguration.interfaces[intf] (InterfaceDescriptor);
+							(* TODO: Actually, some classes specifiy class-specfic descriptors that may contain the class revision the device supports. Use this instead of bcdDevice *)
+							IF (i # NIL) & UsbDriverLoader.LoadClassDriver(i.bInterfaceClass, i.bInterfaceSubClass, i.bInterfaceProtocol, d.bcdDevice) THEN
+								loaded := TRUE;
+							END;
+							INC(intf);
+							IF intf >= LEN(dev.actConfiguration.interfaces) THEN EXIT END;
+						END;
+					END;
+				END;
+			END;
+			RETURN loaded;
+		END ConsultDriverDatabase;
+
+		PROCEDURE LookupDriver(dev : UsbDevice; interface : InterfaceDescriptor; VAR temp : RegisteredDriver) : Usbdi.Driver;
+		VAR drv : Usbdi.Driver; i : LONGINT;
+		BEGIN
+			LOOP (* Search all priority lists *)
+				temp := drivers[i].next;
+				LOOP (* Search all drivers in priority list i *)
+					IF temp = NIL THEN (* No more drivers available *) EXIT; END;
+					drv := temp.probe(dev, interface);
+					IF drv # NIL THEN (* Driver found *) EXIT; END;
+					temp := temp.next;
+				END;
+				IF drv # NIL THEN (* Driver found *) EXIT; END;
+				INC(i); IF (i >= DmMaxPriorities) THEN (* No driver available *) EXIT; END;
+			END;
+			RETURN drv;
+		END LookupDriver;
+
+		(* Returns FALSE if connect failed or trapped *)
+		PROCEDURE SafelyConnect(drv : Usbdi.Driver) : BOOLEAN;
+		VAR connected, trap : BOOLEAN;
+		BEGIN
+			connected := drv.Connect();
+		FINALLY
+			IF trap & (Debug.Level >= Debug.Warnings) THEN KernelLog.String("Usb: Catched TRAP when calling Driver.Connect."); KernelLog.Ln; END;
+			RETURN (~trap & connected);
+		END SafelyConnect;
+
+		PROCEDURE SafelyDisconnect(drv : Usbdi.Driver);
+		VAR trap : BOOLEAN;
+		BEGIN
+			drv.Disconnect;
+		FINALLY
+			IF trap & (Debug.Level >= Debug.Warnings) THEN KernelLog.String("Usb: Catched TRAP when calling Driver.Disconnect."); KernelLog.Ln; END;
+		END SafelyDisconnect;
+
+		(* Checks whether an appropriate driver for the USB device <dev> is registred in registredDrivers.
+		 *  If yes, a unique Plugins.Name is generated and the driver is added to the usbDrivers registry *)
+		PROCEDURE Install(dev : UsbDevice; interfaceIdx : LONGINT);
+		VAR
+			temp : RegisteredDriver;
+			drv : Usbdi.Driver;
+			interface : InterfaceDescriptor;
+			i, res : LONGINT;
+			name : Usbdi.Name;
+			suffix : LONGINT; (* 0-99; suffix is used to generate unique names for AosPlugin.Name *)
+		BEGIN
+			interface := dev.actConfiguration.interfaces[interfaceIdx] (InterfaceDescriptor);
+			(* Search an USB device driver for this device (See USB Common Class Specification, 3.10 Locating USB Drivers) *)
+			(* we have to consult the database first always because of priorities. If a driver has already been loaded previously, this will return FALSE anyway *)
+			IF ConsultDriverDatabase(dev) THEN
+				RETURN;  (* Appropriate device driver has been loaded from driver database. Loading will force bus enumeration, so don't continue here. *)
+			ELSE
+				drv := LookupDriver(dev, interface, temp);
+			END;
+
+			BEGIN {EXCLUSIVE}
+				(* Since it's possible that two threads (active body, hub driver via ProbeDevice) try to install a driver for the same device and interface,
+				we need to check here *)
+				IF (drv # NIL) & (interface.driver = NIL)  THEN
+
+					(* Driver found;  generate a unique name for the instance of this driver to be created *)
+					drv.device := dev; drv.interface := interface;
+
+					(* Get first unused suffix *)
+					i := 0; WHILE (temp.usedSuffix[i] = TRUE) & (i <= 99) DO INC(i); END;
+					IF (i = 99) & (temp.usedSuffix[99] = TRUE) THEN
+						KernelLog.String("Usb: No more than 99 instances of a driver supported"); KernelLog.Ln;
+						RETURN;
+					ELSE
+						temp.usedSuffix[i] := TRUE; suffix := i;
+					END;
+
+					name := AddSuffix(temp.name, suffix);
+
+					drv.SetName(name); drv.desc := temp.desc;
+
+					(* Add this driver to the usbDrivers registry *)
+					usbDrivers.Add(drv, res);
+					IF res # Plugins.Ok THEN
+						KernelLog.String("Usb: Couldn't register USB device driver (res: "); KernelLog.Int(res, 0); KernelLog.String(")"); KernelLog.Ln;
+						temp.usedSuffix[suffix] := FALSE;
+						RETURN;
+					ELSE (* USB device driver successfully registered *)
+						interface.driver := drv;
+						IF ~SafelyConnect(drv) THEN
+							KernelLog.String("Usb: Connect of driver "); KernelLog.String(drv.name); KernelLog.String("("); KernelLog.String(drv.desc); KernelLog.String(") failed."); KernelLog.Ln;
+							ASSERT(drv.device(UsbDevice).parent.hubFlag);
+							ASSERT(drv.device(UsbDevice).parent.portPermanentDisabled # NIL);
+							(* Don't try to re-install a driver until ConnectStatusChange at this port *)
+							drv.device(UsbDevice).parent.portPermanentDisabled[drv.device(UsbDevice).port] := TRUE;
+							drv.device(UsbDevice).Remove;
+							RETURN;
+						END;
+						IF Debug.Trace & Debug.traceDm THEN KernelLog.String("Usb: Registered USB device driver: "); KernelLog.String(name); KernelLog.Ln; END;
+					END;
+				END;
+			END;
+		END Install;
+
+		(* For all USB devices which are attached to any USB root hub in the system the procedure ProbeDeviceChain() is called *)
+		PROCEDURE ProbeDriversInternal;
+		VAR i : LONGINT;
+		BEGIN (* Works with local copy of rootHubs array *)
+			GetRootHubs(rootHubs);
+			IF rootHubs # NIL THEN
+				FOR i := 0 TO LEN(rootHubs)-1 DO
+					ProbeDeviceChain(rootHubs[i]);
+					rootHubs[i] := NIL; (* we don't need the reference anymore *)
+				END;
+			END;
+		END ProbeDriversInternal;
+
+		(* Called by ProbeDrivers; calls Install() for all devices which don't already have a driver instance installed *)
+		PROCEDURE ProbeDeviceChain(dev : UsbDevice);
+		VAR n : LONGINT;
+		BEGIN
+			FOR n := 0 TO dev.actConfiguration.bNumInterfaces - 1 DO
+				IF dev.actConfiguration.interfaces[n](InterfaceDescriptor).driver = NIL THEN
+					(* Probe all device drivers and install a driver instance if a driver for the device is registered *)
+					Install(dev, n);
+				END;
+			END;
+			IF dev.hubFlag THEN
+				FOR n := 0 TO dev.nbrOfPorts - 1 DO
+					IF dev.deviceAtPort[n] # NIL THEN ProbeDeviceChain(dev.deviceAtPort[n]); END;
+				END;
+			END;
+		END ProbeDeviceChain;
+
+		(** Add a USB device driver to the internal registry. Driver names have to be unique and no longer than 30 characters (incl. Null-String) *)
+		PROCEDURE Add*(probe : Usbdi.ProbeProc; CONST name: Usbdi.Name; CONST desc: Usbdi.Description; priority : LONGINT);
+		VAR temp, new : RegisteredDriver; i : LONGINT;
+		BEGIN
+			(* The specified name mustn't be longer than 30 characters (including 0X) *)
+			WHILE (name[i] # 0X) & (i < 32) DO INC(i); END;
+			IF (i > 29) OR (name = "")  THEN
+				KernelLog.String("Usb: Couldn't add driver (name NULL or longer than 30 characters or not NULL-terminated)"); KernelLog.Ln;
+				RETURN;
+			END;
+
+			(* Specified priority must be in the interval [0,DmMaxPriorities-1] *)
+			IF (priority > DmMaxPriorities-1) OR (priority < 0) THEN
+				KernelLog.String("Usb: Couldn't add driver (Priority invalid)"); KernelLog.Ln;
+				RETURN;
+			END;
+
+			BEGIN {EXCLUSIVE}
+				(* Check whether there is no driver with the name <name> registered *)
+				FOR i := 0 TO DmMaxPriorities-1 DO
+					temp := drivers[i].next;
+					WHILE temp # NIL DO
+						IF temp.name = name THEN
+							KernelLog.String("Usb: Couldn't add driver (driver name already registered)"); KernelLog.Ln;
+							RETURN;
+						END;
+						temp := temp.next;
+					END;
+				END;
+
+				(* Okay, arguments are valid, create RegisteredDriver object and add it to internal registry *)
+				NEW(new);
+				new.probe := probe;
+				new.name := name;
+				new.desc := desc;
+				new.next := drivers[priority].next;
+
+				FOR i := 0 TO 99 DO new.usedSuffix[i] := FALSE; END;
+				drivers[priority].next := new;
+			END;
+			IF Debug.Verbose THEN
+				KernelLog.String("Usb: Driver "); KernelLog.String(name); KernelLog.String(" ("); KernelLog.String(desc); KernelLog.String(")");
+				KernelLog.String(" has been added."); KernelLog.Ln;
+			END;
+
+			(* Maybe a USB device is already attached, just waiting for this driver: check! *)
+			ProbeDrivers;
+
+			BEGIN {EXCLUSIVE} INC(nbrOfDriverEvents); END;
+		END Add;
+
+		(* Removes a device driver instance from usbDriver registry; only used by the USB driver itself*)
+		PROCEDURE RemoveInstance(CONST name : Usbdi.Name; dev : UsbDevice);
+		VAR
+			plugin : Plugins.Plugin;
+			driver : Usbdi.Driver;
+			regname : Plugins.Name;
+			temp : RegisteredDriver;
+			i, suffix : LONGINT;
+		BEGIN
+			plugin := usbDrivers.Get(name);
+			IF plugin # NIL THEN (* Uninstall this instance *)
+
+				driver := plugin (Usbdi.Driver);
+
+				usbDrivers.Remove(plugin);
+
+				SafelyDisconnect(driver);
+
+				(* Remove allocated pipes *)
+				driver.device(UsbDevice).controller.FreeAll(driver.device(UsbDevice).address);
+
+				(* Get the name of the registered device driver which generates this instances *)
+				WHILE name[i] # 0X DO regname[i] := name[i]; INC(i); END;
+				regname[i-1] := 0X; regname[i-2] := 0X;
+
+				suffix := GetSuffix(name);
+
+				(* Need to update usedSuffix at the registered driver *)
+				i := 0;
+				LOOP
+					temp := drivers[i].next;
+					WHILE (temp # NIL) & (temp.name # regname) DO temp := temp.next; END;
+					IF temp # NIL THEN (* Registered device driver found *)
+						temp.usedSuffix[suffix] := FALSE;
+						EXIT;
+					END;
+					INC(i); IF (i >= DmMaxPriorities) THEN (* No driver found *) EXIT; END;
+				END;
+
+				IF (i = DmMaxPriorities) & (temp = NIL) THEN (* Registered driver for this instance was not found *)
+					 IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Couldn't find registered driver of the removed driver instance"); KernelLog.Ln; END;
+				END;
+			ELSE (* No such instance found *)
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Warning: Couldn't remove driver instance (no instance found)"); KernelLog.Ln; END;
+			END;
+		END RemoveInstance;
+
+		(**
+		 * Calls Disconnect of all instances of the driver. All instances are removed from the usbDrivers registry
+		 * and the device driver is removed from the internal registry. *)
+		PROCEDURE Remove*(CONST name : Plugins.Name);
+		VAR
+			prev, temp : RegisteredDriver;
+			regname : Plugins.Name;
+			plugin : Plugins.Plugin;
+			dev : UsbDevice;
+			i, j : LONGINT;
+		BEGIN {EXCLUSIVE}
+			IF Debug.Trace & Debug.traceDm THEN KernelLog.String("Usb: Removing driver: "); KernelLog.String(name); KernelLog.Ln; END;
+			(* Remove device driver from internal registry *)
+			LOOP
+				prev := drivers[i];
+				temp := drivers[i].next;
+				WHILE (temp # NIL) & (temp.name # name) DO temp := temp.next; prev := prev.next; END;
+				IF temp # NIL THEN (* Driver found *) EXIT; END;
+				INC(i); IF (i >= DmMaxPriorities) THEN (* No driver available *) EXIT; END;
+			END;
+			(* Remove driver from internal registry and remove all its instances *)
+			IF temp # NIL THEN
+				(* Remove driver from internal registry *)
+				prev.next := temp.next;
+				(* Remove all instances of the driver *)
+				FOR i := 0 TO 99 DO
+					IF temp.usedSuffix[i] = TRUE THEN (* Driver instance found *)
+						(* Get plugin name *)
+						regname := AddSuffix(temp.name, i);
+						plugin := usbDrivers.Get(regname);
+
+						IF plugin = NIL THEN
+							IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Error while trying to remove driver from usbDrivers registry"); KernelLog.Ln; END;
+						ELSE
+							SafelyDisconnect(plugin(Usbdi.Driver));
+							dev := plugin(Usbdi.Driver).device (UsbDevice);
+							IF dev # NIL THEN (* Remove link to driver instance from USB device *)
+								IF ~(dev.hubFlag & (dev.parent = dev)) THEN
+									dev.controller.FreeAll(dev.address);
+								END;
+								FOR j := 0 TO dev.actConfiguration.bNumInterfaces-1 DO
+									IF dev.actConfiguration.interfaces[j](InterfaceDescriptor).driver = plugin(Usbdi.Driver) THEN
+										dev.actConfiguration.interfaces[j](InterfaceDescriptor).driver := NIL;
+									END;
+								END;
+							END;
+							usbDrivers.Remove(plugin);
+							temp.usedSuffix[i]:=FALSE;
+						END;
+					END;
+				END;
+				INC(nbrOfDriverEvents);
+			ELSIF Debug.Level >= Debug.Warnings THEN
+				KernelLog.String("Usb: Warning: Couldn't remove driver "); KernelLog.String(name); KernelLog.Ln;
+			END;
+		END Remove;
+
+		(* Appends the suffix to name; the suffix is a number between 0-99 which is added as 2 ASCII characters (each 1 bytes)
+		 * note: name mustn't be longer than 30 characters (incl. Null-Termination)  *)
+		PROCEDURE AddSuffix*(CONST name: Plugins.Name; suffix : LONGINT) :  Plugins.Name;
+		VAR i : LONGINT; newName : Plugins.Name;
+		BEGIN
+			WHILE name[i]#0X DO newName[i]:=name[i]; INC(i); END;
+			(* Append suffix to name *)
+			IF suffix < 10 THEN
+				newName[i]:="0";
+				newName[i+1]:=CHR(suffix+48);
+				newName[i+2]:=0X;
+			ELSE
+				newName[i]:=CHR((suffix DIV 10)+48);
+				newName[i+1]:=CHR((suffix MOD 10)+48);
+				newName[i+2]:=0X;
+			END;
+			RETURN newName;
+		END AddSuffix;
+
+		(* Returns the suffix of the Plugins.Name name *)
+		PROCEDURE GetSuffix(CONST name : Plugins.Name) : LONGINT;
+		VAR i, suffix : LONGINT;
+		BEGIN
+			WHILE (name[i] # 0X) & (i < 32) DO INC(i); END;
+			suffix:= (ORD(name[i-2]) - 48) * 10 + ORD(name[i-1])-48;
+			ASSERT((suffix >= 0) & (suffix <= 99));
+			RETURN suffix;
+		END GetSuffix;
+
+		(* Displays a list of registered drivers *)
+		PROCEDURE Show*;
+		VAR temp : RegisteredDriver; i : LONGINT;
+		BEGIN
+			KernelLog.Ln; KernelLog.String("Usb: Registered USB device drivers: "); KernelLog.Ln;
+			FOR i := 0 TO DmMaxPriorities - 1 DO
+				temp := drivers[i].next;
+				WHILE temp # NIL DO
+					KernelLog.String("   ");
+					KernelLog.String(temp.name); KernelLog.String(" ("); KernelLog.String(temp.desc); KernelLog.String(")");
+					KernelLog.String(" Priority: "); KernelLog.Int(i, 0); KernelLog.Ln;
+					temp := temp.next;
+				END;
+			END;
+		END Show;
+
+		PROCEDURE ProbeDrivers;
+		BEGIN {EXCLUSIVE}
+			probeDrivers := TRUE;
+		END ProbeDrivers;
+
+		PROCEDURE Terminate;
+		BEGIN
+			BEGIN {EXCLUSIVE} alive := FALSE; END;
+			(* Release object lock to prevent deadlock *)
+			BEGIN {EXCLUSIVE} AWAIT(dead); END;
+		END Terminate;
+
+		PROCEDURE &Init*;
+		VAR  i : LONGINT; temp : RegisteredDriver;
+		BEGIN
+			alive := TRUE; dead := FALSE;
+			FOR i := 0 TO 11 DO NEW(temp); drivers[i] := temp; END; (* Allocate list heads *)
+		END Init;
+
+	BEGIN {ACTIVE}
+		(* This thread decouples the process of checking all connected USB devices for matching device drivers from the caller. 	*)
+		(* It will be active in the following two situations:																	*)
+		(*	- A device driver is successfully registered at the driver manager (Add procedure)									*)
+		(*	- The driver lookup service has been enabled																		*)
+		(* 																													*)
+		(* Note: 																											*)
+		(* When a device is connected to a bus, the thread of the corresponding hub driver will call ProbeDevice, so different	*)
+		(* hubs/busses can install device drivers concurrently. To prevent two threads (this one and the hub driver's one) to con-*)
+		(* currently install the same device driver to the same function, another check is made in proedure install.				*)
+		WHILE alive DO
+			BEGIN {EXCLUSIVE}
+				AWAIT(probeDrivers OR ~alive);
+				probeDrivers := FALSE;
+			END;
+			IF alive THEN (* Check availability of device drivers for all connected devices *)
+				IF Debug.Trace & Debug.traceDm THEN
+					KernelLog.Enter; KernelLog.String("Usb: Check connected devices for available device drivers"); KernelLog.Exit;
+				END;
+				ProbeDriversInternal;
+			END;
+		END;
+		IF Debug.Trace & Debug.traceDm THEN KernelLog.Enter; KernelLog.String("Usb: Driver Manager object terminated."); KernelLog.Exit; END;
+		BEGIN {EXCLUSIVE} dead := TRUE; END;
+	END DriverManager;
+
+TYPE
+	Registry= OBJECT(Plugins.Registry) END Registry;
+
+VAR
+	usbDrivers- : (*Plugins.*)Registry;	(* Instanciated USB device drivers (linked to a attached USB device) 	*)
+	drivers- : DriverManager;   			(* Usb internal registry for installable USB device drivers 			*)
+	rootHubs : RootHubArray;
+	nbrOfTopologyEvents- : LONGINT;	(* Incremented each time a device is connected/disconnected *)
+
+PROCEDURE ParseDeviceDescriptor(buffer : Usbdi.Buffer) : DeviceDescriptor;
+VAR descriptor : DeviceDescriptor;
+BEGIN
+	IF LEN(buffer) >= 18 THEN
+		NEW(descriptor);
+		descriptor.bcdUSB := ORD(buffer[2]) + 256*SYSTEM.VAL(LONGINT, ORD(buffer[3]));
+		descriptor.bDeviceClass := ORD(buffer[4]);
+		descriptor.bDeviceSubClass := ORD(buffer[5]);
+		descriptor.bDeviceProtocol := ORD(buffer[6]);
+		descriptor.bMaxPacketSize0 := ORD(buffer[7]);
+		descriptor.idVendor := ORD(buffer[8]) + 256*SYSTEM.VAL(LONGINT, ORD(buffer[9]));
+		descriptor.idProduct := ORD(buffer[10]) + 256*SYSTEM.VAL(LONGINT, ORD(buffer[11]));
+		descriptor.bcdDevice := ORD(buffer[12]) +  256*SYSTEM.VAL(LONGINT, ORD(buffer[13]));
+		descriptor.iManufacturer := ORD(buffer[14]);
+		descriptor.iProduct := ORD(buffer[15]);
+		descriptor.iSerialNumber := ORD(buffer[16]);
+		descriptor.bNumConfigurations := ORD(buffer[17]);
+	ELSIF Debug.Level >= Debug.Errors THEN KernelLog.String("Usb: Could not parse device descriptor."); KernelLog.Ln;
+	END;
+	RETURN descriptor;
+END ParseDeviceDescriptor;
+
+(**
+ * Parses the USB device qualifier. This descriptor is only available on USB 2.0 devices which can operate as
+ * Low-/Fullspeed and Highspeed USB device. It essentially contains the same information as the device descriptor,
+ * but the values are for  the case that the device would operate at its other operating speed.
+ *)
+PROCEDURE ParseDeviceQualifier(buffer : Usbdi.Buffer) : DeviceDescriptor;
+VAR qualifier : DeviceDescriptor;
+BEGIN
+	IF LEN(buffer) >= 10 THEN
+		NEW(qualifier);
+		qualifier.bcdUSB := ORD(buffer[2]) + 256*SYSTEM.VAL(LONGINT, ORD(buffer[3]));
+		qualifier.bDeviceClass := ORD(buffer[4]);
+		qualifier.bDeviceSubClass := ORD(buffer[5]);
+		qualifier.bDeviceProtocol := ORD(buffer[6]);
+		qualifier.bMaxPacketSize0 := ORD(buffer[7]);
+		qualifier.bNumConfigurations := ORD(buffer[8]);
+	ELSIF Debug.Level >= Debug.Errors THEN KernelLog.String("Usb: Could not parse device qualifier."); KernelLog.Ln;
+	END;
+	RETURN qualifier;
+END ParseDeviceQualifier;
+
+(** Parse all Interface Association Descriptors in the given configuration. All other descriptor types are skipped. *)
+PROCEDURE ParseInterfaceAssociation(buffer : Usbdi.Buffer) : Usbdi.Iads;
+VAR iads : Usbdi.Iads; iad : InterfaceAssociationDescriptor; idx, num, i : LONGINT;
+BEGIN
+	IF Debug.Trace & Debug.traceParsing THEN KernelLog.String("Usb: Parsing interface association descriptors:"); KernelLog.Ln; END;
+	(* Determine number of available IADs *)
+	WHILE(idx+1 < LEN(buffer)) DO
+		IF (ORD(buffer[idx+1]) = DescriptorIad) THEN INC(num); END;
+		idx := idx + ORD(buffer[idx+0]);
+	END;
+
+	idx := 0;
+	IF num > 0 THEN (* Parse the IADs *)
+		NEW(iads, num);
+		LOOP
+			IF idx+8 >= LEN(buffer) THEN EXIT; END;
+			IF i >= LEN(iads) THEN EXIT; END;
+			IF ORD(buffer[idx+1]) = DescriptorIad THEN
+		 		IF Debug.Trace & Debug.traceParsing THEN ShowParse("interface association", idx, ORD(buffer[idx+1]), ORD(buffer[idx+0])); END;
+		 		NEW(iad);
+		 		iad.bFirstInterface := ORD(buffer[idx+2]);
+				iad.bInterfaceCount := ORD(buffer[idx+3]);
+				iad.bFunctionClass := ORD(buffer[idx+4]);
+				iad.bFunctionSubClass := ORD(buffer[idx+5]);
+				iad.bFunctionProtocol := ORD(buffer[idx+6]);
+				iad.iFunction := ORD(buffer[idx+7]) + 256*SYSTEM.VAL(LONGINT, ORD(buffer[idx+8]));
+				iads[i] := iad;
+				INC(i);
+			END;
+			idx := idx + ORD(buffer[idx+0]);
+		END;
+	ELSIF Debug.Trace & Debug.traceParsing THEN KernelLog.String("No interface association descriptors found."); KernelLog.Ln;
+	END;
+	IF i # num THEN (* We didn't find all IADs... we can live without them, but warn the user *)
+		IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Warning: Error when parsing IADs."); KernelLog.Ln; END;
+		RETURN NIL;
+	END;
+	RETURN iads;
+END ParseInterfaceAssociation;
+
+(* Parse all non-standard descriptors found in the configuration *)
+PROCEDURE ParseUnknownDescriptors(configuration : Usbdi.ConfigurationDescriptor; buffer : Usbdi.Buffer) : Usbdi.UnknownDescriptor;
+VAR idx, i,  type,  curIntf, curAltIntf, curEp : LONGINT; list, unknown : Usbdi.UnknownDescriptor;
+
+	PROCEDURE AppendToList(head, unknown : Usbdi.UnknownDescriptor);
+	VAR u : Usbdi.UnknownDescriptor;
+	BEGIN
+		u := head; WHILE(u.next # NIL) DO u := u.next; END;
+		u.next := unknown;
+	END AppendToList;
+
+	PROCEDURE AppendNonStandard(unknown : Usbdi.UnknownDescriptor) : BOOLEAN;
+	VAR i : LONGINT; intf, altIntf : Usbdi.InterfaceDescriptor; endp : Usbdi.EndpointDescriptor;
+	BEGIN
+		IF (curIntf = -1) THEN (* Append to configuration *)
+			IF configuration.unknown = NIL THEN configuration.unknown := unknown;
+			ELSE
+				AppendToList(configuration.unknown, unknown);
+			END;
+		ELSE (* Append to interface, alternate interface or endpoint *)
+			(* Search Interface *)
+			LOOP
+				IF (configuration.interfaces=NIL) OR (i >= LEN(configuration.interfaces)) THEN EXIT; END;
+				intf := configuration.interfaces[i];
+				IF intf.bInterfaceNumber = curIntf THEN EXIT; END;
+				INC(i);
+			END;
+			IF (intf = NIL) OR (intf.bInterfaceNumber # curIntf) THEN RETURN FALSE; END;
+
+			IF curAltIntf # 0 THEN
+				(* Search alternate interface *)
+				i := 0;
+				LOOP
+					IF (intf.alternateInterfaces=NIL) OR (i >= LEN(intf.alternateInterfaces)) THEN EXIT; END;
+					altIntf := intf.alternateInterfaces[i];
+					IF altIntf.bAlternateSetting = curAltIntf THEN EXIT; END;
+					INC(i);
+				END;
+				IF (altIntf = NIL) OR (altIntf.bAlternateSetting # curAltIntf) THEN RETURN FALSE; END;
+				intf := altIntf;
+			END;
+
+			IF curEp = -1 THEN (* Append to interface *)
+				IF intf.unknown = NIL THEN intf.unknown := unknown;
+				ELSE
+					AppendToList(intf.unknown, unknown);
+				END;
+			ELSE (* Append to endpoint *)
+				(* Search endpoint descriptor *)
+				i := 0;
+				LOOP
+					IF (intf.endpoints = NIL) OR (i >= LEN(intf.endpoints)) THEN EXIT; END;
+					endp := intf.endpoints[i];
+					IF endp.bEndpointAddress = curEp THEN EXIT; END;
+					INC(i);
+				END;
+				IF (endp = NIL) OR (endp.bEndpointAddress # curEp) THEN RETURN FALSE; END;
+				IF endp.unknown = NIL THEN endp.unknown := unknown;
+				ELSE
+					AppendToList(endp.unknown, unknown);
+				END;
+			END;
+		END;
+		RETURN TRUE;
+	END AppendNonStandard;
+
+BEGIN
+	ASSERT(configuration # NIL);
+	IF Debug.Trace & Debug.traceParsing THEN KernelLog.String("Usb: Parsing non-standard descriptors:"); KernelLog.Ln; END;
+	curIntf := -1; curAltIntf := -1; curEp := -1;
+	LOOP
+		IF idx + 1 >= LEN(buffer) THEN EXIT; END;
+
+	 	type := ORD(buffer[idx+1]);
+
+	 	IF type = DescriptorConfiguration THEN
+	 		(* skip *)
+	 	ELSIF type = DescriptorIad THEN
+	 		curIntf := -1; curAltIntf := -1;	curEp := -1;
+	 	ELSIF type = DescriptorInterface THEN
+	 		IF idx+3 >= LEN(buffer) THEN EXIT; END;
+	 		curIntf := ORD(buffer[idx+2]);
+			curAltIntf := ORD(buffer[idx+3]);
+			curEp := -1;
+	 	ELSIF type = DescriptorEndpoint THEN
+	 		IF idx+2 >= LEN(buffer) THEN EXIT; END;
+	 		curEp := ORD(buffer[idx+2]);
+	 	ELSE (* Non-Standard descriptor *)
+	 		NEW(unknown);
+	 		unknown.bLength := ORD(buffer[idx+0]);
+	 		unknown.bDescriptorType := ORD(buffer[idx+1]);
+	 		IF Debug.Trace & Debug.traceParsing THEN ShowParse("unknown descriptor", idx, unknown.bDescriptorType, unknown.bLength); END;
+	 		IF idx + unknown.bLength > LEN(buffer) THEN EXIT; END;
+	 		NEW(unknown.descriptor, unknown.bLength);
+	 		FOR i := 0 TO unknown.bLength-1 DO unknown.descriptor[i] := buffer[idx+i] END;
+	 		IF ~AppendNonStandard(unknown) THEN
+	 			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: ParseUnknownDescriptors: Warning: Could not assign non-standard descriptor."); KernelLog.Ln;	END;
+	 			RETURN NIL;
+	 		END;
+	 	END;
+	 	idx := idx + ORD(buffer[idx + 0]);
+	END;
+
+	IF idx # LEN(buffer) THEN
+		IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: ParseUnknownDescriptors: Warning: Parse Error"); KernelLog.Ln; END;
+		list := NIL;
+	END;
+	RETURN list;
+END ParseUnknownDescriptors;
+
+(* Parse the first endpoint descriptor found in the configuration beginning at index idx *)
+PROCEDURE ParseEndpointDescriptor(buffer : Usbdi.Buffer; VAR idx : LONGINT) : EndpointDescriptor;
+VAR endpoint : EndpointDescriptor; dword : SET;
+
+	PROCEDURE GetEndpointType(address, attributes : SET) : LONGINT;
+	VAR type : LONGINT;
+	BEGIN
+		IF attributes * {0..1} = {} THEN
+			type := Usbdi.Control;
+		ELSE
+			IF address * {7} = {} THEN (* direction = OUT *)
+				IF attributes * {0..1} = {0} THEN type := Usbdi.IsochronousOut;
+				ELSIF attributes * {0..1} = {1} THEN type := Usbdi.BulkOut;
+				ELSE type := Usbdi.InterruptOut;
+				END;
+			ELSE (* direction = IN *)
+				IF attributes * {0..1} = {0} THEN type := Usbdi.IsochronousIn;
+				ELSIF attributes * {0..1} = {1} THEN type := Usbdi.BulkIn;
+				ELSE type := Usbdi.InterruptIn;
+				END;
+			END;
+		END;
+		RETURN type;
+	END GetEndpointType;
+
+BEGIN
+	IF (Debug.Trace & Debug.traceParsing) & (idx+1 < LEN(buffer)) THEN ShowParse("endpoint",idx, ORD(buffer[idx+1]), ORD(buffer[idx+0])); END;
+	(* Skip non-USB-standard descriptors (e.g. HID descriptors) *)
+	SkipOthers(DescriptorEndpoint, buffer, idx);
+	IF idx + 6 >= LEN(buffer) THEN
+		IF Debug.Level >= Debug.Errors THEN KernelLog.String("Usb: ParseEndpoint: Buffer too short."); KernelLog.Ln; END;
+		RETURN NIL;
+	END;
+	ASSERT(ORD(buffer[idx+1])=DescriptorEndpoint);
+	NEW(endpoint);
+	endpoint.bLength := ORD(buffer[idx + 0]);
+	endpoint.bEndpointAddress := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, ORD(buffer[idx + 2])) * {0..3, 7});
+	endpoint.bmAttributes := SYSTEM.VAL(SET, ORD(buffer[idx + 3]));
+	endpoint.type := GetEndpointType(SYSTEM.VAL(SET, endpoint.bEndpointAddress), endpoint.bmAttributes);
+	dword := SYSTEM.VAL(SET, ORD(buffer[idx + 4]) + 256*SYSTEM.VAL(LONGINT, ORD(buffer[idx + 5])));
+	endpoint.wMaxPacketSize := SYSTEM.VAL(LONGINT, dword * {0..10});
+	endpoint.mult := LSH(SYSTEM.VAL(LONGINT, dword * {11..12}), -11) + 1;
+	endpoint.bInterval := ORD(buffer[idx + 6]);
+	idx := idx + ORD(buffer[idx + 0]);
+	RETURN endpoint;
+END ParseEndpointDescriptor;
+
+(* Parse the first interface descriptor beginning at index idx  including its endpoints *)
+PROCEDURE ParseInterfaceDescriptor(buffer :Usbdi.Buffer; VAR idx : LONGINT) : InterfaceDescriptor;
+VAR interface : InterfaceDescriptor; e : LONGINT;
+BEGIN
+	IF (Debug.Trace & Debug.traceParsing) & (idx+1 < LEN(buffer))  THEN ShowParse("interface",idx, ORD(buffer[idx+1]), ORD(buffer[idx+0])); END;
+	(* Skip non-USB-standard descriptors (e.g. HID descriptors) *)
+	SkipOthers(DescriptorInterface, buffer, idx);
+	IF idx + 8 >= LEN(buffer) THEN
+		IF Debug.Level >= Debug.Errors THEN KernelLog.String("Usb: ParseInterface: Buffer too short."); KernelLog.Ln; END;
+		RETURN NIL;
+	END;
+	ASSERT(ORD(buffer[idx + 1])=DescriptorInterface);
+	NEW(interface);
+	interface.bLength := ORD(buffer[idx + 0]);
+	interface.bInterfaceNumber := ORD(buffer[idx + 2]);
+	interface.bAlternateSetting := ORD(buffer[idx + 3]);
+	interface.bNumEndpoints := ORD(buffer[idx + 4]);
+	interface.bInterfaceClass := ORD(buffer[idx + 5]);
+	interface.bInterfaceSubClass := ORD(buffer[idx + 6]);
+	interface.bInterfaceProtocol := ORD(buffer[idx + 7]);
+	interface.iInterface := ORD(buffer[idx + 8]);
+
+	idx := idx + ORD(buffer[idx + 0]);
+
+	(* Interface can have zero endpoints (only containing endpoint 0) *)
+	IF (interface.bNumEndpoints > 0)  THEN
+		NEW(interface.endpoints, interface.bNumEndpoints);
+		FOR e := 0 TO interface.bNumEndpoints-1 DO
+			interface.endpoints[e] := ParseEndpointDescriptor(buffer, idx);
+			IF interface.endpoints[e] = NIL THEN RETURN NIL; END;
+		END;
+	END;
+	RETURN interface;
+END ParseInterfaceDescriptor;
+
+(* Parse the configuration descriptor including all standard interfaces, alternate interfaces and endpoints. *)
+PROCEDURE ParseConfigurationDescriptor(buffer : Usbdi.Buffer) : ConfigurationDescriptor;
+VAR configuration : ConfigurationDescriptor; i, j, idx, num, intfNbr : LONGINT;
+
+	(* Return the number of alternate interfaces of interface <intf> starting at idx *)
+	PROCEDURE NumAltInterfaces(intf, idx : LONGINT) : LONGINT;
+	VAR res : LONGINT;
+	BEGIN
+		WHILE(idx + 3 < LEN(buffer)) DO
+			IF (ORD(buffer[idx+1]) = DescriptorInterface) & (ORD(buffer[idx+2]) = intf) & (ORD(buffer[idx+3]) # 0) THEN
+				INC(res);
+			END;
+			idx := idx + ORD(buffer[idx+0]);
+		END;
+		RETURN res;
+	END NumAltInterfaces;
+
+BEGIN
+	IF Debug.Trace & Debug.traceParsing THEN
+		ShowParse("configuration",idx, ORD(buffer[idx+1]), ORD(buffer[idx+0]));
+		KernelLog.String("Usb: Total Length of configuration: "); KernelLog.Int(LEN(buffer), 0); KernelLog.Ln;
+	END;
+	NEW(configuration);
+	configuration.bLength := ORD(buffer[0]);
+	configuration.wTotalLength := ORD(buffer[2])+ 256*SYSTEM.VAL(LONGINT, ORD(buffer[3]));
+	configuration.bNumInterfaces := ORD(buffer[4]);
+	configuration.bConfigurationValue := ORD(buffer[5]);
+	configuration.iConfiguration := ORD(buffer[6]);
+	configuration.bmAttributes := SYSTEM.VAL(SET, ORD(buffer[7]));
+	configuration.bMaxPower := 2*ORD(buffer[8]);
+
+	IF SYSTEM.VAL(SET, ORD(buffer[7])) * {5} # {} THEN configuration.remoteWakeup := TRUE; END;
+	IF SYSTEM.VAL(SET, ORD(buffer[7])) * {6} # {} THEN configuration.selfPowered := TRUE;  END;
+
+	idx := configuration.bLength; (* idx points to first interface or IAD*)
+
+	NEW(configuration.interfaces, configuration.bNumInterfaces); (* Always > 0 *)
+
+	FOR i := 0 TO configuration.bNumInterfaces-1 DO
+
+		IF idx + 1 >= LEN(buffer) THEN
+			IF Debug.Level >= Debug.Errors THEN KernelLog.String("Usb: ParseConfiguration: Buffer too short."); KernelLog.Ln; END;
+			RETURN NIL;
+		END;
+		SkipOthers(DescriptorInterface, buffer, idx);
+		IF ORD(buffer[idx+1]) = DescriptorInterface THEN
+			intfNbr := ORD(buffer[idx+2]);
+			configuration.interfaces[i] := ParseInterfaceDescriptor(buffer, idx);
+			IF configuration.interfaces[i] = NIL THEN RETURN NIL; END;
+			num := NumAltInterfaces(intfNbr, idx);
+			IF Debug.Trace & Debug.traceParsing THEN
+				KernelLog.String("Usb: Parsing: "); KernelLog.Int(num, 0); KernelLog.String(" alternate interfaces found."); KernelLog.Ln;
+			END;
+			IF num # 0 THEN
+				configuration.interfaces[i].numAlternateInterfaces := num;
+				NEW(configuration.interfaces[i].alternateInterfaces, num);
+				FOR j := 0 TO num-1 DO
+					configuration.interfaces[i].alternateInterfaces[j] := ParseInterfaceDescriptor(buffer, idx);
+					IF configuration.interfaces[i].alternateInterfaces[j] = NIL THEN RETURN NIL; END;
+				END;
+			END;
+		END;
+	END;
+	RETURN configuration;
+END ParseConfigurationDescriptor;
+
+(* Skip all descriptors except those with the specified type *)
+PROCEDURE SkipOthers(type : LONGINT; buffer : Usbdi.Buffer; VAR idx : LONGINT);
+BEGIN
+	(* Skip non-USB-standard descriptors (e.g. HID descriptors) *)
+	WHILE(idx+1 < LEN(buffer)) & (ORD(buffer[idx+1]) # type) (* & (ORD(buffer[idx+1]) # DescriptorIad) *) DO
+		IF Debug.Trace & Debug.traceParsing THEN ShowParse("Skip descriptor", idx, ORD(buffer[idx+1]), ORD(buffer[idx+0])); END;
+		idx := idx + ORD(buffer[idx+0]);
+	END;
+END SkipOthers;
+
+PROCEDURE ShowParse(CONST string : ARRAY OF CHAR; index, type, length : LONGINT);
+BEGIN
+	IF Debug.Trace THEN
+	KernelLog.String("Usb: Parsing "); KernelLog.String(string); KernelLog.String(" at index "); KernelLog.Int(index, 0);
+	KernelLog.String(" (Type: "); KernelLog.Int(type, 0); KernelLog.String(", Length: "); KernelLog.Int(length, 0); KernelLog.String(")"); KernelLog.Ln;
+	END;
+END ShowParse;
+
+(* Reads StringDescriptors from USBdevice dev if any available *)
+PROCEDURE GetStrings*(dev : UsbDevice);
+VAR
+	buffer : Usbdi.BufferPtr;
+	langid : LONGINT;
+	i, j, k, len : LONGINT;
+	configuration : ConfigurationDescriptor;
+	interface, altInterface : InterfaceDescriptor;
+
+	PROCEDURE GetString(descriptorIndex, langID : LONGINT) :  Lib.UnicodeString;
+	VAR unicode : Lib.UnicodeString; size, i, len : LONGINT; res : BOOLEAN;
+	BEGIN
+		(*First, get the length of the string descriptor to be loaded... *)
+		NEW(buffer, 2);
+		res := dev.GetDescriptor(DescriptorString,  descriptorIndex, langID, 2, buffer);
+		IF (res = TRUE) & (ORD(buffer[1]) = DescriptorString) & (ORD(buffer[0]) > 3) & (ORD(buffer[0]) MOD 2 = 0) THEN
+			(* ... and then load the string descriptor *)
+			len := ORD(buffer[0]); NEW(buffer, len);
+			IF dev.GetDescriptor(DescriptorString, descriptorIndex, langID, len, buffer) THEN
+				(* ORD(buffer[0]) (length in bytes) - 2 (descriptortype and length field) DIV 2 : device delivers 16byte per character *)
+				size :=  ((ORD(buffer[0])-2) DIV 2);
+				NEW(unicode, size);
+				(* Convert ARRAY OF CHAR to ARRAY OF LONGINT *)
+				FOR i:=0 TO size-1 DO
+					unicode[i] := ORD(buffer[(2*i)+2])+SYSTEM.VAL(LONGINT, ORD(buffer[(2*i)+3]))*100H;
+				END;
+			ELSIF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Couldn't load string descriptor"); KernelLog.Ln;
+			END;
+		ELSIF Debug.Level >= Debug.Warnings  THEN KernelLog.String("Usb: Couldn't get the first 2 bytes of the string descriptor"); KernelLog.Ln;
+		END;
+		RETURN unicode;
+	END GetString;
+
+BEGIN
+	IF Debug.Trace & Debug.traceParsing THEN KernelLog.String("Usb: Read string descriptors... "); KernelLog.Ln; END;
+	IF (dev.descriptor(DeviceDescriptor).iManufacturer=0) & (dev.descriptor(DeviceDescriptor).iProduct=0) & (dev.descriptor(DeviceDescriptor).iSerialNumber=0) THEN (* no string describtors supported *)
+		IF Debug.Trace & Debug.traceParsing THEN KernelLog.String("Usb: No String Descriptors provided by this device."); KernelLog.Ln; END;
+	ELSE
+		(* first get the length of the LANGID code array *)
+		NEW(buffer, 2);
+		IF ~dev.GetDescriptor(DescriptorString, 0, 0, 2,  buffer) OR (ORD(buffer[1]) # DescriptorString) THEN
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Couldn't load the first 2 bytes of StringDescriptor"); KernelLog.Ln; END;
+			RETURN;
+		END;
+
+		(* Get the LANDID code array *)
+		len := ORD(buffer[0]); NEW(buffer, len);
+		IF ~dev.GetDescriptor(DescriptorString, 0, 0, len, buffer) OR (ORD(buffer[1]) # DescriptorString) THEN
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Couldn't load String Descriptor"); KernelLog.Ln; END;
+			RETURN;
+		END;
+
+		(* Get a preferred LANGID code *)
+		IF LangIdSupported(buffer, IdEnglishUS) THEN langid := IdEnglishUS;
+		ELSIF LangIdSupported(buffer, IdEnglishUK) THEN langid := IdEnglishUK;
+		ELSIF LangIdSupported(buffer, IdSystemDefault) THEN langid := IdSystemDefault;
+		ELSIF LangIdSupported(buffer, IdUserDefault) THEN langid := IdUserDefault;
+		ELSIF ORD(buffer[0])-2 > 0 THEN (* at least one other language is supported... use it *)
+			langid := ORD(buffer[3]) + 100H*SYSTEM.VAL(LONGINT, ORD(buffer[4]));
+			IF Debug.Trace & Debug.traceParsing THEN KernelLog.String("Usb: Used LANGID code for GetString():"); KernelLog.Int(langid, 0); KernelLog.Ln; END;
+		ELSE
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Usb: Couldn't load string descriptor (No supported language found)"); KernelLog.Ln; END;
+			RETURN;
+		END;
+
+		(* Get manufacturer string *)
+		IF dev.descriptor(DeviceDescriptor).iManufacturer # 0 THEN
+			dev.descriptor(DeviceDescriptor).uManufacturer := GetString(dev.descriptor(DeviceDescriptor).iManufacturer, langid);
+			dev.descriptor(DeviceDescriptor).sManufacturer := Lib.Unicode2Ascii(dev.descriptor(DeviceDescriptor).uManufacturer);
+		END;
+
+		(* Get product string *)
+		IF dev.descriptor(DeviceDescriptor).iProduct # 0 THEN
+			dev.descriptor(DeviceDescriptor).uProduct := GetString(dev.descriptor(DeviceDescriptor).iProduct, langid);
+			dev.descriptor(DeviceDescriptor).sProduct := Lib.Unicode2Ascii(dev.descriptor(DeviceDescriptor).uProduct);
+		END;
+
+		(* Get serial number *)
+		IF dev.descriptor(DeviceDescriptor).iSerialNumber # 0 THEN
+			dev.descriptor(DeviceDescriptor).uSerialNumber := GetString(dev.descriptor(DeviceDescriptor).iSerialNumber, 0000H); (* 0000H: Language neutral *)
+			dev.descriptor(DeviceDescriptor).sSerialNumber := Lib.Unicode2Ascii(dev.descriptor(DeviceDescriptor).uSerialNumber);
+		END;
+
+		(* Get string descriptors of the configurations and interfaces if available *)
+		FOR i := 0 TO dev.descriptor.bNumConfigurations-1 DO
+			configuration := dev.configurations[i] (ConfigurationDescriptor);
+
+			IF configuration.iConfiguration#0 THEN (* device provide configuration description(s) *)
+				dev.configurations[i](ConfigurationDescriptor).uConfiguration := GetString(configuration.iConfiguration, langid);
+				dev.configurations[i](ConfigurationDescriptor).sConfiguration := Lib.Unicode2Ascii(dev.configurations[i](ConfigurationDescriptor).uConfiguration);
+			END;
+
+			FOR j := 0 TO configuration.bNumInterfaces -1 DO
+				interface := configuration.interfaces[j] (InterfaceDescriptor);
+
+				IF interface.iInterface#0 THEN (* Device provides interface descriptor(s) *)
+					interface.uInterface := GetString(interface.iInterface, langid);
+					interface.sInterface := Lib.Unicode2Ascii(interface.uInterface);
+				END;
+
+				FOR k := 0 TO interface.numAlternateInterfaces -1 DO
+					altInterface := interface.alternateInterfaces[k] (InterfaceDescriptor);
+
+					IF altInterface.iInterface#0 THEN (* Device provides interface descriptor(s) *)
+						altInterface.uInterface := GetString(altInterface.iInterface, langid);
+						altInterface.sInterface := Lib.Unicode2Ascii(altInterface.uInterface);
+					END;
+				END;
+			END;
+		END;
+	END;
+END GetStrings;
+
+(** Returns TRUE if the <langid> is supported, FALSE otherwise *)
+PROCEDURE LangIdSupported(buffer : Usbdi.Buffer; langid : LONGINT): BOOLEAN;
+VAR supported : BOOLEAN; i : LONGINT;
+BEGIN
+	(* LANDID code array:  buf[0]=length of code array, buf[1]=DecriptorString, buf[2*i]+buf[2*i+1] : LangID codes *)
+	IF (ORD(buffer[0]) MOD 2 # 0) OR (ORD(buffer[0]) < 4) THEN RETURN FALSE; END;
+	(* Check whether langID is a element of the LANGID code array *)
+	FOR i := 2 TO ORD(buffer[0])-2 BY 2 DO
+		IF (ORD(buffer[i])+SYSTEM.VAL(LONGINT, ORD(buffer[i+1]))*100H) = langid THEN supported := TRUE; END;
+	END;
+	RETURN supported;
+END LangIdSupported;
+
+PROCEDURE ShowState(state : LONGINT);
+BEGIN
+	IF Debug.Trace THEN
+	CASE state OF
+	|StateDisconnected: KernelLog.String("Disconnected");
+	|StateAttached: KernelLog.String("Attached");
+	|StatePowered: KernelLog.String("Powered");
+	|StateDefault: KernelLog.String("Default");
+	|StateAddress: KernelLog.String("Address");
+	|StateConfigured: KernelLog.String("Configured");
+	|StateSuspended: KernelLog.String("Suspended");
+	ELSE
+		KernelLog.String("Unknown ("); KernelLog.Int(state, 0); KernelLog.String(")");
+	END;
+	END;
+END ShowState;
+
+PROCEDURE ShowStateTransition(dev : UsbDevice; newState : LONGINT);
+BEGIN
+	IF Debug.Trace THEN
+	KernelLog.String("Usb: Device "); dev.ShowName; KernelLog.String(": State transition from ");
+	ShowState(dev.state); KernelLog.String(" to "); ShowState(newState); KernelLog.Ln;
+	END;
+END ShowStateTransition;
+
+PROCEDURE GetRootHubs*(VAR rootHubsCopy : RootHubArray);
+VAR i : LONGINT;
+BEGIN {EXCLUSIVE}
+	IF rootHubs = NIL THEN rootHubsCopy := NIL; RETURN; END;
+	IF (rootHubsCopy = NIL) OR (LEN(rootHubs) # LEN(rootHubsCopy)) THEN
+		NEW(rootHubsCopy, LEN(rootHubs));
+	END;
+	FOR i := 0 TO LEN(rootHubs)-1 DO
+		rootHubsCopy[i] := rootHubs[i];
+	END;
+END GetRootHubs;
+
+PROCEDURE RootHubEvent(event : LONGINT; plugin : Plugins.Plugin);
+VAR hcd : UsbHcdi.Hcd;
+BEGIN
+	hcd := plugin(UsbHcdi.Hcd);
+	IF event = Plugins.EventAdd THEN
+		AddRootHub(hcd);
+	ELSIF event = Plugins.EventRemove THEN
+		RemoveRootHub(hcd);
+	ELSE
+		HALT(90);
+	END;
+	Machine.AtomicInc(nbrOfTopologyEvents);
+END RootHubEvent;
+
+PROCEDURE AddRootHub(hcd : UsbHcdi.Hcd);
+VAR roothub : EmulatedHubDevice; temp : RootHubArray; i : LONGINT;
+BEGIN {EXCLUSIVE}
+	NEW(roothub, hcd);
+	IF rootHubs = NIL THEN
+		NEW(rootHubs, 1);
+		rootHubs[0] := roothub;
+	ELSE
+		NEW(temp, LEN(rootHubs)+1);
+		FOR i := 0 TO LEN(rootHubs)-1 DO
+			temp[i] := rootHubs[i];
+		END;
+		temp[LEN(rootHubs)] := roothub;
+		rootHubs := temp;
+	END;
+	drivers.ProbeDevice(roothub);
+END AddRootHub;
+
+PROCEDURE RemoveRootHub(hcd : UsbHcdi.Hcd);
+VAR i, j : LONGINT; temp : RootHubArray; roothub : EmulatedHubDevice;
+BEGIN {EXCLUSIVE}
+	IF rootHubs # NIL THEN
+		IF LEN(rootHubs) > 1 THEN
+			NEW(temp, LEN(rootHubs)-1);
+			j := 0;
+			FOR i := 0 TO LEN(rootHubs)-1 DO
+				IF rootHubs[i].controller = hcd THEN
+					roothub := rootHubs[i] (EmulatedHubDevice);
+				ELSE
+					IF j < LEN(temp) THEN temp[j] := rootHubs[i]; END; INC(j);
+				END;
+			END;
+		ELSE
+			IF rootHubs[0].controller = hcd THEN
+				roothub := rootHubs[0] (EmulatedHubDevice);
+			END;
+		END;
+
+		IF roothub # NIL THEN (* Found device to be removed *)
+			rootHubs := temp;
+			roothub.Remove;
+		END;
+	END;
+END RemoveRootHub;
+
+PROCEDURE InstallRootHubs;
+VAR table : Plugins.Table; i : LONGINT;
+BEGIN
+	UsbHcdi.controllers.AddEventHandler(RootHubEvent, i); (* ignore res *)
+	UsbHcdi.controllers.GetAll(table);
+	IF table # NIL THEN
+		FOR i := 0 TO LEN(table)-1 DO AddRootHub(table[i](UsbHcdi.Hcd)); END;
+	END;
+END InstallRootHubs;
+
+PROCEDURE Cleanup;
+BEGIN {EXCLUSIVE}
+	UsbDriverLoader.SetListener(NIL);
+	drivers.Terminate;
+	Plugins.main.Remove(usbDrivers);
+	IF Debug.Verbose THEN KernelLog.Enter; KernelLog.String("Usb: USB driver unloaded."); KernelLog.Exit; END;
+END Cleanup;
+
+BEGIN
+	(* System wide registry for USB device drivers *)
+	NEW(usbDrivers, "Usb","USB Device Drivers");
+
+	(* Create internal driver registry *)
+	NEW(drivers); Usbdi.drivers := drivers;
+
+	ASSERT(UsbHcdi.StateDisconnected = StateDisconnected);
+
+	Modules.InstallTermHandler(Cleanup);
+	InstallRootHubs;
+
+	(* Install a notifier that will be called when the driver lookup service is enabled. *)
+	UsbDriverLoader.SetListener(drivers.ProbeDrivers);
+
+	IF Debug.Verbose THEN KernelLog.Enter; KernelLog.String("Usb: USB driver loaded."); KernelLog.Exit; END;
+END Usb.
+
+

source/ARM.UsbBuffers.Mod

@@ -0,0 +1,60 @@
+MODULE UsbBuffers; (** AUTHOR ""; PURPOSE ""; *)
+
+CONST
+	Align = 32;
+
+TYPE
+	BufferPtr * = Buffer;
+	Buffer * = OBJECT
+	VAR
+		data: POINTER TO ARRAY OF CHAR;
+		ofs: LONGINT;
+
+		PROCEDURE & SetSize * (size: LONGINT);
+		BEGIN
+			NEW(data, size + Align);
+			ofs := Align - ADDRESSOF(data[0]) MOD Align;
+			ASSERT(ADDRESSOF(data[ofs]) MOD Align = 0)
+		END SetSize;
+
+		PROCEDURE ToArray * (): POINTER TO ARRAY OF CHAR;
+		VAR
+			ptr: POINTER TO ARRAY OF CHAR;
+			i: LONGINT;
+		BEGIN
+			NEW(ptr, LEN(data) - Align);
+			FOR i := 0 TO LEN(ptr) - 1 DO ptr[i] := data[ofs + i] END;
+			RETURN ptr
+		END ToArray;
+
+		PROCEDURE ToArrayOfs * (): LONGINT;
+		BEGIN
+			RETURN ofs
+		END ToArrayOfs;
+
+		OPERATOR "[]" * (idx: LONGINT): CHAR;
+		BEGIN
+			ASSERT(idx >= 0, 7);
+			ASSERT(idx < LEN(data) - Align, 7);
+			RETURN data[ofs + idx]
+		END "[]";
+
+		OPERATOR "[]" * (idx: LONGINT; val: CHAR);
+		BEGIN
+			ASSERT(idx >= 0, 7);
+			ASSERT(idx < LEN(data) - Align, 7);
+			data[ofs + idx] := val
+		END "[]";
+	END Buffer;
+
+	PROCEDURE GetDataAddress * (buffer: Buffer): ADDRESS;
+	BEGIN
+		ASSERT(ADDRESSOF(buffer.data[buffer.ofs]) # 0);
+		RETURN ADDRESSOF(buffer.data[buffer.ofs])
+	END GetDataAddress;
+
+	OPERATOR "LEN" * (buffer: Buffer): LONGINT;
+	BEGIN
+		RETURN LEN(buffer.data) - Align
+	END "LEN";
+END UsbBuffers.

source/ARM.UsbEhci.Mod

@@ -0,0 +1,3250 @@
+MODULE UsbEhci; (** AUTHOR "staubesv"; PURPOSE "USB Enhanced Host Controller Driver"; *)
+(**
+ * Bluebottle USB Enhanced Host Controller Driver
+ * Implements the UsbHcdi host controller driver interface (HCDI)
+ *
+ * Usage:
+ *
+ * This module provides the core implementation of the EHCI driver. Finding EHCI controllers was moved to a separate
+ * module to increase flexibility. If your EHCI controller is accessible with PCI, the command:
+ * 		UsbEhciPCI.Install ~
+ * will install EHCI drivers as needed. To remove them, simply uninstall Ehci modules:
+ * 		SystemTools.Free UsbEhciPCI UsbEhci ~
+ *
+ * In other cases, specific modules can be written, that will instanciate drivers as needed.
+ *
+ * References:
+ *	Enhanced Host Controller Interface Specification for Universal Serial Bus, Rev. 1.0
+ *
+ * Notes:
+ * - 64bit control data structures: This driver doesn`t support real 64bit operation. If the host controller indicates 64bit capabilities, i.e. all
+ *   pointers used for control data structures as qTd, QHs and buffers are 64bit memory addresses, the upper 32bit of the address are just set to zero
+ *   to selected the 0..4GB segment.
+ *
+ * TODOs:
+ * - FSTN save path pointers
+ * - use sparse tree for more fine granular scheduling
+ * - implement isochronous transfers
+ *	- DataToggle would not work for Control Transfers spanning multiple TDs
+ *
+ * History:
+ *
+ *	24.11.2005	First release (staubesv)
+ * 	15.12.2005	Moved buffer checks to UsbHcdi.Mod (staubesv)
+ *	05.01.2006	Fixed EnhancedHostController.DeleteTranfer (staubesv)
+ *	10.01.2006	H/W scatter/gather support implemented, fixed bug when sending > 16KB blocks (staubesv)
+ *	16.01.2006	FlushPCI added (staubesv)
+ *	08.01.2006	Added ScheduleOn, more safe TD unlinked/QH removal (staubesv)
+ *	01.03.2006	Fixed critical bug in CreateTDList (staubesv)
+ *	02.03.2006 	Implemented port indicator control (staubesv)
+ *	03.04.2006	Improved interrupt sharing (staubesv)
+ *	05.04.2006	Fixed BIOS-to-OS handoff (staubesv)
+ *	28.06.2006	Use KernelLog.Hex instead of UsbHcdi.PrintHex (staubesv)
+ *	30.06.2006	Bugfix in InitFramelist: Also reset QH structure for isochronousQH, fixed bug when removing periodic QHs (staubesv)
+ *	03.06.2006	UpdatePipe removed (staubesv)
+ *	04.06.2006 	Allow LinkTD to automatically clear halt condition (staubesv)
+ *	20.07.2006	Release HC ownership when HC driver is shutdown, introduced OverrideHcOwnership constant (staubesv)
+ *	03.08.2006	Adapted to UsbHcdi, fixed control transfer > 1TD (staubesv)
+ *	13.11.2006	UpdatePipeStatus: Set pipe.status to Usbdi.Stalled when a stall is detected (staubesv)
+ *)
+
+IMPORT SYSTEM, KernelLog, Machine, Kernel, Objects, Modules, Locks, UsbHcdi, Usbdi, Debug := UsbDebug, UsbVarTdAlloc, UsbBuffers;
+
+CONST
+
+	Description * = "USB Enhanced Host Controller";
+
+	(* Some configuration stuff *)
+	HcInterruptThreshold = 01H; (* Maximum rate at which the host controller will issue interrupts (in microframes, 125 microseconds) *)
+	HcFrameListSize = 1024;
+
+	(** Size of TD buffers *)
+	TdBufferSize = 4096;
+
+	ScatterGatherListSize = 4200; (* Number of entries in scatter/gather list -> limits the maximum transfer size *)
+
+	(* How many transaction of a single queue head in the asynchronous schedule list is the host controller allowed to execute
+	within a micro-frame? Valid values: [0..3] (0 disables the feature).  Will only be set if the HC supports the Asynchronous Schedule Park Mode. Higher values
+	should result in higher performance. *)
+	HcAsyncParkModeCount = 3;
+
+	(* Host Controller Capability Registers *)
+	HcCapLength = 00H;
+	HcCapHciVersion = 02H;
+	HcCapSparams = 04H;
+	HcCapCparams * = 08H;
+	HcCapPortroute = 0CH;
+
+	(* Host Controller Operational Registers *)
+	HcUsbCmd * = 00H;
+	HcUsbSts * = 04H;
+	HcUsbIntr * = 08H;
+	HcFrIndex * = 0CH;
+	HcCtrlDsSegment * = 10H;
+	HcPeriodicListBase * = 14H;
+	HcAsyncListAddr * = 18H;
+	HcConfigFlag * = 40H;
+	HcPortSc * = 44H;
+
+	(* HcUsbCmd register fields *)
+	CmdInterruptThreshold * = {16..23};
+	CmdAsyncSchedParkMode * = {11};
+	CmdAsyncSchedParkCount * = {8..9};
+	CmdLightHcReset * = {7}; (* Note: optional *)
+	CmdAsyncAdvDoorbell * = {6};
+	CmdAsyncSchedEnable * = {5};
+	CmdPeriodicSchedEnable * = {4};
+	CmdFrameListSize * = {2 .. 3};
+	CmdHcReset * = {1};
+	CmdRunStop * = {0};
+	CmdReserved * = {10} + {12..15} + {24..31};
+
+	(* HcUsbSts register fields *)
+	StsAsyncSchedule * = {15};
+	StsPeriodicSchedule * = {14};
+	StsReclamation * = {13};
+	StsHcHalted * = {12};
+	(* HcUsbSts & HcUsbIntr common fields *)
+ 	StsAsyncAdvance * = {5};
+	StsHostSystemError * = {4};
+	StsFrameListRollover * = {3};
+	StsPortChange * = {2};
+	StsUsbError * = {1};
+	StsUsbInterrupt * = {0};
+
+	(* Port Status & Control register, EHCIspec p. 26-30 *)
+	PscWakeOnOvercurrent = {22};
+	PscWakeOnDisconnect = {21};
+	PscWakeOnConnect = {20};
+	PscTestControl = {16..19};
+	PscIndicatorControl = {14..15};
+	PscPortOwner = {13};
+	PscPortPower * = {12};
+	PscLineStatus = {10..11};
+	PscPortReset = {8};
+	PscSuspend = {7};
+	PscForcePortResume = {6};
+	PscOvercurrentChange = {5};
+	PscOvercurrentActive = {4};
+	PscPortEnableChange = {3};
+	PscPortEnable *= {2};
+	PscConnectStatusChange = {1};
+	PscCurrentConnectStatus = {0};
+	PscReserved = {9} + {23..31};
+	PscChangeMask = {1, 3, 5};
+
+	(* Queue Element Transfer Descriptor; must be 32byte aligned *)
+	(* Offsets *)
+	QtdNextQtdPointer = 00H;
+	QtdAltNextQtdPointer = 04H;
+	QtdToken = 08H;
+	QtdBufferPtr0 = 0CH;
+	QtdBufferPtr1 = 10H;
+	QtdBufferPtr2 = 14H;
+	QtdBufferPtr3 = 18H;
+	QtdBufferPtr4 = 1CH;
+	QtdExtBufferPtr0 = 20H;
+	QtdExtBufferPtr1 = 24H;
+	QtdExtBufferPtr2 = 28H;
+	QtdExtBufferPtr3 = 2CH;
+	QtdExtBufferPtr4 = 30H;
+	(* Masks *)
+	QtdTerminate = {0};
+	QtdBufferPtr = {12..31};
+
+	(* qTD Token *)
+	QtdDataToggle = {31};
+	QtdBytesToTransfer = {16..30};
+	QtdIoc = {15}; (* Interrupt on complete *)
+	QtdCurrentPage = {12..14};
+	QtdErrorCounter = {10..11};
+	QtdPidCode = {8..9};
+	QtdStatus = {0..7};
+
+	(* Isochronous Transfer Descriptor *)
+	ItdNextLinkPointer = 00H;
+	ItdTransaction0 = 04H;
+	ItdBufferPtr0 = 024H;
+	ItdBufferPtr1 = 028H;
+	ItdBufferPtr2 = 02CH;
+	ItdExtBufferPtr0 = 40H;
+
+	(* ITD Transaction *)
+	ItdTransactionStatus = {28..31};
+	ItdTransactionLength = {16..27};
+	ItdTransactionIoc = {15};
+	ItdTransactionPg = {12..14};
+	ItdTransactionOffset = {0..11};
+
+	(* ITD Buffer Pointers *)
+	ItdBufferPtr = {12..31};
+	(* ItdBufferPtr0 *)
+	ItdEndPt = {8..11};
+	ItdReserved = {7};
+	ItdDevAdr = {0..6};
+	(* ItdBufferPtr1 *)
+	ItdIn = {11};
+	ItdMaxPacketSize = {0..10};
+	(* ItdBufferPtr2 *)
+	ItdMult = {0..1};
+
+	(* ITD Transaction Status *)
+	ItdStatus = {28..31};
+	ItdActive = {31};
+	ItdDataBufferError = {30};
+	ItdBabbleDetected = {29};
+	ItdTransactionError = {28};
+
+	(* Queue Head *)
+	(* Offsets *)
+	QhHorizontalLinkPointer = 00H;
+	QhEpCapabilities1 = 04H;
+	QhEpCapabilities2 = 08H;
+	QhCurrentQtdPointer = 0CH;
+	QhNextQtdPointer = 10H;
+	QhAltNextQtdPointer = 14H;
+	QhQtdToken = 18H;
+	QhBufferPointer0 = 1CH;
+	QhBufferPointer1 = 20H;
+	QhBufferPointer2 = 24H;
+	QhBufferPointer3 = 28H;
+	QhBufferPointer4 = 2CH;
+	QhExtBufferPointer0 = 30H;
+	QhExtBufferPointer1 = 34H;
+	QhExtBufferPointer2 = 38H;
+	QhExtBufferPointer3 = 3CH;
+	QhExtBufferPointer4 = 40H;
+
+	(* Masks *)
+	(* Queue Head Horizontal Link Pointer *)
+	QhTyp = {1..2};
+	QhTypItd = 0;
+	QhTypQh = 1;
+	QhTypSitd = 2;
+	QhTypFstn = 3; (* Frame span traversal node *)
+	QhTerminate = {0};
+
+	(* Queue Head Endpoint Capabilities *)
+	(* Dword 1 *)
+	QhNakCountReload = {28..31};
+	QhControlEndpointFlag = {27};
+	QhMaxPacketLen = {16..26};
+	QhHeadOfReclamation = {15};
+	QhDataToggleControl = {14};
+	QhEndpointSpeed = {12..13};
+	QhEndpointNbr = {8..11};
+	QhInactivate = {7};
+	QhDeviceAddress = {0..6};
+	(* Dword 2 *)
+	QhMultiplier = {30..31}; (* High-Bandwidth Pipe Muliplier *)
+	QhPortNbr = {23..29};
+	QhHubAddr = {16..22};
+	QhSplitCMask = {8..15};
+	QhSMask = {0..7};
+
+	(* Periodic Frame Span Traversal Node (FSTN) *)
+	(* FSTN offsets *)
+	FstnNormalPathLinkPointer = 0;
+	FstnBackPathLinkPointer = 4;
+
+	(* Status fields of qTD Token *)
+	TdActive = {7}; (* If set, the HC will process the qTD *)
+	TdHalted = {6}; (* Caused by babble, error counter transition from one to zero or STALL handshake. Will also clear TdActive. *)
+	TdDataBufferError = {5}; (* Buffer overrun or underrun *)
+	TdBabbleDetected = {4}; (* Babble. Will also set TdHalted *)
+	TdTransactionError = {3}; (* No valid response from device during status update (Timeout, CRC errir, PID wrong...) *)
+	TdMissedMicroFrame = {2};
+	TdSplitTransactionState = {1};
+	TdPingState = {0};
+
+	(* Periodic Frame Span Traversal Node *)
+	FstnNormalPathLink = 00H;
+	FstnBackPathLink = 04H;
+
+	(* Packet Identifier codes *)
+	PidOut = 0;
+	PidIn = 1;
+	PidSetup = 2;
+
+	PageSize = 4096;
+
+	Polling = FALSE;
+
+TYPE
+	Qh = POINTER {UNSAFE,UNTRACED} TO RECORD
+		horizontalLink: Qh;
+		epCapabilities: ARRAY 2 OF LONGINT;
+		current, next, alternate: ADDRESS;
+		token: LONGINT;
+		buffers: ARRAY 5 OF LONGINT;
+		(** For 64bits datastructures *)
+		extBuffers: ARRAY 5 OF LONGINT;
+	END;
+
+	Qtd = POINTER {UNSAFE,UNTRACED} TO RECORD
+		next, alternateNext: ADDRESS;
+		token: LONGINT;
+		buffers: ARRAY 5 OF LONGINT;
+		(** For 64bits datastructures *)
+		extBuffers: ARRAY 5 OF LONGINT;
+	END;
+
+	Itd = POINTER {UNSAFE,UNTRACED} TO RECORD
+		next: ADDRESS;
+		transactions: ARRAY 8 OF LONGINT;
+		buffers: ARRAY 7 OF LONGINT;
+	END;
+
+	EnhancedHostController * = OBJECT (UsbHcdi.Hcd)
+	VAR
+		(** Framelist array. Aligned on 4096, at max 4096 B long *)
+		framelist *: UsbHcdi.AlignedMemSpace;
+		(** Number of 4 B elements actually used in the framelist *)
+		framelistSize: LONGINT;
+		(** Lock for syncrhonizing framelist operations *)
+		framelistLock: Locks.Lock;
+		(** Offset for safe modification of the framelist. Do not modify the framelist less than 1 + framelistOfs of the HC current position. *)
+		framelistOfs: LONGINT;
+
+		pwcr * : LONGINT; (* Port Wake Capability Register; Not implemented by device if pwcr = 0 *)
+
+		(* Information from Host Controller Capability Registers *)
+		(* HCSPARAMS - Structural Parameters *)
+		capDebugPortNumber : LONGINT; 	(* 0: n/a, other: number of debug port (0-15)*)
+		capPortIndicators : BOOLEAN; 		(* Do the ports support port indicator control? *)
+		capNbrOfCompanionHc : LONGINT; 	(* How many companion host controllers are present (0-15) *)
+		capPortsPerCompanion : LONGINT; 	(* Number of ports supported per companion host controller *)
+		capPortRoutingRules : BOOLEAN; 	(* Port routing rules *)
+		capPortPowerControl : BOOLEAN; 	(* Does the HC support Port Power Control? *)
+		capNbrOfPorts : LONGINT; 			(* Number of physical downstream ports implemented by this host controller *)
+		(* HCCPARAMS - Capability Parameters *)
+		capIsoSchedThreshold : LONGINT; 	(* Isochronous Schedule Threshold *)
+		capAsynchSchedPark : BOOLEAN; 	(* Does the controller support the park feature for high-speed transfers? *)
+		capProgrammableFLG : BOOLEAN; 	(* FALSE: use default (1024); TRUE: Frame List size is programmable *)
+		cap64bit : BOOLEAN; 				(* 32 / 64 bit memory pointers in the data structures *)
+
+		(* EHCI Extended Capabilities Pointer. Used in relation with USB legacy support *)
+		eecp : LONGINT;
+
+		(* The size of control data structures is dependent on whether the HC uses 32bit or 64bit address pointers as indicated
+		by the cap64bit field *)
+		sizeQtd, alignQtd : LONGINT;
+		sizeQh, alignQh : LONGINT;
+		sizeItd, alignItd: LONGINT;
+
+		(* HC Companion Port Route Descriptor, NIL of not available.
+		If the capPortRoutingRules is TRUE, the HC provides a description of which port is routed to
+		which companion HC.  *)
+		hcportroute : POINTER TO ARRAY OF LONGINT;
+
+		(* queue heads *)
+		isochronousQh* : LONGINT;
+		interruptQh : POINTER TO ARRAY 11 OF LONGINT;
+
+		(* this array will provide the 16byte aligned TD's for controlTD, bulkTD, isochronousTD and interruptTD[] *)
+		qhlist : UsbHcdi.AlignedMemSpace;
+
+		(** Allocator for TD and QH datastructures *)
+		allocator: UsbVarTdAlloc.Allocator;
+
+		(* The Asynchronous Advance Doorbell interrupt is always enabled by this driver. Since the interrupt handler will
+		clear the bit that were set when it was invoked, it sets hcHandshake to TRUE, so its sticky *)
+		hcHandshake * : BOOLEAN;
+
+		(* Set of all currently enabled interrupts *)
+		interruptsEnabled * : SET;
+		handler: Handler;
+
+		(** Enable power for the specified port *)
+		PROCEDURE EnablePortPower*(port : LONGINT);
+		VAR status : SET;
+		BEGIN
+			status := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			SYSTEM.PUT32(ports[port], status - PscChangeMask + PscPortPower); FlushPCI;
+		END EnablePortPower;
+
+		(** Disable power for the specified port *)
+		PROCEDURE DisablePortPower*(port : LONGINT);
+		VAR status : SET;
+		BEGIN
+			status := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			SYSTEM.PUT32(ports[port], status - PscChangeMask - PscPortPower); FlushPCI;
+		END DisablePortPower;
+
+		(** Enable the specified port.
+		 	The EHCI host controllers do not explicitly support a port enable command. The port will be automatically enabled
+		 	by the host controller after a port reset, if a high-speed capable device is attached to it *)
+		PROCEDURE ResetAndEnablePort*(port : LONGINT) : BOOLEAN;
+		VAR status : SET; mtimer : Kernel.MilliTimer;
+		BEGIN
+			status := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			SYSTEM.PUT32(ports[port], status - PscChangeMask + PscPortReset - PscPortEnable); FlushPCI;
+			Wait(UsbHcdi.PortResetTime); (* >= 10ms, USBspec *)
+			SYSTEM.PUT32(ports[port], status - PscChangeMask - PscPortReset); FlushPCI;
+			Wait(2+1); (* 2ms recovery interval according EHCIspec, p. 28 *)
+			(* The host controller should have automatically enabled this port *)
+			Kernel.SetTimer(mtimer, UsbHcdi.PortEnableTimeout);
+			REPEAT
+				status := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			UNTIL (status * PscPortEnable # {}) OR Kernel.Expired(mtimer);
+			RETURN status * PscPortEnable # {};
+		END ResetAndEnablePort;
+
+		(** Disable the specified port. *)
+		PROCEDURE DisablePort*(port : LONGINT);
+		VAR status : SET;
+		BEGIN
+			status := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			SYSTEM.PUT32(ports[port], status - PscChangeMask- PscPortEnable);
+			FlushPCI;
+		END DisablePort;
+
+		(** Suspend the specified port (selective suspend). *)
+		PROCEDURE SuspendPort*(port : LONGINT) : BOOLEAN;
+		VAR status : SET;
+		BEGIN
+			status := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			IF (status * PscPortEnable # {}) & (status * PscPortOwner = {}) THEN
+				SYSTEM.PUT32(ports[port], status - PscChangeMask + PscSuspend);
+				FlushPCI;
+				RETURN TRUE;
+			END;
+			RETURN FALSE;
+		END SuspendPort;
+
+		(** Resume a selectively suspended port. *)
+		PROCEDURE ResumePort*(port : LONGINT) : BOOLEAN;
+		VAR status : SET; timer : Kernel.Timer;
+		BEGIN
+			status := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			IF status * CmdRunStop = {} THEN
+				(* HC must be running when resume a port. Otherwise, the device would automatically re-renter
+				the suspended mode in 10 ms *)
+				SYSTEM.PUT32(iobase + HcUsbCmd, status + CmdRunStop);
+				FlushPCI;
+			END;
+			status := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			IF (status * PscSuspend # {}) & (status * PscPortOwner = {}) THEN
+				SYSTEM.PUT32(ports[port], status - PscChangeMask + PscForcePortResume);
+				FlushPCI;
+				NEW(timer); timer.Sleep(20); (* EHCI p. 60 *)
+				SYSTEM.PUT32(ports[port], status - PscChangeMask - PscForcePortResume);
+				FlushPCI;
+			END;
+			RETURN SYSTEM.VAL(SET, SYSTEM.GET32(ports[port])) * PscSuspend = {};
+			(* TODO: write 1 to PORTSC Force resume bit if port is suspended; first wait 10ms (EHCIp59)*)
+		END ResumePort;
+
+		(** Suspend all ports and then stop the host controller. *)
+		PROCEDURE Suspend*;
+		VAR dword : SET; i : LONGINT; ignore : BOOLEAN;
+		BEGIN
+			(* Suspend all individual ports *)
+			FOR i := 0 TO portCount - 1 DO ignore := SuspendPort(i); END;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd)); (* Stop HC *)
+			SYSTEM.PUT32(iobase + HcUsbCmd, dword - CmdRunStop);
+			FlushPCI;
+			(* Put HC in lower device state via the PCI power management interface *)
+		END Suspend;
+
+		(** Restart the host controller and selectively resume all suspended ports.  *)
+		PROCEDURE Resume*() : BOOLEAN;
+		VAR dword : SET; i : LONGINT; res : BOOLEAN;
+		BEGIN
+			(* Re-start the HC *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			SYSTEM.PUT32(iobase + HcUsbCmd, dword + CmdRunStop);
+			FlushPCI;
+			(* Resume all individual ports *)
+			res := TRUE;
+			FOR i := 0 TO portCount - 1 DO
+				IF ~ResumePort(i) THEN res := FALSE; END;
+			END;
+			RETURN res;
+		END Resume;
+
+		(**
+		 * Get the status of the specified port.
+		 * Registers which indicate status changes are reset by GetPortStatus.
+		 * Note: UsbHcdi.HighSpeed will only be correctly set when the port is enabled. The hub driver
+		 * takes care of this special behaviour by getting the port status again after it has enabled the port.
+		 * @param port Port to get the status of
+		 * @return Port status
+		 *)
+		PROCEDURE GetPortStatus*(port : LONGINT; ack : BOOLEAN) : SET;
+		VAR status, s : SET;
+		BEGIN
+			s := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			(* Clear all bits that reported a change event; the correspondig register are R/WC *)
+			IF ack & ((s * PscChangeMask) # {}) THEN SYSTEM.PUT32(ports[port], s); END;  FlushPCI;
+			status := {};
+			IF s * PscCurrentConnectStatus # {} THEN status := status + UsbHcdi.PortStatusDevicePresent; END;
+			IF s * PscPortEnable # {} THEN status := status + UsbHcdi.PortStatusEnabled END;
+			IF s * PscSuspend # {} THEN status := status + UsbHcdi.PortStatusSuspended END;
+			IF s * PscOvercurrentActive # {} THEN status := status + UsbHcdi.PortStatusOverCurrent END;
+			IF s * PscPortReset # {} THEN status := status + UsbHcdi.PortStatusReset END;
+			IF s * PscPortPower # {} THEN status := status + UsbHcdi.PortStatusPowered END;
+			IF s * PscConnectStatusChange # {} THEN status := status + UsbHcdi.PortStatusConnectChange END;
+			IF s * PscPortEnableChange # {} THEN status := status + UsbHcdi.PortStatusEnabledChange END;
+			IF s * PscOvercurrentChange # {} THEN status := status + UsbHcdi.PortStatusOverCurrentChange END;
+			IF s * PscTestControl # {} THEN status := status + UsbHcdi.PortStatusTestControl END;
+			IF s * PscIndicatorControl # {} THEN status := status + UsbHcdi.PortStatusIndicatorControl END;
+			IF s * PscWakeOnOvercurrent # {} THEN status := status + UsbHcdi.PortStatusWakeOnOvercurrent; END;
+			IF s * PscWakeOnDisconnect # {} THEN status := status + UsbHcdi.PortStatusWakeOnDisconnect; END;
+			IF s * PscWakeOnConnect # {} THEN status := status + UsbHcdi.PortStatusWakeOnConnect; END;
+			IF s * PscPortOwner # {} THEN status := status + UsbHcdi.PortStatusPortOwner; END;
+			(* When a device is attached to a port of the root hub, the hub driver will try to reset and enable the port.
+			The EHCI HC only enables the port if the connected device is a high-speed device which is determined during
+			the reset. So if a device is attached to the port, the port is not in reset and it's enabled, it is a high-speed device *)
+			IF (s * PscPortEnable = {}) & (s * PscCurrentConnectStatus # {}) & (s * PscPortPower # {}) & (s * {10} # {}) THEN (* Lowspeed device connected *)
+				status := status + UsbHcdi.PortStatusLowSpeed;
+			ELSIF (s * PscCurrentConnectStatus # {}) & (s * PscPortReset = {}) & (s * PscPortEnable # {}) THEN
+				status := status + UsbHcdi.PortStatusHighSpeed;
+			END;
+			RETURN status;
+		END GetPortStatus;
+
+		PROCEDURE HasCompanion*(): BOOLEAN;
+		BEGIN
+			RETURN TRUE
+		END HasCompanion;
+
+		(** Route the specified port to a companion host controller if supported. *)
+		PROCEDURE RoutePortToCompanion*(port : LONGINT);
+		VAR dword : SET;
+		BEGIN
+			(* Assert ports are not globally routed to companion controllers *)
+			ASSERT(SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcConfigFlag)) * {0} # {});
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			SYSTEM.PUT32(ports[port], dword - PscChangeMask + PscPortOwner); FlushPCI;
+		END RoutePortToCompanion;
+
+		(** Indicate a port state using the port indicators *)
+		PROCEDURE IndicatePort*(port, indicate : LONGINT);
+		VAR indicators, dword : SET;
+		BEGIN
+			IF indicate = UsbHcdi.Amber THEN indicators := {14};
+			ELSIF indicate = UsbHcdi.Green THEN indicators := {15};
+			ELSE indicators := {};
+			END;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			dword := dword - PscIndicatorControl + indicators;
+			SYSTEM.PUT32(ports[port], dword); FlushPCI;
+		END IndicatePort;
+
+		(**	Return the current frame number.
+		 	The micro-frame number is incremented each micro-frame, i.e. per 125us. There are 8 micro-frames per frame *)
+		PROCEDURE GetFrameNumber*() : INTEGER;
+		BEGIN
+			RETURN SYSTEM.VAL(INTEGER, SYSTEM.VAL(SET, LSH(SYSTEM.GET32(iobase + HcFrIndex), -3)) * {0..10});
+		END GetFrameNumber;
+
+		(*
+		 * Contruct the queue head of the specified pipe.
+		 * Fill in the following DWORDs into pipe.descriptors[0]:
+		 * - Queue Head Endpoint Capabilities 1
+		 * - Queue Head Endpoint Capabilities 2
+		 * - Current qTD Pointer
+		 * - Next qTD Pointer, Alternate Next qTD Pointer, qTD Token & all five qTD Buffer Pointers
+		 * The Queue Head Horizontal Link Pointer will be set by InsertPipeQH
+		 * @param pipe
+		 *)
+		PROCEDURE BuildQueueHead * (pipe : UsbHcdi.Pipe);
+		VAR
+			qh: Qh;
+			dword: SET;
+			nakRL, multi, mmask: LONGINT;
+		BEGIN
+			ASSERT(pipe.maxPacketSize <= 1024); (* Maximum allowed packet size *)
+
+			(*
+			pipe.descriptors[0] := Align(pipe.descriptors[0], alignQh);
+			pipe.tdBase := pipe.descriptors[0] + alignQh;;
+			*)
+			(*
+			IF ~CheckBoundary(pipe.descriptors[0], sizeQh) THEN
+				INC(pipe.descriptors[0], sizeQh);
+				INC(pipe.tdBase, sizeQh)
+			END;
+			*)
+			ASSERT(pipe.descriptors[0] # 0);
+			qh := pipe.descriptors[0];
+			
+			(* Queue Head Horizontal Link Pointer is not set here *)
+			(* Queue Head Endpoint Capabilities 1 *)
+			nakRL := 3;
+			IF pipe.type = UsbHcdi.PipeInterrupt THEN nakRL := 0; END; (* EHCIspec, p.83 *)
+
+			dword := LSH(SYSTEM.VAL(SET, nakRL), 28) * QhNakCountReload;
+			IF (pipe.speed # UsbHcdi.HighSpeed) & (pipe.type = UsbHcdi.PipeControl) THEN
+				dword := dword + QhControlEndpointFlag;
+			END;
+			IF pipe.type = UsbHcdi.PipeControl THEN dword := dword  + QhDataToggleControl;  END;
+			dword := dword + LSH(SYSTEM.VAL(SET, pipe.maxPacketSize), 16) * QhMaxPacketLen;
+			IF (pipe.speed = UsbHcdi.LowSpeed) THEN (* EPS - endpoint speed *)
+				dword := dword + {12}; (* Low-speed endpoint *)
+			ELSIF (pipe.speed = UsbHcdi.FullSpeed) THEN
+				(* Do nothing; Full-speed endpoint *)
+			ELSIF (pipe.speed = UsbHcdi.HighSpeed) THEN
+				dword := dword + {13}; (* High-speed endpoint *)
+			ELSE
+				HALT(99);
+			END;
+			dword := dword + LSH(SYSTEM.VAL(SET, pipe.endpoint), 8) * QhEndpointNbr;
+			dword := dword + SYSTEM.VAL(SET, pipe.address) * QhDeviceAddress;
+			qh.epCapabilities[0] := SYSTEM.VAL(LONGINT, dword);
+
+			(* Queue Head Endpoint Capabilities 2 *)
+			multi := 1; (* TODO: How many transactions per frame for high-speed isochronous and interrupts transfer are allowed? *)
+			dword := LSH(SYSTEM.VAL(SET, multi), 30) * QhMultiplier;
+
+			IF ((pipe.speed = UsbHcdi.LowSpeed) OR (pipe.speed = UsbHcdi.FullSpeed)) & (pipe.ttAddress # 0) THEN
+				(* Hub port and address for split transaction *)
+				dword := dword + LSH(SYSTEM.VAL(SET, pipe.ttAddress), 16) * QhHubAddr;
+				dword := dword + LSH(SYSTEM.VAL(SET, pipe.ttPort + 1), 23) * QhPortNbr;
+
+				IF pipe.type = UsbHcdi.PipeInterrupt THEN
+					(* In which micro-frames the HC should issue Complete Split tokens *)
+					dword := dword + LSH({2..6}, 8) * QhSplitCMask;
+				END;
+			END;
+
+			mmask := 1;
+			IF (pipe.type = UsbHcdi.PipeInterrupt) OR (pipe.type = UsbHcdi.PipeIsochronous) THEN
+				dword := dword + SYSTEM.VAL(SET, mmask)  * QhSMask;
+			END;
+			qh.epCapabilities[1] := SYSTEM.VAL(LONGINT, dword);
+
+			qh.current := 0;
+
+			(* Zero-out the queue head transfer overlay *)
+			qh.next := SYSTEM.VAL(LONGINT, QhTerminate);
+			qh.alternate := SYSTEM.VAL(LONGINT, QhTerminate);
+			qh.token := 0;
+
+			qh.buffers[0] := 0;
+			qh.buffers[1] := 0;
+			qh.buffers[2] := 0;
+			qh.buffers[3] := 0;
+			qh.buffers[4] := 0;
+			IF cap64bit THEN
+				qh.extBuffers[0] := 0;
+				qh.extBuffers[1] := 0;
+				qh.extBuffers[2] := 0;
+				qh.extBuffers[3] := 0;
+				qh.extBuffers[4] := 0
+			END;
+		END BuildQueueHead;
+
+		(** Build a Queue Head for the specified pipe and insert it into the host controller schedule. *)
+		PROCEDURE InsertQH (pipe : UsbHcdi.Pipe) (*: BOOLEAN*);
+		VAR
+			new, curr: Qh;
+			adr, asyncListAddr, queue: LONGINT;
+			dword: SET;
+		BEGIN (*{EXCLUSIVE}*) (* Call from exclusive sections only *)
+			ASSERT(state = UsbHcdi.Initialized);
+			ASSERT((pipe.maxPacketSize > 0));
+			ASSERT(pipe # NIL);
+			ASSERT((pipe.type = UsbHcdi.PipeControl) OR (pipe.type = UsbHcdi.PipeBulk)
+				OR (pipe.type = UsbHcdi.PipeInterrupt));
+
+			(* Build a new QH *)
+			(*pipe.descriptorLock.Acquire;*)
+			(*ASSERT(pipe.descriptors = NIL);
+			NEW(pipe.descriptors, 1);*)
+			pipe.descriptors[0] := allocator.Allocate(sizeQh);
+			AssertAlignment(pipe.descriptors[0], alignQh);
+			BuildQueueHead(pipe);
+			(*pipe.descriptorLock.Release;*)
+			(*ShowQueueHead(pipe.descriptors[0], 0, cap64bit);*)
+
+			(* Insert bulk and control QHs into circular QH list. *)
+			new := pipe.descriptors[0];
+			IF (pipe.type = UsbHcdi.PipeControl) OR (pipe.type = UsbHcdi.PipeBulk) THEN
+				(* Insert into the asynchronous schedule list *)
+				asyncListAddr := SYSTEM.GET32(iobase + HcAsyncListAddr);
+				IF asyncListAddr = 0 THEN (* Not queue heads in the list yet *)
+					(* Since the address is obviously invalid, the asynchronous schedule mustn't be enabled *)
+					ASSERT(SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * StsAsyncSchedule = {});
+					new.horizontalLink := QhHorizontalLink(new, QhTypQh, FALSE);
+					(* If the asynchronous schedule is enabled, exactly one queue head MUST have the H-bit set. *)
+					dword := SYSTEM.VAL(SET, new.epCapabilities[0]) + QhHeadOfReclamation;
+					new.epCapabilities[0] := SYSTEM.VAL(LONGINT, dword);
+					(*Machine.FlushDCacheRange(new, sizeQh);*)
+					(*ShowQueueHead(new, 0, cap64bit);*)
+
+					(* Insert the queue head into the schedule list and activate the asynchronous schedule *)
+					SYSTEM.PUT32(iobase + HcAsyncListAddr, new); FlushPCI;
+					IF ~ScheduleOn(CmdAsyncSchedEnable, TRUE) & (Debug.Level >= Debug.Errors) THEN Show("Failed to enable async schedule."); KernelLog.Ln; END;
+				ELSE
+					ASSERT(SYSTEM.VAL(SET, asyncListAddr) * {0..4} = {}); (* 32byte alignment *)
+					curr := asyncListAddr;
+					new.horizontalLink := curr.horizontalLink;
+					(*Machine.FlushDCacheRange(new, sizeQh);*)
+
+					(* Insert the newly created queue head into the asynchronous schedule list. *)
+					curr.horizontalLink := QhHorizontalLink(new, QhTypQh, FALSE);
+					(*Machine.FlushDCacheRange(curr, sizeQh);*)
+				END;
+			ELSE (* pipe is interrupt *)
+				curr := SYSTEM.VAL(Qh, pipe.queue);
+				new := pipe.descriptors[0];
+				new.horizontalLink := curr.horizontalLink;
+				(*SYSTEM.PUT32(pipe.qh + QhHorizontalLinkPointer, adr);*)
+				curr.horizontalLink := SYSTEM.VAL(ADDRESS, new) + SYSTEM.VAL(LONGINT, {1} - {2});
+				(*SYSTEM.PUT32(pipe.queue + QhHorizontalLinkPointer, pipe.qh + SYSTEM.VAL(LONGINT, {1} - {2}));*)
+				(*Machine.FlushDCacheRange(SYSTEM.VAL(ADDRESS, new), sizeQh);
+				Machine.FlushDCacheRange(pipe.queue, sizeQh);*)
+				(* Enable the periodic list if necessary *)
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts));
+				IF dword * StsPeriodicSchedule = {} THEN
+					IF ~ScheduleOn(CmdPeriodicSchedEnable, TRUE) THEN
+						IF Debug.Level >= Debug.Errors THEN Show("Could not enable periodic schedule."); KernelLog.Ln; END;
+					END;
+				END;
+			END;
+			IF Debug.Trace & Debug.traceQueuing THEN Show("Inserted QH at "); KernelLog.Address(pipe.descriptors[0]); KernelLog.Ln END;
+			(*Machine.FlushDCacheRange(pipe.descriptors[0], sizeQh)*)
+		END InsertQH;
+
+		(* Enable/Disable the periodic or asynchronous schedule. *)
+		PROCEDURE ScheduleOn(cmd : SET; on : BOOLEAN) : BOOLEAN;
+		VAR dword, sts : SET; mtimer : Kernel.MilliTimer;
+		BEGIN (* Caller must hold obj lock *)
+			ASSERT((cmd = CmdPeriodicSchedEnable) OR (cmd = CmdAsyncSchedEnable));
+			IF Debug.Trace & Debug.traceQueuing THEN
+				IF on THEN Show("Enabling"); ELSE Show("Disabling"); END;
+				IF cmd = CmdAsyncSchedEnable THEN KernelLog.String(" asynchronous schedule."); ELSE KernelLog.String(" periodic schedule."); END;
+				KernelLog.Ln;
+			END;
+			IF cmd = CmdAsyncSchedEnable THEN sts := StsAsyncSchedule; ELSE sts := StsPeriodicSchedule; END;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			ASSERT(dword * cmd = LSH(SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * sts, -10)); (* HcUsbCmd & HcUsbSts in consistent state *)
+			IF on THEN dword := dword + cmd; ELSE dword := dword - cmd; END;
+			SYSTEM.PUT32(iobase + HcUsbCmd, dword); FlushPCI;
+
+			(* Wait until the HC reaches the desired state *)
+			Kernel.SetTimer(mtimer, 500);
+			WHILE ~Kernel.Expired(mtimer) & ((SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * sts # {}) # on) DO
+				Objects.Yield;
+			END;
+			RETURN (SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * sts # {}) = on;
+		END ScheduleOn;
+
+		(*
+		 * Remove a queue head data structure from the host controller's asynchronous schedule.
+		 * The asynchronous schedule is a circular linked list of queue heads. At least one queue heads has
+		 * the H-bit (Head of asynchronous schedule list) set which is used by the host controller to detect
+		 * empty list conditions. There are two cases when we remove a queue head:
+		 * 1) It is the only queue head in the list. In this case, we disabled the asynchronous schedule execution and
+		 *     and remove the queue head then.
+		 * 2) There are other queue heads in the list. If the queue head to be removed is the head of the list, we
+		 *      need to set the H-bit for another queue head.
+		 *
+		 * Precondition: TDs are already removed from the QH, QH is inactive
+		 *)
+		 PROCEDURE RemoveAsyncQH(pipe : UsbHcdi.Pipe);
+		 VAR
+		 	(*start, cur, prev  : LONGINT; dword : SET;*)
+		 	start, curr, prev: Qh;
+		 	dword: SET;
+		 BEGIN (* Caller must hold obj lock *)
+			prev := SYSTEM.GET32(iobase + HcAsyncListAddr);
+			ASSERT((SYSTEM.VAL(ADDRESS, prev) # 0) & (SYSTEM.VAL(SET, prev) * {0..4} = {}));
+			(*Machine.InvalidateDCacheRange(SYSTEM.VAL(ADDRESS, prev), sizeQh);*)
+			prev := prev.horizontalLink;
+			ASSERT((SYSTEM.VAL(SET, prev) * {1} # {}) & (SYSTEM.VAL(SET, prev) * QhTerminate = {})); (* Pointer references queue head *)
+			prev := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, prev) * {5..31});
+			(*Machine.InvalidateDCacheRange(prev, sizeQh);*)
+			curr := prev.horizontalLink;
+			ASSERT((SYSTEM.VAL(SET, curr) * {1} # {}) & (SYSTEM.VAL(SET, curr) * QhTerminate  =  {})); (* Pointer references queue head *)
+			curr := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, curr) * {5..31});
+			(*Machine.InvalidateDCacheRange(curr, sizeQh);*)
+
+			(* prev is the address of the queue head that points to the queue head with the address cur *)
+			IF curr = prev THEN (* Only one queue head in the list *)
+				ASSERT(SYSTEM.VAL(SET, curr.epCapabilities[0]) * QhHeadOfReclamation # {});
+				(*ASSERT(SYSTEM.VAL(SET, curr.token) * TdActive = {});*)
+				IF curr = pipe.descriptors[0] THEN (* just disable asynchronous schedule *)
+					IF ScheduleOn(CmdAsyncSchedEnable, FALSE) THEN
+						SYSTEM.PUT32(iobase + HcAsyncListAddr, 0); FlushPCI; (* Mark as invalid. *)
+						(* Free the queue head *)
+						(*pipe.descriptorLock.Acquire;*)
+						pipe.descriptors := NIL;
+						(*pipe.descriptorLock.Release;*)
+						allocator.Free(curr, sizeQh)
+					ELSIF Debug.Level >= Debug.Errors  THEN Show("Could not disable async schedule."); KernelLog.Ln;
+					END;
+				ELSIF Debug.Level >= Debug.Warnings THEN Show("Failed to remove QH from asynchronous schedule: QH not found."); KernelLog.Ln;
+				END;
+			ELSE (* Find and remove the queue head in the list *)
+				(* Search the queue head that references the queue head to be removed *)
+				start := curr;
+				LOOP
+					dword := SYSTEM.VAL(SET, curr.horizontalLink);
+					ASSERT(dword * QhTerminate = {}); (* Circular lists don't terminate *)
+					ASSERT(dword * {1} # {}); (* Pointer references queue head *)
+					ASSERT(dword * {2..4} = {}); (* qTD pointers must be 32byte aligned *)
+					prev := curr;
+					curr := SYSTEM.VAL(LONGINT, dword * {5..31});
+					IF curr = pipe.descriptors[0] THEN (* QH found *) EXIT; END;
+					IF curr = start THEN (* list completely searched but QH not found *) EXIT; END;
+					(*Machine.InvalidateDCacheRange(curr, sizeQh);*)
+				END;
+
+				IF curr = pipe.descriptors[0] THEN (* Found the queue head. prev is pointing to it *)
+					(* If we remove the head of reclamation, elect a new one *)
+					IF SYSTEM.VAL(SET, curr.epCapabilities[0]) * QhHeadOfReclamation # {} THEN
+						IF Debug.Trace & Debug.traceQueuing THEN Show("Electing new head of reclamation."); KernelLog.Ln; END;
+						dword := SYSTEM.VAL(SET, prev.epCapabilities[0]);
+						prev.epCapabilities[0] := SYSTEM.VAL(LONGINT, dword + QhHeadOfReclamation);
+					END;
+					(* Remove QH from asynchronous list and inforam host controller *)
+					prev.horizontalLink := curr.horizontalLink;
+					(*Machine.FlushDCacheRange(prev, sizeQh);*)
+
+					(* Free QH *)
+					(*pipe.descriptorLock.Acquire;*)
+					pipe.descriptors := NIL;
+					(*pipe.descriptorLock.Release;*)
+					allocator.Free(curr, sizeQh)
+				ELSIF Debug.Level >= Debug.Warnings THEN Show("Failed to remove QH from asynchronous list: QH not found."); KernelLog.Ln;
+				END;
+				(* Before we may free the pipe ressources, we have to make sure that the HC has no cached references to the structure 	*)
+				(* we just removed. 																									*)
+				IF ~HcHandshake() THEN
+					IF Debug.Level >= Debug.Errors THEN Show("UsbEhci: Serious error: HC handshake failed."); KernelLog.Ln; END;
+				END;
+			END;
+			IF Debug.Trace & Debug.traceQueuing THEN Show("Removed QH at "); KernelLog.Address(curr); KernelLog.Ln; END;
+		 END RemoveAsyncQH;
+
+	 	(*
+	 	 * Inform the host controller that we removed something from the asynchronous schedule list. This is
+		 * necessary since the HC could have cached a copy of the pointer to the queue head we've just removed.
+		 *)
+		PROCEDURE HcHandshake() : BOOLEAN;
+		VAR dword : SET; mtimer : Kernel.MilliTimer; result : BOOLEAN;
+		BEGIN (* caller holds object lock *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts));
+			ASSERT(dword * StsAsyncSchedule # {}); (* HC behaviour undefined if ringing doorbell while async schedule is off *)
+			hcHandshake := FALSE;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			SYSTEM.PUT32(iobase + HcUsbCmd, dword + CmdAsyncAdvDoorbell); FlushPCI;
+			Kernel.SetTimer(mtimer, 500);
+			WHILE ~Kernel.Expired(mtimer) & (SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd)) * CmdAsyncAdvDoorbell # {}) DO
+				Objects.Yield;
+			END;
+			result := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd)) * CmdAsyncAdvDoorbell = {}; (* The HC should have cleared the bit *)
+			IF Debug.Trace & Debug.traceQueuing THEN
+				Show("HC handshake "); IF result THEN KernelLog.String("succeeded."); ELSE KernelLog.String("failed."); END; KernelLog.Ln;
+			END;
+			RETURN result;
+		END HcHandshake;
+
+		PROCEDURE RemovePeriodicQH(pipe : UsbHcdi.Pipe);
+		VAR
+			timer : Kernel.Timer;
+			cur, temp : Qh;
+			next : SET;
+		BEGIN (* caller must hold obj lock *)
+			IF pipe.descriptors[0] = 0 THEN RETURN; END;
+			cur := pipe.descriptors[0];
+			LOOP
+				(*Machine.InvalidateDCacheRange(cur, sizeQh);*)
+				next := SYSTEM.VAL(SET, cur.horizontalLink);
+				IF next * {5..31} = SYSTEM.VAL(SET, pipe.descriptors[0]) * {5..31} THEN (* found *)
+					temp := SYSTEM.GET32(pipe.descriptors[0] + QhHorizontalLinkPointer);
+					cur .horizontalLink := temp;
+					(*Machine.FlushDCacheRange(cur, sizeQh);)*)
+					IF Debug.Trace & Debug.traceQueuing THEN KernelLog.String("UsbEhci: Deleted Interrupt Pipe QH."); KernelLog.Ln; END;
+					NEW(timer); timer.Sleep(10); (* HC has still access to QH, wait > 1ms *)
+					EXIT;
+				ELSIF next * QhTerminate # {} THEN (* not found, reached end of list *)
+					IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbEhci: Could not delete interrupt QH -> QH not found."); KernelLog.Ln; END;
+					EXIT;
+				ELSE
+					cur := SYSTEM.VAL(LONGINT, next * {5..31});
+				END;
+			END;
+			IF Debug.Trace & Debug.traceQueuing THEN Show("Removed QH at "); KernelLog.Hex(pipe.descriptors[0], 8); KernelLog.Ln; END;
+		END RemovePeriodicQH;
+
+		(** Remove the pipe's queue head from the host controller schedule *)
+		PROCEDURE RemoveQH (pipe : UsbHcdi.Pipe);
+		BEGIN (*{EXCLUSIVE}*)
+			IF Debug.Trace & Debug.traceQueuing THEN Show("Removing QH at "); KernelLog.Hex(pipe.descriptors[0], 8); KernelLog.Ln; END;
+			(* Assume that no transfer is in progress for this pipe *)
+
+			(* Then remove the pipe's queue head from the host controller schedule *)
+			IF (pipe.type = UsbHcdi.PipeControl) OR (pipe.type = UsbHcdi.PipeBulk) THEN
+				RemoveAsyncQH(pipe);
+			ELSIF pipe.type = UsbHcdi.PipeInterrupt THEN
+				RemovePeriodicQH(pipe);
+			END;
+		END RemoveQH;
+
+		(*(** Checks whether TDs may be linked to the pipe's QH *)
+		PROCEDURE LinkTDsAllowed*(pipe : UsbHcdi.Pipe) : BOOLEAN;
+		VAR dword : SET;
+		BEGIN {EXCLUSIVE}
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(pipe.descriptors[0] + QhQtdToken));
+			IF dword * TdActive # {} THEN
+				IF Debug.Level >= Debug.Errors THEN Show("LinkTDs: ERROR: PIPE IS STILL ACTIVE!!!!"); KernelLog.Ln; END;
+				pipe.status := Usbdi.Error; pipe.errors := pipe.errors + UsbHcdi.LinkTDsFailed;
+				RETURN FALSE;
+			END;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(pipe.descriptors[0] + QhNextQtdPointer));
+			IF dword * QhTerminate = {} THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbEhci: LinkTDs: Overwriten valid pointer ?!?"); KernelLog.Ln; END;
+				pipe.status := Usbdi.Error; pipe.errors := pipe.errors + UsbHcdi.LinkTDsFailed;
+				RETURN FALSE;
+			END;
+			RETURN TRUE;
+		END LinkTDsAllowed;
+
+		(* Insert the TD list <td> into the queue (ED) <queue> *)
+		PROCEDURE LinkTDs*(pipe : UsbHcdi.Pipe;  qtd : Machine.Address32);
+		VAR dword : SET;
+		BEGIN {EXCLUSIVE}
+			ASSERT(SYSTEM.VAL(SET, qtd) * {0..4} = {}); (* 32byte alignment *)
+			(* Pipe must be inactive... *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(pipe.descriptors[0] + QhQtdToken));
+			IF dword * TdHalted # {} THEN
+				IF Debug.Trace & Debug.tracePipes THEN Show("LinkTDs: Automatically clear halt condition"); KernelLog.Ln; END;
+				ClearHalt(pipe);
+			END;
+
+			SYSTEM.PUT32(pipe.descriptors[0] + QhNextQtdPointer, qtd);
+			(*Machine.FlushDCacheRange(pipe.descriptors[0], sizeQh);
+			Machine.FlushDCacheRange(qtd, sizeQtd);*)
+			IF SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * StsAsyncSchedule = {} THEN
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+				IF dword * CmdAsyncSchedEnable = {} THEN
+					IF ~ScheduleOn(CmdAsyncSchedEnable, TRUE) & (Debug.Level >= Debug.Errors) THEN Show("Failed to re-enabled async schedule."); KernelLog.Ln; END;
+				END;
+			END;
+		END LinkTDs;*)
+
+		(** Remove all transfer descriptors from the pipe's queue head *)
+		PROCEDURE UnlinkTDs (transfer: UsbHcdi.TransferToken);
+		VAR
+			i, len: LONGINT;
+			dword: SET;
+			timer: Kernel.Timer;
+			prev, qtd: Qtd;
+			qh: Qh;
+			mtimer : Kernel.MilliTimer;
+		BEGIN (*{EXCLUSIVE}*)
+			(*IF pipe.firstTD = 0 THEN RETURN END; (* pipe has not yet been used *)*)
+			(* We must inactivate all qTD of the queue head... *)
+
+			(* we should wait until the transaction overlay is also inactive *)
+			qh := transfer.pipe.descriptors[0];
+			Kernel.SetTimer(mtimer, 2000);
+			(*Machine.InvalidateDCacheRange(qh, sizeQh);*)
+			dword := SYSTEM.VAL(SET, qh.token);
+			WHILE ~Kernel.Expired(mtimer) & (dword  * TdActive # {}) DO
+				(*Machine.InvalidateDCacheRange(qh, sizeQh);*)
+				dword := SYSTEM.VAL(SET, qh.token);
+				Objects.Yield;
+			END;
+			IF dword * TdActive # {} THEN
+				IF Debug.Level >= Debug.Errors THEN Show("Transaction overlay indicates active transfer!"); KernelLog.Ln; END;
+			END;
+
+			len := LEN(transfer.tds);
+
+			WHILE (i < len) & (transfer.tds[i] # 0) DO
+				qtd := transfer.tds[i];
+				AssertAlignment(qtd, alignQtd);
+				(*Machine.InvalidateDCacheRange(qtd, sizeQtd);*)
+				dword := SYSTEM.VAL(SET, qtd.token);
+				allocator.Free(qtd, sizeQtd);
+				INC(i)
+			END;
+
+			qh.next := SYSTEM.VAL(LONGINT, QhTerminate);
+			qh.alternate := SYSTEM.VAL(LONGINT, QhTerminate);
+			IF transfer.len > 0 THEN
+				Machine.InvalidateDCacheRange(transfer.buffer, transfer.len)
+			END;
+		END UnlinkTDs;
+
+		(* Remove ITDs from framelist and free them *)
+		PROCEDURE UnlinkIso (transfer: UsbHcdi.TransferToken);
+		VAR
+			i, count: LONGINT;
+			hcFrame: ADDRESS;
+			itd: Itd;
+		BEGIN
+			(*ShowFramelist(ADDRESSOF(framelist.data[framelist.base]), framelistSize, LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize, FALSE);
+			Objects.Yield;*)
+			WHILE transfer.tds[count] # 0 DO INC(count) END;
+			(* Make sure that HC will not process the ITDs during our work *)
+			(*TRACE('Unlink acquire FL');*)
+			(*!framelistLock.Acquire;*)
+			REPEAT
+				hcFrame := ADDRESSOF(framelist.data[framelist.base]) + (LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize) * 4;
+				(*KernelLog.Enter; KernelLog.String("Unlink waiting for HC"); KernelLog.Exit;*)
+			UNTIL (hcFrame + 3 < transfer.tds[0]) OR (hcFrame >= transfer.tds[count]);
+
+			(*KernelLog.Enter; KernelLog.String("Unlink at "); KernelLog.Int((transfer.tds[0] - ADDRESSOF(framelist.data[framelist.base])) DIV 4, 0); KernelLog.String(", HC: "); KernelLog.Int(LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize, 0); KernelLog.Exit;*)
+			WHILE transfer.tds[i] # 0 DO
+				itd := SYSTEM.GET32(transfer.tds[i]);
+				ASSERT(SYSTEM.VAL(ADDRESS, itd) MOD 32 = 0);
+				ASSERT(SYSTEM.VAL(ADDRESS, itd) DIV 32 # 0);
+				SYSTEM.PUT32(transfer.tds[i], itd.next);
+				allocator.Free(itd, sizeItd);
+				INC(i)
+			END;
+			(*Machine.FlushDCacheRange(ADDRESSOF(framelist.data[framelist.base]), framelistSize * 4);*)
+			(*ShowFramelist(ADDRESSOF(framelist.data[framelist.base]), framelistSize, LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize, FALSE);*)
+			(*KernelLog.Enter; KernelLog.String("Unlinked"); KernelLog.Exit;*)
+			(*!framelistLock.Release;*)
+			(*TRACE('Unlink release FL');*)
+		END UnlinkIso;
+
+		(*
+		 * Clears the Halt bit in the pipe's queue head and removes any qTD from the pipe.
+		 * Note that this only makes sense if the Halt feature of the USB device is also cleared used the ClearFeature standard
+		 * request. This procedure here only changes the pipe's queue head.
+		 *)
+		PROCEDURE ClearHalt(pipe : UsbHcdi.Pipe);
+		VAR
+			qh: Qh;
+			dword : SET;
+		BEGIN
+			qh := pipe.descriptors[0];
+			(*Machine.InvalidateDCacheRange(qh, sizeQh);*)
+			dword := SYSTEM.VAL(SET, qh.token);
+			IF dword * TdHalted # {} THEN
+				qh.current := 0;
+				(* Zero-out the queue head transfer overlay *)
+				qh.next := SYSTEM.VAL(LONGINT, QhTerminate);
+				qh.alternate := SYSTEM.VAL(LONGINT, QhTerminate);
+				qh.token := 0;
+				qh.buffers[0] := 0;
+				qh.buffers[1] := 0;
+				qh.buffers[2] := 0;
+				qh.buffers[3] := 0;
+				qh.buffers[4] := 0;
+				IF cap64bit THEN
+					qh.extBuffers[0] := 0;
+					qh.extBuffers[1] := 0;
+					qh.extBuffers[2] := 0;
+					qh.extBuffers[3] := 0;
+					qh.extBuffers[4] := 0;
+				END;
+				(*Machine.FlushDCacheRange(qh, sizeQh)*)
+			ELSIF Debug.Level >= Debug.Warnings THEN Show("Tried to clear a non-halted pipe."); KernelLog.Ln;
+			END;
+		END ClearHalt;
+
+		PROCEDURE Schedule * (transfer: UsbHcdi.TransferToken);
+		BEGIN
+			(* Checks *)
+			ASSERT(transfer # NIL);
+			ASSERT(transfer.pipe # NIL);
+			CASE transfer.pipe.type OF
+				 UsbHcdi.PipeControl, UsbHcdi.PipeBulk:
+					ScheduleAperiodic(transfer)
+				|UsbHcdi.PipeIsochronous:
+					ScheduleIso(transfer)
+				|UsbHcdi.PipeInterrupt:
+					(*ScheduleInterrupt(transfer)*)
+					ScheduleAperiodic(transfer)
+			ELSE
+				(* Unknown pipe type *)
+				ASSERT((transfer.pipe.type = UsbHcdi.PipeControl) OR (transfer.pipe.type = UsbHcdi.PipeIsochronous)
+					OR (transfer.pipe.type = UsbHcdi.PipeBulk) OR (transfer.pipe.type = UsbHcdi.PipeInterrupt));
+			END;
+		END Schedule;
+
+		(**
+		 * Cancel scheduled transfer.
+		 * Tries to cancel a scheduled transfer. Fails if transfer is completed or active.
+		 *)
+		PROCEDURE Cancel * (transfer: UsbHcdi.TransferToken): BOOLEAN;
+		BEGIN (*{EXCLUSIVE}*) HALT(301) (* abstract *)
+		END Cancel;
+
+		(**
+		 * Checks that a periodic pipe policy is allowed for this HCD. Returns FALSE if the policy
+		 * requires too much bandwidth, TRUE if the policy is schedulable.
+		 *)
+		PROCEDURE CheckPipePolicy * (interval, size: LONGINT): BOOLEAN;
+		BEGIN
+			RETURN TRUE
+		END CheckPipePolicy;
+
+		(**
+		 * Some HCDIs require some pipe creations to be notified to them. This procedure is called whenever a new
+		 * pipe is created, so that the HCDI can take the necessary actions. This is needed, e.g. for bulk and control pipes
+		 * on EHCI: we build one qh per pipe.
+		 *)
+		PROCEDURE RegisterPipe * (pipe: UsbHcdi.Pipe);
+		BEGIN (* Call only from exclusive sections *)
+			CASE pipe.type OF
+				 UsbHcdi.PipeControl, UsbHcdi.PipeBulk:
+				 	NEW(pipe.descriptors, 1);
+					InsertQH(pipe)
+				|UsbHcdi.PipeIsochronous: (* nothing to do *)
+				|UsbHcdi.PipeInterrupt:
+					IF pipe.irqInterval = 1 THEN (* 1ms queue *)
+						pipe.queue := interruptQh[0];
+					ELSIF pipe.irqInterval < 4 THEN (* 2ms queue *)
+						pipe.queue := interruptQh[1];
+					ELSIF pipe.irqInterval < 8 THEN (* 4ms queue *)
+						pipe.queue := interruptQh[2];
+					ELSIF pipe.irqInterval < 16 THEN (* 8ms queue *)
+						pipe.queue := interruptQh[3];
+					ELSIF pipe.irqInterval < 32 THEN (* 16ms queue *)
+						pipe.queue := interruptQh[4];
+					ELSE
+						pipe.queue := interruptQh[5]; (* 32 ms queue *)
+					END;
+					NEW(pipe.descriptors, 2);
+					InsertQH(pipe);
+					pipe.descriptors[1] := allocator.Allocate(sizeItd)
+			ELSE
+				(* Unknown pipe type *)
+				ASSERT((pipe.type = UsbHcdi.PipeControl) OR (pipe.type = UsbHcdi.PipeIsochronous)
+					OR (pipe.type = UsbHcdi.PipeBulk) OR (pipe.type = UsbHcdi.PipeInterrupt))
+			END
+		END RegisterPipe;
+
+		(**
+		 * Some HCDIs require some pipe freeing to be notified to them. This procedure is called whenever a pipe is freed,
+		 * so that the HCDI can take necessary steps. This is needed e.g. for bulk and control pipes on EHCI, so that the
+		 * controller can remove the queue head.
+		 *)
+		PROCEDURE UnregisterPipe * (pipe: UsbHcdi.Pipe);
+		BEGIN
+			CASE pipe.type OF
+				 UsbHcdi.PipeControl, UsbHcdi.PipeBulk, UsbHcdi.PipeInterrupt:
+					RemoveQH(pipe)
+				|UsbHcdi.PipeIsochronous:
+			ELSE
+				(* Unknown pipe type *)
+				ASSERT((pipe.type = UsbHcdi.PipeControl) OR (pipe.type = UsbHcdi.PipeIsochronous)
+					OR (pipe.type = UsbHcdi.PipeBulk) OR (pipe.type = UsbHcdi.PipeInterrupt))
+			END
+		END UnregisterPipe;
+
+(*
+		(**
+		 * Put the specified control transfer into the host controller's schedule.
+		 * USB Control Transfers use a three stage protocol:
+		 *  - stage 1: control setup transaction
+		 *  - stage 2: optional data stage
+		 *  - stage 3: status transaction
+		 * For high-speed devices, the PING protocol must be used for OUT transactions in the data stage and status stage.
+		 *
+		 *
+		 * @param pipe
+		 * @param direction Direction of the control transfer (UsbHcdi.In (device-to-host) | UsbHcdi.Out (host-to-device))
+		 * @param msg Control message
+		 * @param bufferlen Number of bytes transmitted/received in the data stage
+		 * @param buffer Buffer where to get/put the specified number of bytes
+		 *)
+		PROCEDURE ScheduleControl*(pipe : UsbHcdi.Pipe; direction : LONGINT; msg : UsbHcdi.ControlMessage;  bufferLen : LONGINT; VAR buffer : Usbdi.Buffer);
+		VAR
+			qtd : Machine.Address32;
+			dword : SET;
+			ranges : ARRAY ScatterGatherListSize OF Machine.Range;
+			numRanges : LONGINT;
+		BEGIN
+			(*Machine.FlushDCacheRange(ADDRESSOF(buffer[0]), bufferLen);*)
+			(* pipe.tdBase = pipe.descriptors[0] + 32 in UsbHcdi *)
+(*			pipe.firstTD := pipe.tdBase;
+			ASSERT(pipe.firstTD = pipe.descriptors[0] + alignQh); 
+*)			
+			pipe.firstTD := pipe.tdBase - 32 + alignQh;
+			AssertAlignment(pipe.firstTD, alignQtd);
+			ASSERT(SYSTEM.VAL(SET, pipe.firstTD) * {0..4} = {}); (* qTDs must be 32byte aligned *)
+			ASSERT(pipe.firstTD MOD alignQtd = 0);
+
+			IF (pipe.speed = UsbHcdi.LowSpeed) OR (pipe.speed = UsbHcdi.FullSpeed) THEN
+				IF pipe.maxRetries = 0 THEN
+					(* For low-speed and full-speed devices, the value 0 is not allowed *)
+					pipe.maxRetries := 3;
+				END;
+			END;
+
+			(* Stage1: Control setup transaction *)
+			qtd := pipe.firstTD;
+			ASSERT((qtd + sizeQtd - 1 <= ADDRESSOF(pipe.tdBuffer[pipe.tdBufferLen-1])));
+
+			AssertAlignment(qtd+sizeQtd, alignQtd);
+			SYSTEM.PUT32(qtd + QtdNextQtdPointer, qtd + sizeQtd);
+			SYSTEM.PUT32(qtd + QtdAltNextQtdPointer, QtdTerminate); (* Mark Alternate Next qTD Pointer as invalid *)
+
+			dword := LSH(SYSTEM.VAL(SET, pipe.maxRetries), 10) * QtdErrorCounter; (* DataToggle = FALSE; Current Page = 0; no IOC *)
+			dword := dword + LSH(SYSTEM.VAL(SET, 8), 16) * QtdBytesToTransfer; (* 8byte control message *)
+			dword := dword + LSH(SYSTEM.VAL(SET, PidSetup), 8) * QtdPidCode + TdActive;
+			SYSTEM.PUT32(qtd + QtdToken, dword);
+
+			Machine.TranslateVirtual(ADDRESSOF(msg[0]), 8, numRanges, ranges);
+			IF numRanges = 0 THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbEhci: ScheduleControl: Scatter/Gather list too small."); KernelLog.Ln; END;
+				pipe.status := Usbdi.Error; pipe.errors := pipe.errors + UsbHcdi.TransferTooLarge; RETURN;
+			END;
+
+			(* The HC will access the next buffer pointer when the buffer crosses a physical page... *)
+			SYSTEM.PUT32(qtd + QtdBufferPtr0, ranges[0].adr);
+			IF numRanges > 1 THEN (* buffer is across page boundaries *)
+				SYSTEM.PUT32(qtd + QtdBufferPtr1, ranges[1].adr)
+			ELSE
+				SYSTEM.PUT32(qtd + QtdBufferPtr1, 0);
+			END;
+			SYSTEM.PUT32(qtd + QtdBufferPtr2, 0);
+			SYSTEM.PUT32(qtd + QtdBufferPtr3, 0);
+			SYSTEM.PUT32(qtd + QtdBufferPtr4, 0);
+			IF cap64bit THEN
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr0, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr1, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr2, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr3, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr4, 0);
+			END;
+			Machine.FlushDCacheRange(qtd, sizeQtd);
+
+			(* Setup phase always starts with dataToggle = FALSE, so now it must be TRUE *)
+			pipe.dataToggle := TRUE;
+
+			(* Stage 2: Optional data stage *)
+			IF bufferLen # 0 THEN
+				IF ~CreateTDList(pipe, direction, bufferLen, 0, buffer, qtd + sizeQtd, qtd, TRUE) THEN
+					pipe.status := Usbdi.Error; pipe.errors := UsbHcdi.Internal; RETURN;
+				END;
+			END;
+
+			Machine.FlushDCacheRange(qtd, sizeQtd);
+			qtd := qtd + sizeQtd;
+			AssertAlignment(qtd, alignQtd);
+			IF qtd + sizeQtd - 1 > ADDRESSOF(pipe.tdBuffer[pipe.tdBufferLen-1]) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbEhci: TD buffer too small."); KernelLog.Ln; END;
+				pipe.status := Usbdi.Error; pipe.errors := pipe.errors + UsbHcdi.OutOfTDs; RETURN;
+			END;
+
+			(* stage 3: status: build status TD *)
+
+			SYSTEM.PUT32(qtd + QtdNextQtdPointer, QtdTerminate); (* Last qTD in chain *)
+			SYSTEM.PUT32(qtd + QtdAltNextQtdPointer, QtdTerminate); (* Mark Alternate Next qTD Pointer as invalid *)
+
+			dword := QtdDataToggle + TdActive; (* dataToggle always TRUE and set ind TD in status stage; CC = not accessed *)
+
+			IF (direction = UsbHcdi.Out) OR (bufferLen = 0) THEN
+				dword := dword + LSH(SYSTEM.VAL(SET, PidIn), 8);
+			ELSE
+				dword := dword + LSH(SYSTEM.VAL(SET, PidOut), 8);
+				IF pipe.speed = UsbHcdi.HighSpeed THEN (* Do PING protocol *)
+					dword := dword + TdPingState;
+				END;
+			END;
+			dword := dword + LSH(SYSTEM.VAL(SET, pipe.maxRetries), 10) * QtdErrorCounter;
+			IF pipe.ioc THEN dword := dword + QtdIoc; END; (* Set interrupt on completion bit *)
+			SYSTEM.PUT32(qtd + QtdToken, dword);
+
+			SYSTEM.PUT32(qtd + QtdBufferPtr0, 0);
+			SYSTEM.PUT32(qtd + QtdBufferPtr1, 0);
+			SYSTEM.PUT32(qtd + QtdBufferPtr2, 0);
+			SYSTEM.PUT32(qtd + QtdBufferPtr3, 0);
+			SYSTEM.PUT32(qtd + QtdBufferPtr4, 0);
+			IF cap64bit THEN
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr0, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr1, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr2, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr3, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr4, 0);
+			END;
+
+			Machine.FlushDCacheRange(ADDRESSOF(msg[0]), LEN(msg));
+			Machine.FlushDCacheRange(ADDRESSOF(buffer[0]), bufferLen);
+			Machine.FlushDCacheRange(qtd, sizeQtd);
+			pipe.lastTD := qtd;
+		END ScheduleControl;
+
+		PROCEDURE Schedule*(pipe : UsbHcdi.Pipe; bufferLen, offset: LONGINT; VAR buffer: Usbdi.Buffer);
+		VAR dword : SET;
+		BEGIN
+			Machine.FlushDCacheRange(ADDRESSOF(buffer[offset]), bufferLen);
+			SYSTEM.PUT32(pipe.descriptors[0] + QhCurrentQtdPointer, 0);
+(*			pipe.firstTD := pipe.tdBase;
+			ASSERT(pipe.firstTD = pipe.descriptors[0] + alignQh); 
+*)
+			pipe.firstTD :=  pipe.tdBase - 32 + alignQh;
+			
+			AssertAlignment(pipe.firstTD, alignQtd);
+			ASSERT(pipe.firstTD MOD alignQtd = 0);
+			ASSERT(SYSTEM.VAL(SET, pipe.firstTD) * {0..4} = {}); (* qTDs must be 32byte aligned *)
+
+			IF ~CreateTDList(pipe, pipe.direction, bufferLen, offset, buffer, pipe.firstTD, pipe.lastTD, FALSE) THEN
+				pipe.status := Usbdi.Error; pipe.errors := pipe.errors + UsbHcdi.LinkTDsFailed; RETURN;
+			END;
+
+			SYSTEM.PUT32(pipe.lastTD + QtdNextQtdPointer, QhTerminate);
+			IF pipe.ioc THEN
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(pipe.lastTD + QtdToken));
+				dword := dword + QtdIoc;
+				SYSTEM.PUT32(pipe.lastTD + QtdToken, dword);
+			END;
+		END Schedule;
+*)
+		(** Creates qTDs for the transfer and append them to the qtd list of the corresponding pipe. *)
+		PROCEDURE ScheduleAperiodic (transfer: UsbHcdi.TransferToken);
+		VAR
+			qh: Qh;
+			first, last: Qtd;
+			curr: Qtd;
+			dword: SET;
+			i, count: LONGINT;
+		BEGIN {EXCLUSIVE}
+			(*transfer.pipe.transferLock.Acquire;*)
+			IF transfer.len > 0 THEN
+				Machine.FlushDCacheRange(transfer.buffer, transfer.len)
+			END;
+			qh := transfer.pipe.descriptors[0];
+			ASSERT(SYSTEM.VAL(ADDRESS, qh) # 0);
+			AssertAlignment(qh, alignQh);
+
+			(*IF ~ScheduleOn(CmdAsyncSchedEnable, FALSE) THEN TRACE('BOOOOOOOOOOOOOOOOOOOOOOP') ELSE (*TRACE('YOUHOU')*) END;*)
+			IF transfer.pipe.type = UsbHcdi.PipeControl THEN
+				CreateControlTDs(transfer.pipe, transfer.pipe.direction, transfer.message, transfer.buffer, transfer.len, first, last, count)
+			ELSE
+				CreateQTDList(transfer.pipe, transfer.pipe.direction, transfer.len, transfer.buffer, first, last, count, FALSE)
+			END;
+
+			(* last is the end of the new qTD list: mark its next link as invalid *)
+			last.next := SYSTEM.VAL(LONGINT, QtdTerminate);
+			IF transfer.pipe.ioc THEN
+				dword := SYSTEM.VAL(SET, last.token) + QtdIoc;
+				last.token := SYSTEM.VAL(LONGINT, dword)
+			END;
+			(*Machine.FlushDCacheRange(last, sizeQtd);*)
+
+			(* Find the last qtd for the queue head and append the list 'first' to them. Acquire the pipe descriptor lock for that. *)
+			(*transfer.pipe.descriptorLock.Acquire;*)
+			(*IF ~ScheduleOn(CmdAsyncSchedEnable, FALSE) THEN
+				(*TRACE('BEEEEEEEEEP')*)
+			END;*)
+			(*Machine.InvalidateDCacheRange(qh, sizeQh);*)
+			dword := SYSTEM.VAL(SET, qh.token);
+			IF dword * TdHalted # {} THEN
+				IF Debug.Trace & Debug.tracePipes THEN Show("LinkTDs: Automatically clear halt condition"); KernelLog.Ln; END;
+				ClearHalt(transfer.pipe);
+			END;
+			REPEAT UNTIL (SYSTEM.VAL(SET, qh.token) * TdActive = {}) OR (SYSTEM.GET32(iobase + HcAsyncListAddr) # qh);
+			(*IF dword * TdActive # {} THEN Show("Schedule: pipe is active"); KernelLog.Ln; ShowQueueHead(qh, qh.current, cap64bit); Wait(10000) END;*)
+			IF (SYSTEM.VAL(SET, qh.next) * QhTerminate # {}) OR (qh.next = 0) THEN
+				qh.next := first
+			ELSE
+				curr := qh.next;
+				WHILE (SYSTEM.VAL(SET, curr.next) - QhTerminate # {}) & (QhTerminate * SYSTEM.VAL(SET, curr.next) = {}) DO
+					curr := curr.next
+				END;
+				curr.next := first;
+			END;
+			(*IF dword * TdActive # {} THEN Show("Schedule: pipe is active"); KernelLog.Ln; ShowQueueHead(qh, qh.next, cap64bit); LOOP END END;*)
+			(*KernelLog.String("========================================================="); KernelLog.Ln;
+			TRACE(transfer.len);*)
+			(*ShowQueueHead(qh, first, cap64bit);*)
+			IF ~ScheduleOn(CmdAsyncSchedEnable, TRUE) THEN
+				KernelLog.Enter;
+				KernelLog.String("UsbEhci: could not enable ansynchronous scheduling");
+				KernelLog.Exit
+			END;
+
+			(* Update transfer info with tds *)
+			i := 0;
+			curr := first;
+			WHILE i < count DO
+				ASSERT(SYSTEM.VAL(SET, curr) * QtdTerminate = {});
+				transfer.tds[i] := curr;
+				curr := curr.next;
+				INC(i)
+			END;
+			ASSERT(i = count);
+			ASSERT(SYSTEM.VAL(SET, curr) * QtdTerminate # {});
+			transfer.status := Usbdi.InProgress;
+
+			(* if pipe supports toggle, toggle *)
+			IF (transfer.pipe.type = UsbHcdi.PipeBulk) OR (transfer.pipe.type = UsbHcdi.PipeInterrupt) THEN
+				IF transfer.pipe.dataToggle THEN
+					transfer.pipe.dataToggle := FALSE
+				ELSE
+					transfer.pipe.dataToggle := TRUE
+				END
+			END
+		END ScheduleAperiodic;
+
+		(** Create TDs for a control pipe request *)
+		PROCEDURE CreateControlTDs (pipe: UsbHcdi.Pipe; direction: LONGINT; msg: UsbHcdi.ControlMessage; buffer: (*ARRAY OF CHAR*) ADDRESS; len: LONGINT;VAR firstTD, lastTD: Qtd; VAR tdCount: LONGINT);
+		VAR
+			curr, next, last: Qtd;
+			dword: SET;
+			ranges: POINTER TO ARRAY OF Machine.Range;
+			count, i, numRanges: LONGINT;
+		BEGIN
+			IF (pipe.speed = UsbHcdi.LowSpeed) OR (pipe.speed = UsbHcdi.FullSpeed) THEN
+				IF pipe.maxRetries = 0 THEN
+					(* For low-speed and full-speed devices, the value 0 is not allowed *)
+					pipe.maxRetries := 3;
+				END;
+			END;
+
+			(* Stage1: Control setup transaction *)
+			firstTD := allocator.Allocate(sizeQtd);
+			curr := firstTD;
+			AssertAlignment(curr, alignQtd);
+
+			curr.alternateNext := SYSTEM.VAL(LONGINT, QtdTerminate); (* Mark Alternate Next qTD Pointer as invalid *)
+
+			dword := LSH(SYSTEM.VAL(SET, pipe.maxRetries), 10) * QtdErrorCounter; (* DataToggle = FALSE; Current Page = 0; no IOC *)
+			dword := dword + LSH(SYSTEM.VAL(SET, 8), 16) * QtdBytesToTransfer; (* 8byte control message *)
+			dword := dword + LSH(SYSTEM.VAL(SET, PidSetup), 8) * QtdPidCode + TdActive;
+			curr.token := SYSTEM.VAL(LONGINT, dword);
+
+			i := (LEN(msg) DIV PageSize) + 1;
+			REPEAT
+				NEW(ranges, i);
+				Machine.TranslateVirtual(UsbBuffers.GetDataAddress(msg), 8, numRanges, ranges^);
+				INC(i)
+			UNTIL numRanges # 0;
+
+			(* The HC will access the next buffer pointer when the buffer crosses a physical page... *)
+			curr.buffers[0] := ranges[0].adr;
+			IF numRanges > 1 THEN (* buffer is across page boundaries *)
+				curr.buffers[1] := ranges[1].adr
+			ELSE
+				curr.buffers[1] := 0
+			END;
+			curr.buffers[2] := 0;
+			curr.buffers[3] := 0;
+			curr.buffers[4] := 0;
+			(*SYSTEM.PUT32(qtd + QtdBufferPtr2, 0);
+			SYSTEM.PUT32(qtd + QtdBufferPtr3, 0);
+			SYSTEM.PUT32(qtd + QtdBufferPtr4, 0);*)
+			IF cap64bit THEN
+				curr.extBuffers[0] := 0;
+				curr.extBuffers[1] := 0;
+				curr.extBuffers[2] := 0;
+				curr.extBuffers[3] := 0;
+				curr.extBuffers[4] := 0;
+				(*SYSTEM.PUT32(qtd + QtdExtBufferPtr0, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr1, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr2, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr3, 0);
+				SYSTEM.PUT32(qtd + QtdExtBufferPtr4, 0);*)
+			END;
+			(*Machine.FlushDCacheRange(curr, sizeQtd);*)
+	
+			(* Setup phase always starts with dataToggle = FALSE, so now it must be TRUE *)
+			pipe.dataToggle := TRUE;
+
+			(* Stage 2: Optional data stage *)
+			IF len # 0 THEN
+				CreateQTDList(pipe, direction, len, buffer, next, last, count, TRUE)
+			END;
+			IF count = 0 THEN
+				last := curr
+			ELSE
+				curr.next := next;
+				(*Machine.FlushDCacheRange(curr, sizeQtd)*)
+			END;
+
+			curr := allocator.Allocate(sizeQtd);
+			last.next := curr;
+			(*Machine.FlushDCacheRange(last, sizeQtd);*)
+			AssertAlignment(curr, alignQtd);
+
+			(* stage 3: status: build status TD *)
+			curr.next := SYSTEM.VAL(LONGINT, QtdTerminate);
+			curr.alternateNext := SYSTEM.VAL(LONGINT, QtdTerminate);
+
+			dword := QtdDataToggle + TdActive; (* dataToggle always TRUE and set ind TD in status stage; CC = not accessed *)
+
+			IF (direction = UsbHcdi.Out) OR (len = 0) THEN
+				dword := dword + LSH(SYSTEM.VAL(SET, PidIn), 8);
+			ELSE
+				dword := dword + LSH(SYSTEM.VAL(SET, PidOut), 8);
+				(*TRACE(pipe.speed);*)
+				(*IF pipe.speed = UsbHcdi.HighSpeed THEN (* Do PING protocol *)
+					dword := dword + TdPingState;
+				END;*)
+			END;
+			dword := dword + LSH(SYSTEM.VAL(SET, pipe.maxRetries), 10) * QtdErrorCounter;
+			IF pipe.ioc THEN dword := dword + QtdIoc; END; (* Set interrupt on completion bit *)
+			curr.token := SYSTEM.VAL(LONGINT, dword);
+
+			curr.buffers[0] := 0;
+			curr.buffers[1] := 0;
+			curr.buffers[2] := 0;
+			curr.buffers[3] := 0;
+			curr.buffers[4] := 0;
+			IF cap64bit THEN
+				curr.extBuffers[0] := 0;
+				curr.extBuffers[1] := 0;
+				curr.extBuffers[2] := 0;
+				curr.extBuffers[3] := 0;
+				curr.extBuffers[4] := 0;
+			END;
+
+			Machine.FlushDCacheRange(UsbBuffers.GetDataAddress(msg), LEN(msg));
+			(*Machine.FlushDCacheRange(buffer, len);*)
+			(*Machine.FlushDCacheRange(curr, sizeQtd);*)
+			lastTD := curr;
+			tdCount := count + 2
+		END CreateControlTDs;
+
+		(** Create qTD list *)
+		PROCEDURE CreateQTDList (pipe: UsbHcdi.Pipe; direction, len: LONGINT; buffer: ADDRESS; VAR firstTD, lastTD: Qtd; VAR tdCount: LONGINT; tdToggle: BOOLEAN);
+		VAR
+			prev, qtd: Qtd;
+			restlen, curlen, temp: LONGINT;
+			i, j: LONGINT;
+			dword: SET;
+			numRanges, idx, offset: LONGINT;
+			sgList: POINTER TO ARRAY OF Machine.Range;
+			buf: ADDRESS;
+			t: HUGEINT;
+		BEGIN
+			ASSERT((pipe.maxRetries >= 0) & (pipe.maxRetries <= 3));
+
+			Machine.FlushDCacheRange(buffer, len);
+			j := (len DIV PageSize) + 1;
+			REPEAT
+				NEW(sgList, j);
+				Machine.TranslateVirtual(buffer, len, numRanges, sgList^);
+				INC(j, 1)
+			UNTIL numRanges # 0;
+
+			(*KernelLog.Enter; KernelLog.Address(buffer); KernelLog.String(" -> "); KernelLog.Address(sgList[0].adr); KernelLog.Exit;*)
+
+			idx := 0;
+			offset := 0;		(* offset from last qTD (must fill multiples of packetSize into qTD buffers) *)
+			curlen := 0; 		(* amount of data that is transferred in a single qTD *)
+			restlen := len;	(* total amount of data to be transferred *)
+			firstTD := 0;
+			lastTD := 0;
+			tdCount := 0;
+			buf := sgList[idx].adr + offset;
+
+			WHILE restlen > 0 DO (* build qTD chain *)
+				(*TRACE(restlen);*)
+				(* allocate the next qTD of the chain *)
+				qtd := allocator.Allocate(sizeQtd);
+				IF SYSTEM.VAL(ADDRESS, prev) = 0 THEN
+					firstTD := qtd
+				ELSE
+					buf := GetNextPhysicalPage(sgList^, idx, buf);
+					prev.next := qtd;
+					(*Machine.FlushDCacheRange(prev, sizeQtd)*)
+				END;
+
+				(* Each qTD has four buffer pointers. Each buffer is 4K. The buffer must be virtually contiguous but may be				*)
+				(* physically non-contiguous. The HC detects crossings of page boundaries and increments the current buffer pointer. 	*)
+				qtd.buffers[0] := buf;
+
+				curlen := PageSize - LONGINT(buf MOD PageSize);
+				(*TRACE(curlen);*)
+				IF curlen > restlen THEN (* No other buffer pointers needed, fits into the first page *)
+					curlen := restlen;
+				END;
+				(*TRACE(curlen);*)
+				ASSERT(curlen > 0);
+
+				restlen := restlen - curlen; offset := 0;
+				(*TRACE(restlen);*)
+
+				(* Fill in the other 4 buffer pointers *)
+				FOR j := 1 TO 4 DO
+					IF restlen <= 0 THEN
+						(*TRACE(0);*)
+						qtd.buffers[j] := 0;
+					ELSE
+						IF j = 4 THEN (* last buffer available in this qTD *)
+							temp := PageSize - ((curlen + PageSize) MOD pipe.maxPacketSize); (* data that fits into the last buffer (max) *)
+							IF restlen > temp THEN
+								(* The HC will issues USB transaction at pipe.maxPacketSize granularity. If this is not the 			*)
+								(* last qTD of this qTD chain, curlen must be multiple of pipe.maxPacketSize. If the last qTD of this	*)
+								(* chain was not a multiple of pipe.maxPacketSize, the device will send more data (since we 		*)
+								(* requested more data) and the HC thinks it's a babble.											*)
+								curlen := curlen + temp; restlen := restlen - temp; offset := temp;
+								(*TRACE(curlen);*)
+								buf := GetNextPhysicalPage(sgList^, idx, buf);
+								ASSERT(buf MOD PageSize = 0);
+								qtd.buffers[j] := buf;
+								IF temp < PageSize THEN
+									DEC(buf, PageSize - temp)
+								END;
+							ELSE (* this is the last qTD in chains *)
+								curlen := curlen + restlen; restlen := 0;
+								(*TRACE(curlen);*)
+								buf := GetNextPhysicalPage(sgList^, idx, buf);
+								ASSERT(buf MOD PageSize = 0);
+								qtd.buffers[j] := buf
+							END;
+						ELSE
+							IF restlen > PageSize THEN
+								(*TRACE(curlen);*)
+								curlen := curlen + PageSize; restlen := restlen - PageSize;
+							ELSE
+								(*TRACE(curlen);*)
+								curlen := curlen + restlen; restlen := 0;
+							END;
+							buf := GetNextPhysicalPage(sgList^, idx, buf);
+							ASSERT(buf MOD PageSize = 0);
+							qtd.buffers[j] := buf;
+						END
+					END
+				END;
+
+				IF cap64bit THEN
+					qtd.extBuffers[0] := 0;
+					qtd.extBuffers[1] := 0;
+					qtd.extBuffers[2] := 0;
+					qtd.extBuffers[3] := 0;
+					qtd.extBuffers[4] := 0;
+				END;
+
+				qtd.alternateNext := SYSTEM.VAL(LONGINT, QtdTerminate); (* Mark Alternate Next qTD Pointer as invalid *)
+
+				(*TRACE(curlen);*)
+				ASSERT(curlen <= 5000H); (* Maximum allowed value for a single qTD: 5*4KB *)
+
+				dword := TdActive;
+				dword := dword + LSH(SYSTEM.VAL(SET, pipe.maxRetries), 10) * QtdErrorCounter;  (* Current Page=0 *)
+				(*TRACE(dword);*)
+				dword := dword + LSH(SYSTEM.VAL(SET, curlen), 16) * QtdBytesToTransfer;
+				(*TRACE(dword);*)
+
+				IF tdToggle THEN
+					IF pipe.dataToggle THEN	dword := dword + QtdDataToggle; END;
+
+					(* Calculate datatoggle value for next TD *)
+					IF (curlen DIV pipe.maxPacketSize) MOD 2 # 0 THEN
+						pipe.dataToggle := ~pipe.dataToggle;
+					END;
+				END;
+
+				IF direction = UsbHcdi.In  THEN
+					dword := dword + LSH(SYSTEM.VAL(SET, PidIn), 8);
+				ELSIF direction = UsbHcdi.Out THEN
+					dword := dword + LSH(SYSTEM.VAL(SET, PidOut), 8);
+					IF pipe.speed = UsbHcdi.HighSpeed THEN (* Do PING protocol *)
+						dword := dword + TdPingState;
+					END;
+				END;
+				(*TRACE(dword);*)
+
+				qtd.token := SYSTEM.VAL(LONGINT, dword);
+				(*TRACE(SYSTEM.VAL(SET, qtd.token));*)
+				(*Machine.FlushDCacheRange(qtd, sizeQtd);*)
+				prev := qtd;
+				INC(tdCount)
+			END;
+
+			lastTD := qtd;
+		END CreateQTDList;
+
+		PROCEDURE ScheduleIso (transfer: UsbHcdi.TransferToken);
+		VAR
+			sgList: ARRAY 1024 OF Machine.Range;
+			itds: ARRAY UsbHcdi.MaxTDs OF ADDRESS;
+			first, last, itd: Itd;
+			adr, currBuf, currOfs, currRange, currTrans, i, itdTxSize, j,
+			numFrames, numRanges, remLen, transSize: LONGINT;
+			fridx, interval, frame: LONGINT;
+			firstEntry: ADDRESS;
+			dword: SET;
+		BEGIN
+			Machine.TranslateVirtual(transfer.buffer, transfer.len, numRanges, sgList);
+			Machine.FlushDCacheRange(transfer.buffer, transfer.len);
+			IF numRanges = 0 THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbEhci: Schedule: Scatter/Gather list too small"); KernelLog.Ln; END;
+				transfer.pipe.status := Usbdi.Error; transfer.pipe.errors := transfer.pipe.errors + UsbHcdi.TransferTooLarge;
+			END;
+
+			(* Compute transfer parameters *)
+			remLen := transfer.len;
+
+			(*KernelLog.String(":: Iso TX size: "); KernelLog.Int(transfer.len, 0); KernelLog.Ln;
+			KernelLog.String(":: iTD Size: "); KernelLog.Int(sizeItd, 0); KernelLog.Ln;*)
+
+			(* Prepare iTDs *)
+			first := allocator.Allocate(sizeItd);
+			itd := first;
+			IF itd = ADDRESS(0) THEN
+				KernelLog.String("ITD = 0, ABORTING");
+				KernelLog.Ln;
+				ASSERT(itd # ADDRESS(0))
+			END;
+
+			numFrames := 0;
+			currRange := 0;
+			currOfs := sgList[currRange].adr MOD 4096;
+			adr := sgList[currRange].adr - sgList[currRange].adr MOD 4096;
+			REPEAT
+				itdTxSize := 0;
+				currBuf := 0;
+				currTrans := 0;
+
+				itd.buffers[currBuf] := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, adr) * ItdBufferPtr);
+
+				(* iTD transactions *)
+				WHILE (remLen > 0) & (currTrans < 8) DO
+					(* Compute buffer offset and buffer page *)
+					IF currOfs >= 4096 THEN
+						(* Buffer offset overflow, allocate new buffer *)
+						currOfs := currOfs MOD 4096;
+						INC(currBuf);
+
+						INC(adr, 4096);
+						IF adr >= sgList[currRange].adr + sgList[currRange].size THEN
+							INC(currRange);
+							adr := sgList[currRange].adr - sgList[currRange].adr MOD 4096
+						END;
+						dword := SYSTEM.VAL(SET, adr) * ItdBufferPtr;
+						itd.buffers[currBuf] := SYSTEM.VAL(LONGINT, dword)
+					END;
+
+					(* Compute transaction size and evaluate need for IOC *)
+					transSize := transfer.pipe.maxPacketSize * transfer.pipe.mult;
+					dword := {};
+					IF remLen <= transSize THEN
+						(* Last transaction: transfer remaining bytes and set IOC *)
+						transSize := remLen;
+						dword := ItdTransactionIoc
+					END;
+
+					(* Write transaction *)
+					dword := dword + ItdActive + SYSTEM.VAL(SET, LSH(transSize, 16)) + SYSTEM.VAL(SET, LSH(currBuf, 12)) * {12 .. 14} + SYSTEM.VAL(SET, currOfs) * {0 .. 11};
+					itd.transactions[currTrans] := SYSTEM.VAL(LONGINT, dword);
+
+					INC(itdTxSize, transSize);
+					DEC(remLen, transSize);
+					INC(currOfs, transSize);
+					INC(currTrans)
+
+					(*;KernelLog.String(":: iTD "); KernelLog.Int((itd - pipe.firstTD) DIV sizeItd, 0);
+					(*KernelLog.String(" :: Transaction "); KernelLog.Int(currTrans, 0);
+					KernelLog.String(" :: TX Size "); KernelLog.Int(transSize, 0);
+					KernelLog.String(" :: Remaining "); KernelLog.Int(remLen, 0);*) KernelLog.Ln;*)
+
+				END;
+
+				(* First buffer is always used.
+				 * This buffer can be the same as the last buffer of the previous iTD or the next physical page,
+				 * depending on the overflow condition of currOfs. *)
+				IF currOfs >= 4096 THEN
+					(* Buffer offset overflow, allocate new buffer *)
+					currOfs := currOfs MOD 4096;
+					INC(currBuf);
+
+					INC(adr, 4096);
+					IF adr >= sgList[currRange].adr + sgList[currRange].size THEN
+						INC(currRange);
+						adr := sgList[currRange].adr - sgList[currRange].adr MOD 4096
+					END;
+					IF currBuf < 7 THEN
+						dword := SYSTEM.VAL(SET, adr) * ItdBufferPtr;
+						itd.buffers[currBuf] := SYSTEM.VAL(LONGINT, dword);
+					END
+				END;
+
+				(* Next Link *)
+				dword := {0};
+				itd.next := 1(*SYSTEM.VAL(LONGINT, dword)*);
+
+				(* iTD endpoint parameters *)
+				dword := SYSTEM.VAL(SET, itd.buffers[0]);
+				dword := dword * ItdBufferPtr + SYSTEM.VAL(SET, LSH(transfer.pipe.endpoint, 8)) * {8 .. 11} + SYSTEM.VAL(SET, transfer.pipe.address) * {0 .. 6};
+				itd.buffers[0] := SYSTEM.VAL(LONGINT, dword);
+				dword := SYSTEM.VAL(SET, itd.buffers[1]);
+				dword := dword * ItdBufferPtr + SYSTEM.VAL(SET, transfer.pipe.maxPacketSize) * {0 .. 10};
+				IF transfer.pipe.direction = UsbHcdi.In THEN INCL(dword, 11) END;
+				itd.buffers[1] := SYSTEM.VAL(LONGINT, dword);
+				dword := SYSTEM.VAL(SET, itd.buffers[2]);
+				dword := dword * ItdBufferPtr + SYSTEM.VAL(SET, transfer.pipe.mult) * {0 .. 1};
+				itd.buffers[2] := SYSTEM.VAL(LONGINT, dword);
+
+				(*Machine.FlushDCacheRange(itd, sizeItd);*)
+				(*ShowItd(itd, 0);
+				TRACE(SYSTEM.GET32(iobase + HcPeriodicListBase));*)
+				itds[numFrames] := itd;
+				INC(numFrames);
+				IF remLen > 0 THEN
+					itd := allocator.Allocate(sizeItd);
+				END;
+			UNTIL (remLen = 0) OR (numFrames = framelistSize);
+
+			IF (*numFrames >= framelistSize*) remLen > 0 THEN
+				KernelLog.String("UsbEhci: cannot schedule more than 1 frame list (");
+				KernelLog.Int(framelistSize, 0);
+				KernelLog.String(" iTDs) at a time. Transfer is too big.");
+				KernelLog.Ln;
+				ASSERT(remLen = 0)
+			END;
+
+			last := itd;
+
+			(*KernelLog.String("Number of iTDs:	"); KernelLog.Int(numFrames, 0); KernelLog.Ln;
+			KernelLog.String("Total tx size:	"); KernelLog.Int(transfer.len, 0); KernelLog.Ln;*)
+
+			(**TRACE('Sched acquire FL');*)
+			(*framelistLock.Acquire;*)
+			(*IF ~ScheduleOn(CmdPeriodicSchedEnable, FALSE) THEN TRACE('BOOOOOOOOOOOOOOOOOOOOOOP') ELSE (*TRACE('YOUHOU')*) END;*)
+			(* Link iTDs: find next framelist offset *)
+			(*!interval := LSH(1, pipe.irqInterval) DIV 8;
+			IF interval = 0 THEN interval := 1 END;*)
+			BEGIN {EXCLUSIVE}
+				interval := 1;
+				frame := framelist.base;
+				(* We must put the iTDs at this offset from the base of the periodic frame list: current index + 1 for uncertainty + 1 to get the next *)
+				SYSTEM.GET(iobase + HcFrIndex, fridx);
+				(*TRACE(fridx, LSH(fridx, -3) MOD framelistSize);*)
+				fridx := (LSH(fridx, -3) MOD framelistSize) + 1 + framelistOfs + 2;
+				firstEntry := ADDRESSOF(framelist.data[frame + fridx]);
+				(*TRACE(SYSTEM.GET32(iobase + HcPeriodicListBase), ADDRESSOF(framelist.data[framelist.base]));*)
+
+				(* Insert iTDs *)
+				(*TRACE(fridx);*)
+				(*ShowFramelist(ADDRESSOF(framelist.data[framelist.base]), framelistSize, LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize, FALSE);*)
+				(*KernelLog.Enter; KernelLog.String("Link HC: "); KernelLog.Int(fridx - 3 - framelistOfs, 0); KernelLog.Exit;*)
+				FOR i := 0 TO numFrames - 1 DO
+					itd := itds[i];
+					ASSERT(SYSTEM.VAL(ADDRESS, itd) # 0);
+					fridx := fridx MOD framelistSize;
+
+					(* Get the next free entry *)
+					WHILE framelist.data[frame + fridx] MOD 8 = 0 DO fridx := (fridx + interval) MOD framelistSize END;
+
+					(*ShowItd(itd, 0);*)
+					(* Not an itd entry *)
+					itd.next := framelist.data[frame + fridx];
+					(*KernelLog.Enter; KernelLog.String("Link at "); KernelLog.Int(fridx, 0); KernelLog.Exit;*)
+					(*SYSTEM.PUT32(frame + 4 * fridx, SYSTEM.VAL(SET, itd) * {5 .. 31}); (* Typ = 00 and T = 0 *)*)
+					framelist.data[frame + fridx] := SYSTEM.VAL(ADDRESS, itd);
+
+					(* Store the address of the framelist entry instead of ITD directly *)
+					transfer.tds[i] := ADDRESSOF(framelist.data[frame + fridx]);
+					INC(fridx, interval)
+				END;
+			END;
+			(*lastEntry := ADDRESSOF(framelist.data[frame + fridx + 1]);*)
+			(*framelistLock.Release;*)
+			(*Machine.FlushDCacheRange(ADDRESSOF(framelist.data[frame]), framelistSize * 4);*)
+			(*TRACE('Sched release FL');*)
+
+			(*ShowFramelist(ADDRESSOF(framelist.data[framelist.base]), framelistSize, LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize, FALSE);*)
+
+			IF SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * StsPeriodicSchedule = {} THEN
+				(*TRACE('PERIODIC SCHEDULE DISABLED');*)
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+				IF dword * CmdPeriodicSchedEnable = {} THEN
+					IF ~ScheduleOn(CmdPeriodicSchedEnable, TRUE) & (Debug.Level >= Debug.Errors) THEN Show("Failed to re-enable periodic schedule."); KernelLog.Ln; END;
+				END
+			END;
+			transfer.status := Usbdi.InProgress;
+
+			(*KernelLog.Enter; KernelLog.String(":: Entered iTDs at frame #"); KernelLog.Int(frame, 0); KernelLog.Ln;*)
+			(*KernelLog.String(":: Isochronous Scheduling Threshold: "); KernelLog.Int(frameListOfs, 0); KernelLog.Exit;*)
+		END ScheduleIso;
+
+		(** Schedule an interrupt transfer: create an iTD, link to QH from it and place it in framelist. *)
+		PROCEDURE ScheduleInterrupt (transfer: UsbHcdi.TransferToken);
+		VAR
+			itd: Itd;
+			qh: Qh;
+			frame, fridx: LONGINT;
+		BEGIN
+			(*itd := transfer.pipe.descriptors[1];*)
+			qh := transfer.pipe.descriptors[0];
+
+			(*itd.next := QhHorizontalLink(qh, QhTypQh, FALSE);*)
+
+			frame := framelist.base;
+			(* We must put the iTDs at this offset from the base of the periodic frame list: current index + 1 for uncertainty + 1 to get the next *)
+			fridx := SYSTEM.GET32(iobase + HcFrIndex);
+			fridx := (LSH(fridx, -3) MOD framelistSize) + 1 + framelistOfs;
+			framelist.data[frame + fridx] := QhHorizontalLink(qh, QhTypQh, FALSE);
+		END ScheduleInterrupt;
+
+		PROCEDURE InterruptHandler;
+		VAR s  : SET;
+		BEGIN
+			IF Debug.Stats THEN INC(NnofInterrupts); END;
+			IF state >= UsbHcdi.Initialized THEN
+				s := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * interruptsEnabled;
+				(* Reset interrupt status register (Write clear)*)
+				SYSTEM.PUT32(iobase + HcUsbSts, s (** {0..5}*)); FlushPCI;
+				handler.Handle(s)
+			END
+		END InterruptHandler;
+
+		PROCEDURE DoHandleInterrupt (s: SET);
+		BEGIN (* Works without being exclusive *)
+			IF s # {} THEN
+				IF Debug.Stats THEN INC(NnofInterruptsHandled); END;
+				IF Debug.Trace & Debug.traceInterrupts THEN
+					Show("Interrupt: "); ShowInterrupts(s); KernelLog.Ln;
+				END;
+
+				IF s * StsAsyncAdvance # {} THEN hcHandshake := TRUE; END;
+				IF s * StsHostSystemError # {} THEN
+					 Show("Serious error. Please restart the EHCI driver:");
+					 IF SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * StsHcHalted # {} THEN
+					 	KernelLog.String(" [HC halted]");
+					 	SetState(UsbHcdi.Halted);
+					 END;
+					 KernelLog.Ln;
+				END;
+				IF s * StsFrameListRollover # {} THEN END;
+				IF s * StsPortChange # {} THEN
+					(* TODO: If wake-up, time 20ms, poll PscSuspend... enable HC if necessary*)
+					IF statusChangeHandler # NIL THEN  statusChangeHandler(Usbdi.Ok, 0); END;
+				END;
+				IF (s * (StsUsbError + StsUsbInterrupt) # {}) OR (19 IN s) THEN (* USB Interrupt occured: can be IOC or ShortPacketInt *)
+					NotifyCompletionHandlers;
+				END;
+			END;
+		END DoHandleInterrupt;
+
+		(* re-evaluate the status of the pipe's qh (endpoint descriptor) and its TD list *)
+		PROCEDURE UpdatePipeStatus * (pipe : UsbHcdi.Pipe);
+		BEGIN
+			CASE pipe.type OF
+				 UsbHcdi.PipeControl, UsbHcdi.PipeBulk, UsbHcdi.PipeInterrupt:
+					UpdatePipeStatusAsync(pipe)
+				|UsbHcdi.PipeIsochronous:
+					UpdatePipeStatusIso(pipe)
+			ELSE
+				(* Unknown pipe type *)
+				HALT(8)
+			END
+		END UpdatePipeStatus;
+
+		PROCEDURE UpdatePipeStatusAsync (pipe: UsbHcdi.Pipe);
+		CONST
+			MaxLoops = 10000;
+		VAR
+			qh: Qh;
+			after, before, qtd: Qtd;
+			transfer: UsbHcdi.TransferToken;
+			s, errors: SET;
+			i, restLen, len: LONGINT;
+			loop: LONGINT;
+			active: BOOLEAN;
+		BEGIN {EXCLUSIVE}
+			FlushPCI;
+			(* First look up active bit in the QH tranfer overlay *)
+			qh := pipe.descriptors[0];
+			(*Machine.InvalidateDCacheRange(qh, sizeQh);*)
+			(*s := SYSTEM.VAL(SET, qh.token);
+			(*ShowQueueHead(qh, 0, cap64bit); Wait(100);*)
+			IF (s * TdActive # {}) & (s * (TdHalted + TdDataBufferError + TdBabbleDetected + TdTransactionError + TdMissedMicroFrame) = {}) THEN
+				(* The HC hasn't yet executed the transaction *)
+				RETURN
+			END;*)
+			(*! 
+				Disabling scheduling here leads to problems: some of the transfers does not get finished!
+			*)
+			(*REPEAT UNTIL (SYSTEM.VAL(SET, qh.token) * TdActive = {}) OR (SYSTEM.GET32(iobase + HcAsyncListAddr) # qh);*)
+			(*qh.token := SYSTEM.VAL(LONGINT, s - TdActive);*)
+
+			(*ASSERT(pipe.transferLock # NIL);*)
+			(*pipe.transferLock.Acquire;*)
+			transfer := pipe.transfers;
+			WHILE transfer # NIL DO
+				errors := UsbHcdi.NoErrors;
+				active := FALSE;
+				IF transfer.status = Usbdi.InProgress THEN
+					i := -1;
+					WHILE transfer.tds[i + 1] # 0 DO INC(i) END;
+					ASSERT(i >= 0);
+					qtd := transfer.tds[i];
+					IF SYSTEM.VAL(SET, qtd.token) * TdActive # {} THEN
+						i := 0;
+						(* Examine only transfers that are in progress *)
+						WHILE (i < LEN(transfer.tds)) & (transfer.tds[i] # 0) & ~active & (errors = UsbHcdi.NoErrors) DO
+							qtd := transfer.tds[i];
+							AssertAlignment(qtd, alignQtd);
+							(*Machine.InvalidateDCacheRange(qtd, sizeQtd);*)
+
+							s := SYSTEM.VAL(SET, qtd.token) * QtdStatus - TdPingState - TdSplitTransactionState;
+							IF s * TdActive # {} THEN
+								(* qTD is still active, no errors so far: skip to next transfer *)
+								active := TRUE
+							END;
+							(* At least one error occured *)
+							IF s * TdHalted # {} THEN errors := errors + UsbHcdi.Stalled; END;
+							IF s * TdDataBufferError # {} THEN errors := errors + UsbHcdi.Databuffer; END;
+							IF s * TdBabbleDetected # {} THEN errors := errors + UsbHcdi.Babble; END;
+							IF s * TdTransactionError # {} THEN errors := errors + UsbHcdi.CrcTimeout; END;
+							IF s * TdMissedMicroFrame # {} THEN errors := errors + UsbHcdi.Internal; END;
+
+							IF ~active & (transfer.len > 0) THEN
+								(* Data had to be transfered... *)
+								(* The host controller decrements the Total Bytes To Transfer field according the amount of data it did
+								transfer. If this field has not the value zero, the host controller did not transfer all data. If there is no
+								error reported, this is a short packet condition, which can be okay. *)
+
+								(* len bytes should have been transfered for this TD *)
+								len := SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, qtd.token) * QtdBytesToTransfer, -16));
+
+								IF (len # 0) THEN (* Short packet *)
+									restLen := restLen + len;
+								END
+							END;
+							INC(i)
+						END
+					END;
+
+					IF active THEN
+						(* no update for this transfer: still active *)
+					ELSE
+						transfer.errors := errors;
+						IF errors # UsbHcdi.NoErrors THEN
+							transfer.transfered := transfer.len - restLen;
+							IF errors * UsbHcdi.Stalled # {} THEN
+								transfer.status := Usbdi.Stalled;
+							ELSE
+								transfer.status := Usbdi.Error;
+							END;
+						ELSE
+							IF restLen = 0 THEN
+								transfer.transfered:= transfer.len;
+								transfer.status := Usbdi.Ok;
+							ELSE
+								transfer.transfered := transfer.len - restLen;
+								transfer.status := Usbdi.ShortPacket;
+								transfer.errors := transfer.errors + UsbHcdi.ShortPacket;
+							END
+						END;
+						(* transfer finished, unlink its TDs *)
+						UnlinkTDs(transfer);
+					END
+				END;
+				(*TRACE(pipe, pipe.transferLock, transfer, transfer.next);*)
+				transfer := transfer.next
+			END;
+			(*pipe.transferLock.ReleaseRead;*)
+
+			(*Machine.InvalidateDCacheRange(pipe.descriptors[0], sizeQh);*)
+			s := SYSTEM.VAL(SET, qh.token);
+			IF s * TdHalted # {} THEN
+				ClearHalt(pipe);
+			END;
+
+		END UpdatePipeStatusAsync;
+
+		(*
+		PROCEDURE UpdatePipeStatusInterrupt (pipe: UsbHcdi.Pipe);
+		VAR
+			qh: Qh;
+			transfer: UsbHcdi.TransferToken;
+			s, errors: SET;
+			restLen, len: LONGINT;
+			active, error: BOOLEAN;
+		BEGIN
+			FlushPCI;
+			(* First look up active bit in the QH tranfer overlay *)
+			qh := pipe.descriptors[0];
+			Machine.InvalidateDCacheRange(qh, sizeQh);
+			s := SYSTEM.VAL(SET, qh.token);
+			(*ShowQueueHead(qh, 0, cap64bit); Wait(1000);*)
+			IF s * TdActive # {}  THEN  (* The HC hasn't yet executed the transaction *) RETURN; END;
+
+			errors := UsbHcdi.NoErrors;
+
+			s := SYSTEM.VAL(SET, qh.token) * QtdStatus - TdPingState - TdSplitTransactionState;
+			IF s = {} THEN
+				(* No errors occured *)
+			ELSIF s * TdActive # {} THEN
+				(* qTD is still active, no errors so far: skip to next transfer *)
+				active := TRUE
+			ELSE
+				(* At least one error occured *)
+				IF s * TdHalted # {} THEN errors := errors + UsbHcdi.Stalled; END;
+				IF s * TdDataBufferError # {} THEN errors := errors + UsbHcdi.Databuffer; END;
+				IF s * TdBabbleDetected # {} THEN errors := errors + UsbHcdi.Babble; END;
+				IF s * TdTransactionError # {} THEN errors := errors + UsbHcdi.CrcTimeout; END;
+				IF s * TdMissedMicroFrame # {} THEN errors := errors + UsbHcdi.Internal; END;
+				error := TRUE
+			END;
+
+			IF ~active & (transfer.len > 0) THEN
+				(* Data had to be transfered... *)
+				(* The host controller decrements the Total Bytes To Transfer field according the amount of data it did
+				transfer. If this field has not the value zero, the host controller did not transfer all data. If there is no
+				error reported, this is a short packet condition, which can be okay. *)
+
+				(* len bytes should have been transfered for this TD *)
+				len := SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, qh.token) * QtdBytesToTransfer, -16));
+
+				IF (len # 0) THEN (* Short packet *)
+					restLen := restLen + len;
+				END
+			END;
+
+			transfer.errors := errors;
+			IF error THEN
+				transfer.transfered := transfer.len - restLen;
+				IF errors * UsbHcdi.Stalled # {} THEN
+					transfer.status := Usbdi.Stalled;
+				ELSE
+					transfer.status := Usbdi.Error;
+				END
+			ELSE
+				IF restLen = 0 THEN
+					transfer.transfered:= transfer.len;
+					transfer.status := Usbdi.Ok;
+				ELSE
+					transfer.transfered := transfer.len - restLen;
+					transfer.status := Usbdi.ShortPacket;
+					transfer.errors := transfer.errors + UsbHcdi.ShortPacket;
+				END
+			END;
+		END UpdatePipeStatusInterrupt;
+		*)
+
+		PROCEDURE UpdatePipeStatusIso (pipe: UsbHcdi.Pipe);
+		VAR
+			transfer: UsbHcdi.TransferToken;
+			itd: Itd;
+			dword, errors: SET;
+			len, tx, totalItd, activeItd: LONGINT;
+			active: BOOLEAN;
+		BEGIN
+			(*ShowFramelist(ADDRESSOF(framelist.data[framelist.base]), framelistSize, LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize, TRUE);*)
+			(*TRACE('Update acquire Tk Read');*)
+			(*pipe.transferLock.AcquireRead;
+			framelistLock.Acquire;*)
+			transfer := pipe.transfers;
+			WHILE (transfer # NIL) DO
+				IF transfer.status = Usbdi.InProgress THEN
+					errors := {};
+					len := 0;
+					tx := 0;
+					totalItd := 0;
+					activeItd := 0;
+					active := FALSE;
+					(*TRACE('Update acquire FL');*)
+					WHILE (totalItd < UsbHcdi.MaxTDs) & (transfer.tds[totalItd] # ADDRESS(0)) (*& (transfer.tds[totalItd] # ADDRESS(1))*) DO
+						ASSERT(transfer.tds[totalItd] # ADDRESS(1));
+						itd := SYSTEM.GET32(transfer.tds[totalItd]);
+						(*(*! TRACE *)KernelLog.Enter; KernelLog.String('Update '); KernelLog.Address(itd); KernelLog.Char(' '); KernelLog.Int(SYSTEM.VAL(ADDRESS, itd) MOD 32, 0);
+						KernelLog.Char(' '); KernelLog.Int(SYSTEM.VAL(ADDRESS, itd) DIV 32, 0); KernelLog.Exit;*)
+						ASSERT(SYSTEM.VAL(ADDRESS, itd) MOD 32 = 0);
+						ASSERT(SYSTEM.VAL(ADDRESS, itd) DIV 32 # 0);
+						(*Machine.InvalidateDCacheRange(itd, sizeItd);*)
+						FOR tx := 0 TO 7 DO
+							dword := SYSTEM.VAL(SET, itd.transactions[tx]);
+							IF ItdActive * dword = {} THEN
+								(* Transaction finished, examine *)
+								errors := errors + dword * ItdTransactionStatus;
+								INC(len, LSH(SYSTEM.VAL(LONGINT, dword * ItdTransactionLength), -16));
+							ELSE
+								active := TRUE;
+							END
+						END;
+						INC(totalItd);
+					END;
+					(*TRACE('Update release FL');*)
+					(*KernelLog.Ln;*)
+					IF errors # {} THEN
+						(*ShowItd(itd, 8);*)
+						IF ItdDataBufferError * errors # {} THEN
+							transfer.errors := transfer.errors + UsbHcdi.Databuffer
+						END;
+						IF ItdBabbleDetected * errors # {} THEN
+							transfer.errors := transfer.errors + UsbHcdi.Babble
+						END;
+						transfer.status := Usbdi.Error;
+						IF ItdTransactionError * errors # {} THEN
+							transfer.errors := transfer.errors + UsbHcdi.Stalled;
+							transfer.status := Usbdi.Stalled
+						END;
+						BEGIN {EXCLUSIVE}
+							UnlinkIso(transfer)
+						END
+					ELSIF ~active THEN
+						IF len = transfer.len THEN
+							transfer.transfered := transfer.len;
+							transfer.status := Usbdi.Ok
+						ELSE
+							transfer.transfered := len;
+							transfer.status := Usbdi.ShortPacket;
+							transfer.errors := pipe.errors + UsbHcdi.ShortPacket;
+						END;
+						BEGIN {EXCLUSIVE}
+							UnlinkIso(transfer)
+						END
+					ELSE
+						(*transfer.transfered := len*)
+					END
+				END;
+				transfer := transfer.next;
+			END;
+			(*framelistLock.Release;
+			pipe.transferLock.ReleaseRead;*)
+			(*TRACE('Update release Tk Read');*)
+			(*TRACE(activeItd, totalItd)*)
+		END UpdatePipeStatusIso;
+
+		(* Reset the host controller. Note: This will NOT assert reset on the USB downstream ports. *)
+		PROCEDURE HardwareReset * () : BOOLEAN;
+		CONST MaxWaits = 1000; (* Timeout in milliseconds the HC must have completed the reset command *)
+		VAR dword : SET; i : LONGINT;
+		BEGIN
+			(* Host software mustn't reset the host controller when it's running. Stop it and ... *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			SYSTEM.PUT32(iobase + HcUsbCmd, dword - CmdRunStop); FlushPCI;
+			(* ... wait until the HC is halted.*)
+			i := 1; dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts));
+			WHILE (dword * StsHcHalted = {}) & (i <= MaxWaits) DO
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts));
+				INC(i); Wait(1);
+			END;
+
+			IF dword * StsHcHalted = {} THEN (* HC did not stop *) RETURN FALSE; END;
+
+			(* Do the actual reset operation *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			SYSTEM.PUT32(iobase + HcUsbCmd, dword + CmdHcReset); FlushPCI;
+
+			(* The host controller should clear the HCRESET bit when it has finished resetting *)
+			FOR i := 1 TO MaxWaits DO
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+				IF dword * CmdHcReset = {} THEN
+					RETURN TRUE;
+				END;
+				Wait(1);
+			END;
+			RETURN FALSE;
+		END HardwareReset;
+
+		(* HC moves to UsbSuspend state und almost all operational registers are reset.
+  		Does not affect the root hub and its downstream ports *)
+		PROCEDURE SoftwareReset*() : BOOLEAN;
+		BEGIN
+			(* TODO: Implement *)
+			RETURN FALSE;
+		END SoftwareReset;
+
+		(* Initialization of the data structures of the Host Controller Communication Area *)
+		PROCEDURE InitFrameList(): BOOLEAN;
+		VAR fstn, i, k, j, shift : LONGINT;
+		BEGIN
+			(* Host controller interface data structures should not cross page-boundaries... 32 byte alignment.
+			These queue heads are used as skeleton and never contain any qTDs. *)
+			qhlist := UsbHcdi.GetAlignedMemSpace(2048, 4096);
+			framelist := UsbHcdi.GetAlignedMemSpace(4096, 4096); (* Must be 4K aligned *)
+			framelist := UsbHcdi.GetAlignedMemSpace(1024*1024, 1024*1024); (* Must be 4K aligned *)
+			Machine.DisableDCacheRange(ADDRESSOF(framelist.data[framelist.base]), 1024*1024);
+			IF capIsoSchedThreshold >= 8 THEN
+				framelistOfs := capIsoSchedThreshold MOD 8
+			ELSIF capIsoSchedThreshold > 0 THEN
+				framelistOfs := 1
+			ELSE
+				framelistOfs := 0
+			END;
+
+			(* Mark all entries of the framelist as invalid *)
+			FOR i := 0 TO 1024 - 1 DO
+				framelist.data[framelist.base + i] := 1
+			END;
+
+			(* Set up QHs. 11 Interrupt QHs and the isochronousQh + fstn *)
+			shift := sizeQh DIV 4; ASSERT(sizeQh MOD 4 = 0);
+			FOR i := 0 TO 12 DO
+				qhlist.data[qhlist.base + i*shift + (QhEpCapabilities1 DIV 4)] := 0;
+				qhlist.data[qhlist.base + i*shift + (QhEpCapabilities2 DIV 4)] := 0;
+				qhlist.data[qhlist.base + i*shift + (QhQtdToken DIV 4)] := 0;
+				qhlist.data[qhlist.base + i*shift + (QhCurrentQtdPointer DIV 4)] := 0;
+				qhlist.data[qhlist.base + i*shift + (QhNextQtdPointer DIV 4)] := 1; (* Pointer not valid *)
+				qhlist.data[qhlist.base + i*shift + (QhAltNextQtdPointer DIV 4)] := 1; (* Pointer not valid *)
+				FOR j := 0 TO 4 DO
+					qhlist.data[qhlist.base + i*shift + (QhBufferPointer0 DIV 4) + j] := 0;
+				END;
+				IF cap64bit THEN
+					FOR j := 0 TO 4 DO
+						qhlist.data[qhlist.base + i*shift + (QhExtBufferPointer0 DIV 4) + j] := 0;
+					END;
+				END;
+			END;
+
+			(* Addresses of queue heads *)
+			NEW(interruptQh);
+			FOR i := 0 TO 10 DO interruptQh[i] := Machine.Ensure32BitAddress (ADDRESSOF(qhlist.data[qhlist.base]) + i*sizeQh); END;
+			fstn := interruptQh[10] + sizeQh; (* Actually 8 bytes *)
+			isochronousQh := fstn + sizeQh;
+
+			FOR i := 10 TO 2 BY -1 DO
+				SYSTEM.PUT32(interruptQh[i] + QhHorizontalLinkPointer, interruptQh[i-1] + LSH(QhTypQh, 1));
+			END;
+
+			(* Link restore indicator. InterruptQh[1] points to FSTN points to InterruptQh[0] *)
+			SYSTEM.PUT32(interruptQh[1] + QhHorizontalLinkPointer, fstn + LSH(QhTypFstn, 1));
+			SYSTEM.PUT32(fstn + FstnNormalPathLinkPointer, interruptQh[0] + LSH(QhTypQh, 1));
+			SYSTEM.PUT32(fstn + FstnBackPathLinkPointer, QhTerminate); (* Indicates restore indicator *)
+
+			(* Interrupt Qh for 1ms points to isochronousQh *)
+			SYSTEM.PUT32(interruptQh[0] + QhHorizontalLinkPointer, isochronousQh + LSH(QhTypQh, 1));
+			SYSTEM.PUT32(isochronousQh + QhHorizontalLinkPointer, QhTerminate);
+
+			(* tree structure:
+			    interrupt[0]: 1ms    interrupt[1]: 2ms    interrupt[2]: 4ms    interrupt[3]: 8ms    interrupt[4]: 16ms	    interrupt[5]: 32ms  *)
+
+			(* => end of queue 10 points to 9, end of 8 points to 7 , ..., end of 1 points to 0 *)
+			(* => if we start at queue 10, then we will pass all others too; if we start at 9 then we will pass all queues < 9, too etc.*)
+
+			(* queue 0 executes 1024x, queue 1 executes 512x, queue 2 executes 256x,  queue 3 executes 128x*)
+			(* queue 4 executes 64x, queue 5 executes 32x, queue 6 executes 16x,  queue 7 executes 8x*)
+			(* queue 8 executes 4x, queue 9 executes 2x, queue 10 executes 1x *)
+
+			(* What does the following mean? => We count the 1's (starting at lsb) until we pass a zero *)
+			(* This count gives the queue number for a given slot *)
+
+			FOR i := 0 TO 1023 DO (* i is slot number, we want to calc the queue number (k) for this slot *)
+				k := 0; j := i;
+				LOOP
+					IF (SYSTEM.VAL(SET, j) * {0}) = {} THEN EXIT; END;
+					INC(k); j := j DIV 2;
+				END;
+				framelist.data[framelist.base + i] := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, interruptQh[k]) + {1});
+				(*Machine.FlushDCacheRange(ADDRESSOF(framelist.data[framelist.base]), 4096);*)
+			END;
+			RETURN TRUE;
+		END InitFrameList;
+
+		(* Initializes the host controller and builds up the data structures of the HCCA.
+		 * @param iobase I/O base address (virtual, pointing to capability register at offset 0)
+		 * @param int Interrupt Line
+		 * @return TRUE if initialization succeeded, FALSE otherwise
+		 *)
+		PROCEDURE Init * (iobase , irq  :  LONGINT) : BOOLEAN;
+		VAR
+			reg  : LONGINT;
+			dword : SET; qword : HUGEINT;
+			ignore : BOOLEAN;
+			i : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceInit THEN KernelLog.String("UsbEhci: Starting host controller initialization..."); KernelLog.Ln; END;
+			SELF.iobase := iobase; SELF.irq := irq;
+			isHighSpeed := TRUE;  DMAchaining := TRUE; sgListSize := ScatterGatherListSize;
+
+			(* Read in the Host Controller Capability Registers *)
+			(* Get and check EHCI revision *)
+			reg := SYSTEM.GET16(iobase + HcCapHciVersion);
+			IF reg # 0100H THEN
+				KernelLog.String("UsbEhci: Revision of EHCI Programming Interface not supported."); KernelLog.Ln;
+				RETURN FALSE;
+			END;
+
+			(* Get and parse the HC structural parameters register (HCSPARAM) *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcCapSparams));
+			capDebugPortNumber := SYSTEM.VAL(LONGINT, LSH(dword * {20..31}, -20));
+			IF dword * {16} # {} THEN capPortIndicators := TRUE; ELSE capPortIndicators := FALSE; END;
+			capNbrOfCompanionHc := SYSTEM.VAL(LONGINT, LSH(dword * {12..15}, -12));
+			capPortsPerCompanion := SYSTEM.VAL(LONGINT, LSH(dword * {8..11}, -8));
+			IF dword * {7} # {} THEN capPortRoutingRules := TRUE; ELSE capPortRoutingRules := FALSE; END;
+			IF dword * {4} # {} THEN capPortPowerControl := TRUE; ELSE capPortPowerControl := FALSE; END;
+			capNbrOfPorts  := SYSTEM.VAL(LONGINT, dword * {0..3});
+
+			(* Get and parse the HC capability parameters register (HCCPARAM) *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcCapCparams));
+			capIsoSchedThreshold := SYSTEM.VAL(LONGINT, LSH(dword * {4..7}, -4));
+			IF dword * {2} # {} THEN capAsynchSchedPark := TRUE; ELSE capAsynchSchedPark := FALSE; END;
+			IF dword * {1} # {} THEN capProgrammableFLG := TRUE; ELSE capProgrammableFLG := FALSE; END;
+			IF dword * {0} # {} THEN cap64bit := TRUE; ELSE cap64bit := FALSE; END;
+			(* Get the EHCI Extended Capabilities Pointer (EECP) *)
+			eecp := SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, dword) * {8..15}, - 8));
+
+			(* Get HC companion port route description (60bits, 4bits per port *)
+			IF capPortRoutingRules THEN (* HC companion port route description available *)
+				qword := SYSTEM.GET32(iobase + HcCapPortroute);
+				qword := qword + LSH(SYSTEM.GET32(iobase + HcCapPortroute + 4), 32);
+				NEW(hcportroute, 16);
+				FOR i := 0 TO 15 DO
+					hcportroute[i] := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, LSH(qword, -i)) * {0..3});
+				END;
+			END;
+
+			(* Program the framelist size *)
+			IF capProgrammableFLG THEN (* Size of frame list can be programmed... use constant value *)
+				(* TODO: Programm it *)
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+				dword := dword - CmdFrameListSize - {15};
+				SYSTEM.PUT32(iobase + HcUsbCmd, dword);
+				framelistSize := 1024;
+			ELSE
+				framelistSize := 1024 (*LSH(SYSTEM.VAL(LONGINT, dword * {2 .. 3}), -2)*);
+				(*IF 15 IN dword THEN INC(framelistSize, 4) END;*)
+			END;
+			IF Debug.Trace & Debug.traceInit THEN
+				KernelLog.String("UsbEhci: Set frame list size to "); KernelLog.Int(framelistSize, 0);
+				KernelLog.String(" elements."); KernelLog.Ln;
+			END;
+
+			(* Build the emulated hub descriptor *)
+			dword := {};
+			NEW(hubDescriptor, 8);
+			hubDescriptor[0] := CHR(7);
+			hubDescriptor[1] := CHR(29H); (* Hub Descriptor *)
+			hubDescriptor[2] := CHR(capNbrOfPorts);
+			IF capPortPowerControl THEN (* If power control is available, EHCI root hubs always provide per port power control *)
+				dword := dword + {0};
+			ELSE (* No power switching implemented *)
+				dword := dword + {1};
+			END;
+			dword := dword + {3}; (* EHCI root hubs always provide per port overcurrent protection *)
+			IF capPortIndicators THEN (* Port indicator control support available *) dword := dword + {7}; END;
+			hubDescriptor[3] := CHR(SYSTEM.VAL(LONGINT, dword));
+			hubDescriptor[4] := CHR(0); (* Reserved *)
+			hubDescriptor[5] := CHR(10); (* 20ms Power on to power good *)
+			hubDescriptor[6] := CHR(0); (* Root hubs don't draw current from the USB *)
+
+			(* The Host Controller Capability Registers are readonly so we don't need further access to them and set
+			iobase to the base of the Host Controller Operational Registers *)
+			iobase := iobase + SYSTEM.GET8(iobase + HcCapLength);
+			SELF.iobase := iobase;
+
+			(* Calculate offset from iobase of the port status/controll register for each port *)
+			portCount := capNbrOfPorts;
+			NEW(ports, portCount);
+			FOR i := 0 TO portCount - 1 DO ports[i] := iobase + HcPortSc + i*4; END;
+
+			IF ~HardwareReset() THEN RETURN FALSE; END;
+
+			(* Bluebottle does not yet support 64bit address space. Set the 4GB segment selector for the control data structures to zero. *)
+			SYSTEM.PUT32(iobase + HcCtrlDsSegment, 0);
+
+			(* Note that the control data structures must finally be 32byte aligned. Since they occupy subsequent memory location when
+			associated with pipes, the value are rounded up to the next value for which value MOD 32 = 0 holds. *)
+			IF cap64bit THEN
+				sizeQh := 96; (* Actually: 68 Bytes *)
+				sizeQtd := 64; (* Actually: 52 Bytes *)
+				sizeItd := 96;
+				(*alignQh := 128;
+				alignQtd := 64;*)
+			ELSE
+				sizeQh := 64; (* Actually: 48 Byte *)
+				sizeQtd := 32;
+				sizeItd := 64;
+				(*alignQh := 64;
+				alignQtd := 32;*)
+			END;
+			(* Allocation scheme takes care of 4kB page crossing *)
+			alignQh := 32;
+			alignQtd := 32;
+			alignItd := 32;
+
+			IF ~InitFrameList() THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbEhci: Initialization of HCCA failed."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			(* If the Asynchronous Schedule Park Mode is not available or not enabled, the host controller must not
+			 * execute more than one bus transaction per queue head, per traversal of the asynchronous schedule. If it
+			 * is enabled, the host controller may execute Asynchronous Schedule Park Mode Count transaction if the
+			 * endpoint belongs to a high-speed device. Results in better bus utilization. *)
+			IF capAsynchSchedPark THEN
+				ASSERT((HcAsyncParkModeCount >= 0) & (HcAsyncParkModeCount < 4));
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+				IF HcAsyncParkModeCount = 0 THEN (* Disable Asynchronous Schedule Park Mode *)
+					dword := dword - CmdAsyncSchedParkMode;
+				ELSE (* Enable Asynchronous Schedule Park Mode and set its count field in USBCMD *)
+					dword := dword + LSH(SYSTEM.VAL(SET, HcAsyncParkModeCount), 8) * CmdAsyncSchedParkCount;
+					dword := dword + CmdAsyncSchedParkMode;
+				END;
+				SYSTEM.PUT32(iobase + HcUsbCmd, dword);
+			END;
+
+			(* Set interrupt threshold *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			dword := dword - {16 .. 23};
+			IF ((HcInterruptThreshold#01H) & (HcInterruptThreshold#02H) & (HcInterruptThreshold#04H) & (HcInterruptThreshold#08H)
+			    & (HcInterruptThreshold#10H) & (HcInterruptThreshold#20H) & (HcInterruptThreshold#40H)) THEN
+				(* Wrong parameter value... use default *)
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbEhci: Interrupt Threshold value invalid... using default setting."); KernelLog.Ln; END;
+				dword := dword + SYSTEM.VAL(SET, LSH(08H, 16)) * {16..23};
+			ELSE
+				dword := dword + SYSTEM.VAL(SET, LSH(HcInterruptThreshold, 16)) *  {16..23};
+			END;
+			SYSTEM.PUT32(iobase + HcUsbCmd, dword); FlushPCI;
+
+			(* Try to start the host controller *)
+			IF Start() = FALSE THEN (* ERROR: Couldn't start the host controller. Controller was probably not correctly initialized. *)
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbEhci: Couldn't start host controller."); KernelLog.Ln; END;
+				ignore := HardwareReset();
+				RETURN FALSE;
+			END;
+
+			RETURN TRUE;
+		END Init;
+
+		(* PCI writes may be posted. A read forces posted writes to be flushed before the read transaction is proceeded. *)
+		PROCEDURE FlushPCI *;
+		VAR ignore : LONGINT;
+		BEGIN
+			ignore := SYSTEM.GET32(iobase + HcUsbSts);
+		END FlushPCI;
+
+		(* Release the HC ownership semaphore; eecp is the EHCI Extended Capability Pointer *)
+		PROCEDURE ReleaseHcOwnerShip(bus, device, function, eecp : LONGINT);
+		END ReleaseHcOwnerShip;
+
+		(*
+		 * Start the host controller.
+		 * This will:
+		 * - enable interrupts for the host controller and install a interrupt handler
+		 * - set the addresses for the periodic and asynchronous lists
+		 * - turn the host controller on
+		 * - route all ports to the EHCI controller
+		 * - power on all ports of the root hub
+	  	 *)
+		PROCEDURE Start * ():BOOLEAN;
+		VAR dword : SET;
+		TYPE 
+			IRQPoller = OBJECT (* support polling i/o IRQ -- for testing *)
+				VAR handler: PROCEDURE {DELEGATE}; timer: Kernel.Timer;
+				
+				PROCEDURE & Init* (h: PROCEDURE {DELEGATE});
+				BEGIN
+					handler := h;
+					NEW(timer); 
+				END Init;
+				
+			BEGIN{ACTIVE}
+				LOOP
+					handler();
+					timer.Sleep(100);
+				END;
+			END IRQPoller;
+		VAR is: IRQPoller;
+		BEGIN
+			IF Debug.Trace & Debug.traceInit THEN KernelLog.String("UsbEhci: Starting host controller... "); KernelLog.Ln; END;
+			(* Enable Interrupts *)
+			
+			IF Polling THEN
+				NEW(is, InterruptHandler)
+			ELSE
+			END;
+			Objects.InstallHandler(InterruptHandler, Machine.IRQ0+irq);
+
+			(* Clear interrupts *)
+			SYSTEM.PUT32(iobase + HcUsbSts, 0);
+			
+			(* Enable all interrupts except the frame list rollover interrupt *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbIntr));
+			interruptsEnabled := dword + {0..5}  - StsFrameListRollover;
+			SYSTEM.PUT32(iobase + HcUsbIntr, interruptsEnabled);
+
+			(* Set Addresses for queue heads *)
+			SYSTEM.PUT32(iobase + HcPeriodicListBase, ADDRESSOF(framelist.data[framelist.base]));
+			SYSTEM.PUT32(iobase + HcAsyncListAddr, 0); (* Invalid address -> list empty *)
+
+			(* Start controller *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			dword := dword + CmdRunStop;
+			SYSTEM.PUT32(iobase + HcUsbCmd, dword); FlushPCI;
+			SetState(UsbHcdi.Operational);
+
+			(* Route all ports to this EHCI host controller *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcConfigFlag));
+			dword := dword + {0};
+			SYSTEM.PUT32(iobase + HcConfigFlag, dword); FlushPCI;
+
+			SetState(UsbHcdi.Initialized);
+			RETURN TRUE;
+		END Start;
+
+		PROCEDURE &Default*(bus, device, function : LONGINT);
+		BEGIN
+			Default^(bus, device, function); (* The high-speed default pipe uses 64byte maxPacketSize0 *)
+			pipes[0, 0, 0].maxPacketSize := 64;
+			NEW(allocator, 4096, 32);
+			NEW(framelistLock);
+			NEW(handler, SELF)
+		END Default;
+
+		PROCEDURE Cleanup;
+		BEGIN
+			IF state >= UsbHcdi.Initialized THEN Objects.RemoveHandler(InterruptHandler, Machine.IRQ0 + irq); END;
+			Cleanup^;
+			IF ~HardwareReset() THEN
+				IF Debug.Level >= Debug.Errors THEN Show("Host controller reset failed."); KernelLog.Ln; END;
+			END;
+			(* Release ownership of host controller *)
+			ReleaseHcOwnerShip(bus, device, function, eecp);
+			(* Unmap the HC's operational registers *)
+			Machine.UnmapPhysical(iobase, 4096);
+			handler.Stop
+		END Cleanup;
+
+		(** Displays the host controller's data struture on KernelLog *)
+		PROCEDURE ShowSchedule*;
+		CONST MaxIterations =21;
+		VAR dword : SET; first, cur : LONGINT; i, ms : LONGINT;
+		BEGIN
+			IF Debug.Trace THEN
+				KernelLog.String("Host Controller Data Structures for ");
+				KernelLog.String(name);
+				KernelLog.String(" ("); KernelLog.String(desc); KernelLog.String("):"); KernelLog.Ln;
+				KernelLog.String("Periodic Schedule: "); KernelLog.Ln;
+				IF SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * StsPeriodicSchedule = {} THEN
+					KernelLog.String("Periodic schedule is disabled."); KernelLog.Ln;
+				END;
+
+				KernelLog.String("*** Isochronous schedule: "); KernelLog.Ln;
+				IF SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * StsPeriodicSchedule # {} THEN
+					ShowFramelist(ADDRESSOF(framelist.data[framelist.base]), framelistSize, LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize, TRUE)
+				ELSE
+					KernelLog.String("Periodic schedule is not enabled"); KernelLog.Ln
+				END;
+
+				KernelLog.String("*** Asynchronous list: "); KernelLog.Ln;
+				IF SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts)) * StsAsyncSchedule # {} THEN
+					first := SYSTEM.GET32(iobase + HcAsyncListAddr);
+					IF (SYSTEM.VAL(SET, first) * {0..4} = {}) & (first # 0) THEN
+						first := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, first) * {5..31});
+						ShowQueueHead(first, 0, cap64bit);
+						i := 0; cur := first;
+						LOOP
+							cur := SYSTEM.GET32(cur + QhHorizontalLinkPointer);
+							IF (SYSTEM.VAL(SET, cur) * {2..4} # {}) OR (SYSTEM.VAL(SET, cur) * {1} = {}) THEN
+								KernelLog.String("Error: Queue head horizontal link pointer is invalid."); KernelLog.Ln;
+								EXIT;
+							END;
+							cur := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, cur) * {5..31});
+							IF (cur = first) OR (i >= MaxIterations) THEN EXIT END;
+
+							ShowQueueHead(cur, 0, cap64bit);
+							INC(i);
+						END;
+						IF i >= MaxIterations THEN
+							KernelLog.String("MaxIterations reached. Aborting..."); KernelLog.Ln;
+						END;
+					ELSE KernelLog.String("Error: Asynchronous Schedule List address is invalid.");
+					END;
+				ELSE KernelLog.String("Asynchronous Schedule is disabled.");
+				END;
+				KernelLog.Ln;
+			END;
+		END ShowSchedule;
+
+		PROCEDURE ShowPipe*(pipe : UsbHcdi.Pipe);
+		VAR
+			qh: Qh;
+		BEGIN
+			IF pipe.descriptors # NIL THEN
+				qh := SYSTEM.VAL(Qh, pipe.descriptors[0]);
+				IF (qh.next # 0) & (SYSTEM.VAL(SET, qh.next) * QhTerminate = {}) THEN
+					KernelLog.String("QH and qTD link:"); KernelLog.Ln;
+					ShowQueueHead(qh, qh.next, cap64bit)
+				ELSIF (qh.current # 0) & (SYSTEM.VAL(SET, qh.current) * QhTerminate = {}) THEN
+					KernelLog.String("QH and current qTD:"); KernelLog.Ln;
+					ShowQueueHead(qh, qh.current, cap64bit)
+				ELSE
+					KernelLog.String("QH with no scheduled qTD:"); KernelLog.Ln;
+					ShowQueueHead(qh, 0, cap64bit)
+				END
+			END
+		END ShowPipe;
+
+		(* Show some information on this host controller on KernelLog *)
+		PROCEDURE Diag;
+		VAR dword : SET;
+		BEGIN
+			IF Debug.Trace THEN
+			Diag^;
+			(* Host Controller structural capabilities *)
+			KernelLog.String("  HC Structural Parameters: "); KernelLog.Ln;
+			KernelLog.String("    Nbr of ports: "); KernelLog.Int(capNbrOfPorts, 0);
+			KernelLog.String(", Debug port: ");
+			IF capDebugPortNumber # 0 THEN KernelLog.Int(capDebugPortNumber, 0); ELSE KernelLog.String("n/a"); END;
+			KernelLog.Ln;
+			KernelLog.String("    Per port power control: ");
+			IF capPortPowerControl THEN KernelLog.String("Yes"); ELSE KernelLog.String("No"); END;
+			KernelLog.String(", Port indicator control: ");
+			IF capPortIndicators THEN KernelLog.String("Available"); ELSE KernelLog.String("n/a"); END;
+			KernelLog.Ln;
+			KernelLog.String("    Nbr of companion HCs: "); KernelLog.Int(capNbrOfCompanionHc, 0);
+			KernelLog.String(", Ports per companion: "); KernelLog.Int(capPortsPerCompanion, 0);
+			KernelLog.String(", Port routing rules: ");
+			IF capPortRoutingRules THEN KernelLog.String("Available"); ELSE KernelLog.String("n/a"); END;
+			KernelLog.Ln;
+			(* Host Controller capability parameters *)
+			KernelLog.String("  HC Capablilty Parameters:"); KernelLog.Ln;
+			KernelLog.String("    64bit Data Structures: "); IF cap64bit THEN KernelLog.String("Yes"); ELSE KernelLog.String("No"); END;
+			KernelLog.String(", Async Schedule Park Mode support: ");  IF capAsynchSchedPark THEN KernelLog.String("Yes"); ELSE KernelLog.String("No"); END;
+			KernelLog.Ln;
+			KernelLog.String("    Programmable Frame List Size: "); IF capAsynchSchedPark THEN KernelLog.String("Yes"); ELSE KernelLog.String("No"); END;
+			KernelLog.String(", Isochronous Scheduling Threshold: "); KernelLog.Int(capIsoSchedThreshold, 0);
+			KernelLog.Ln;
+			(* Host Controller Command Register *)
+			KernelLog.String("  HC Command Register: "); KernelLog.Ln;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			KernelLog.String("    Interrupt Threshold: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * {16..23}, -16)), 0);
+			KernelLog.String(", Async Schedule Park Mode: ");
+			IF dword * {11} # {} THEN
+				KernelLog.String("Enabled ("); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * {8..9}, -8)), 0); KernelLog.Char(")");
+			ELSE
+				KernelLog.String("Disabled");
+			END;
+			KernelLog.String(", Frame List Size: ");
+			CASE SYSTEM.VAL(LONGINT, LSH(dword * {2..3}, -2)) OF
+			 	0: KernelLog.String("1024");
+			 	|1: KernelLog.String("512");
+			 	|2: KernelLog.String("256");
+			 	|3: KernelLog.String("Reserved");
+			 END;
+			KernelLog.Ln;
+			(* Host Controller Status information *)
+			KernelLog.String("  HC Status Register:"); KernelLog.Ln;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbSts));
+			KernelLog.String("      Asynchronous Schedule: ");
+			IF dword * StsAsyncSchedule # {} THEN KernelLog.String("Enabled"); ELSE KernelLog.String("Disabled"); END;
+			KernelLog.String(", Periodic Schedule: ");
+			IF dword * StsPeriodicSchedule # {} THEN KernelLog.String("Enabled"); ELSE KernelLog.String("Disabled"); END;
+			KernelLog.String("  ");
+			IF dword * StsReclamation # {} THEN KernelLog.String("[Reclamation]"); END;
+			IF dword * StsHcHalted # {} THEN KernelLog.String("[HcHalted]"); END;
+			KernelLog.Ln;
+			KernelLog.String("      Interrupt Status: "); ShowInterrupts(dword); KernelLog.Ln;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbIntr));
+			KernelLog.String("      Interrupts Enabled: "); ShowInterrupts(dword); KernelLog.Ln;
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbCmd));
+			KernelLog.String("    HC operation: ");
+			IF dword * {0} # {} THEN KernelLog.String("Running"); ELSE KernelLog.String("Stopped"); END;
+			KernelLog.Ln;
+			END;
+		END Diag;
+
+		PROCEDURE Show * (CONST txt : ARRAY OF CHAR);
+		BEGIN
+			KernelLog.String("UsbEhci: "); KernelLog.String(name); KernelLog.String(" ("); KernelLog.String(desc); KernelLog.String("): ");
+			KernelLog.String(txt);
+		END Show;
+
+		PROCEDURE ShowHcFl;
+		BEGIN
+		 	ShowFramelist(ADDRESSOF(framelist.data[framelist.base]), framelistSize, LSH(SYSTEM.GET32(iobase + HcFrIndex), -3) MOD framelistSize, FALSE)
+		 END ShowHcFl;
+	END EnhancedHostController;
+
+
+	Handler = OBJECT
+	VAR
+		hc: EnhancedHostController;
+		handle, active, trapped, next: BOOLEAN;
+		interrupts: SET;
+
+		PROCEDURE & Init (hc: EnhancedHostController);
+		BEGIN
+			SELF.hc := hc;
+			active := TRUE
+		END Init;
+
+		PROCEDURE Handle (interrupts: SET);
+		BEGIN {EXCLUSIVE}
+			IF next THEN
+				(*KernelLog.Enter; KernelLog.String("HANDLER: merging"); KernelLog.Exit;*)
+				SELF.interrupts := SELF.interrupts + interrupts
+			ELSE
+				IF handle THEN
+					(*KernelLog.Enter; KernelLog.String("HANDLER: buffering"); KernelLog.Exit;*)
+					next := TRUE
+				ELSE
+					(*KernelLog.Enter; KernelLog.String("HANDLER: handling"); KernelLog.Exit;*)
+					handle := TRUE
+				END;
+				SELF.interrupts := interrupts
+			END;
+		END Handle;
+
+		PROCEDURE Stop;
+		BEGIN {EXCLUSIVE}
+			active := FALSE
+		END Stop;
+
+	BEGIN {ACTIVE, SAFE}
+		IF trapped THEN
+			hc.Show("WARNING: interrupt handler trapped");
+			KernelLog.Ln
+		END;
+		trapped := TRUE;
+		handle := FALSE;
+		LOOP
+			BEGIN {EXCLUSIVE}
+				AWAIT(~active OR handle);
+				IF ~active THEN EXIT END;
+			END;
+			hc.DoHandleInterrupt(interrupts);
+			BEGIN {EXCLUSIVE}
+				(*KernelLog.Enter; KernelLog.String("HANDLER: done -- "); KernelLog.Boolean(handle); KernelLog.String(", "); KernelLog.Boolean(next); KernelLog.Exit;*)
+				handle := next;
+				next := FALSE
+			END;
+		END
+	END Handler;
+
+PROCEDURE AssertAlignment(desc: ADDRESS; alignment: SIZE);
+BEGIN
+	ASSERT(desc MOD alignment = 0);
+END AssertAlignment;
+
+(**
+ * Computes the address of the next 4kB page in the page sequence sgList.
+ *
+ * Return the physical page of size 4kB which is right after the physical address (adr + size) in the page ranges sgList.
+ * idx is the page index of adr in sgList. When returning, idx indexes the sgList entry containing the return value.
+ *)
+PROCEDURE GetNextPhysicalPage (CONST sgList: ARRAY OF Machine.Range; VAR idx: LONGINT; adr: ADDRESS): ADDRESS;
+BEGIN
+	IF adr + PageSize - adr MOD PageSize >= sgList[idx].adr + sgList[idx].size THEN
+		(* Next Page beginning not in the current range: use the next range *)
+		INC(idx);
+		ASSERT(idx < LEN(sgList));
+		RETURN sgList[idx].adr
+	ELSE
+		(* Align to next page size *)
+		RETURN adr + (-adr) MOD PageSize
+	END;
+END GetNextPhysicalPage;
+
+PROCEDURE AdvancePhysBuf (CONST sgList: ARRAY OF Machine.Range; VAR idx: LONGINT; adr: ADDRESS; size: LONGINT): ADDRESS;
+VAR
+	next: LONGINT;
+BEGIN
+	next := adr + size;
+	WHILE next - sgList[idx].adr > sgList[idx].size DO
+		(* Range continues into next buffer *)
+		DEC(size, sgList[idx].size);
+		INC(idx);
+		next := sgList[idx].adr + size
+	END;
+	RETURN next
+END AdvancePhysBuf;	
+
+PROCEDURE Indent(spaces : LONGINT);
+VAR i : LONGINT;
+BEGIN
+	FOR i := 1 TO spaces DO KernelLog.Char(" "); END;
+END Indent;
+
+(*
+ * Display a textual representation of a queue head data structure and its associated qTD.
+ * @param qh Virtual memory address of queue head
+ * @param firstQtd First qTD of this queue. If 0, the qTD chain will not be shown
+ *)
+PROCEDURE ShowQueueHead(qh, firstQtd : ADDRESS; cap64bit : BOOLEAN);
+CONST MaxChainLen = 32;
+VAR
+	dword : SET;
+	val, chainlen : LONGINT;
+
+	PROCEDURE ShowQhTyp(qh : LONGINT);
+	BEGIN
+		IF Debug.Trace THEN
+		val := SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, qh) * QhTyp, -1));
+		IF val = 0 THEN KernelLog.String("Isochronous Tranfers Desriptor");
+		ELSIF val = 1 THEN KernelLog.String("Queue Head");
+		ELSIF val = 2 THEN KernelLog.String("Split Transaction Isochronous Transfer Descriptor");
+		ELSIF val = 3 THEN KernelLog.String("Frame Span Traversal Node");
+		END;
+		END;
+	END ShowQhTyp;
+
+BEGIN
+	IF Debug.Trace THEN
+	KernelLog.String("EHCI data structure at "); KernelLog.Address(qh); KernelLog.String(": ");
+	ShowQhTyp(qh); KernelLog.String(" ");
+
+	IF qh = 0 THEN KernelLog.String("Error: QH pointer = 0"); KernelLog.Ln;RETURN;
+	ELSIF SYSTEM.VAL(SET, qh) * {0..4} # {} THEN KernelLog.String("Error: Not aligned"); KernelLog.Ln; RETURN;
+	END;
+	KernelLog.Ln;
+
+	KernelLog.String("    Endpoint Capabilities 1: ");
+	dword := SYSTEM.VAL(SET, SYSTEM.GET32(qh + QhEpCapabilities1));
+	KernelLog.String(" DeviceAddr: "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * QhDeviceAddress), 0);
+	KernelLog.String(", Endpoint: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QhEndpointNbr, -8)), 0);
+	KernelLog.String(", Speed: "); val := SYSTEM.VAL(LONGINT, LSH(dword * QhEndpointSpeed, -12));
+	IF val = 0 THEN KernelLog.String("FullSpeed");
+	ELSIF val = 1 THEN KernelLog.String("LowSpeed");
+	ELSIF val = 2 THEN  KernelLog.String("HighSpeed");
+	ELSE KernelLog.String("ERROR: Not set correctly");
+	END;
+	KernelLog.String(", MaxPacketSize: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QhMaxPacketLen, -16)), 0);
+	KernelLog.String(", NakRL: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QhNakCountReload, -28)), 0);
+
+	KernelLog.String(", Flags: ");
+	IF dword * QhControlEndpointFlag # {} THEN	KernelLog.String("[ControlEp]"); END;
+	IF dword * QhDataToggleControl # {} THEN KernelLog.String("[DataToggleControl]"); END;
+	IF dword * QhHeadOfReclamation  # {} THEN KernelLog.String("[Head]"); END;
+	IF dword * QhInactivate # {} THEN KernelLog.String("[Inactivate]"); END;
+	KernelLog.Ln;
+
+	KernelLog.String("    Endpoint Capabilities 2: ");
+	dword := SYSTEM.VAL(SET, SYSTEM.GET32(qh + QhEpCapabilities2));
+	KernelLog.String("Mult: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QhMultiplier, -30)), 0);
+	KernelLog.String(", HubAddr: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QhHubAddr, -16)), 0);
+	KernelLog.String(", HubPort: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QhPortNbr, -23)), 0);
+	KernelLog.String(", SplitCMask: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QhSplitCMask, -8)), 0);
+	KernelLog.String(", QhSMask: "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * QhSMask), 0);
+	KernelLog.Ln;
+
+	KernelLog.String("    Queue Head Horizontal Link Pointer: ");
+	dword := SYSTEM.VAL(SET, SYSTEM.GET32(qh + QhHorizontalLinkPointer));
+	IF dword * QhTerminate # {} THEN
+		KernelLog.String("Invalid ("); KernelLog.Address(SYSTEM.VAL(LONGINT, dword)); KernelLog.String("H)");
+	ELSE
+		KernelLog.Address(SYSTEM.VAL(LONGINT, dword * {5..31}));
+		KernelLog.String(" ("); ShowQhTyp(SYSTEM.VAL(LONGINT, dword)); KernelLog.String(")");
+	END;
+	KernelLog.Ln;
+
+	dword := SYSTEM.VAL(SET, SYSTEM.GET32(qh + QhCurrentQtdPointer));
+	KernelLog.String("    Current qTD Pointer: "); KernelLog.Address(SYSTEM.VAL(LONGINT, dword));
+	KernelLog.String(", Next qTD Pointer: "); KernelLog.Address(SYSTEM.GET32(qh + QhNextQtdPointer));
+	KernelLog.Ln;
+
+	KernelLog.String("    Transfer overlay: "); KernelLog.Ln;
+	ShowQtd(qh+ QhNextQtdPointer, 8, cap64bit, TRUE); KernelLog.Ln;
+
+	IF firstQtd # 0 THEN (* show qTD chain *)
+		KernelLog.String("    qTD chain:"); KernelLog.Ln;
+		IF SYSTEM.VAL(SET, firstQtd) * {0..4} # {} THEN
+			KernelLog.String("        qTD Pointer not 32byte aligned!"); KernelLog.Ln;
+		ELSE
+			chainlen := 0;
+			WHILE(SYSTEM.VAL(SET, firstQtd) * QhTerminate = {}) & (chainlen < MaxChainLen) DO
+				INC(chainlen);
+				ShowQtd(firstQtd, 8, cap64bit, FALSE); KernelLog.Ln;
+				(* Get next qTD *)
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(firstQtd + QtdNextQtdPointer));
+				IF dword * {1..4} # {} THEN
+					KernelLog.String("        Alignment error!"); KernelLog.Ln;
+					chainlen := MaxChainLen; (* abort *)
+				ELSIF dword * QhTerminate # {} THEN
+					KernelLog.String("        End of Chain"); KernelLog.Ln;
+					chainlen := MaxChainLen; (* abort *)
+				ELSE
+					firstQtd := SYSTEM.VAL(LONGINT, dword * {5..31});
+				END;
+			END;
+		END;
+	END;
+	END;
+END ShowQueueHead;
+
+PROCEDURE ShowQtd(adr: ADDRESS; spaces: LONGINT; cap64bit, overlay: BOOLEAN);
+VAR
+	i, val: LONGINT;
+	dword: SET;
+	qtd: Qtd;
+BEGIN
+	IF Debug.Trace THEN
+		qtd := adr;
+		Indent(spaces);
+		KernelLog.String("qTD at "); KernelLog.Address(qtd);  KernelLog.String(": ");
+
+		IF SYSTEM.VAL(SET, qtd) * {0..3} # {} THEN
+			(* Regular qTDs are 32byte aligned. We allow 16byte alignment for the transfer overlay area *)
+			KernelLog.String("Not 16byte aligned... aborting."); KernelLog.Ln; RETURN;
+		ELSIF adr = 0 THEN
+			KernelLog.String("Address = 0?"); KernelLog.Ln; RETURN;
+		END;
+		KernelLog.Ln;
+
+		Indent(spaces+ 4);
+		KernelLog.String("qTD Token: ");
+		dword := SYSTEM.VAL(SET, qtd.token);
+		KernelLog.String("Pid: ");
+		val := SYSTEM.VAL(LONGINT, LSH(dword * QtdPidCode, -8));
+		IF val = PidSetup THEN KernelLog.String("SETUP");
+		ELSIF val = PidIn THEN KernelLog.String("IN");
+		ELSIF val = PidOut THEN KernelLog.String("OUT");
+		ELSE KernelLog.String("PID ERROR!");
+		END;
+
+		KernelLog.String(", DataToggle: ");
+		IF dword * QtdDataToggle # {} THEN KernelLog.String("DATA1"); ELSE  KernelLog.String("DATA0"); END;
+
+		KernelLog.String(", Bytes to transfer: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QtdBytesToTransfer, -16)), 0);
+		KernelLog.String(", IOC: "); IF dword * QtdIoc # {} THEN KernelLog.String("Yes"); ELSE KernelLog.String("No"); END;
+		KernelLog.String(", CERR: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QtdErrorCounter, -10)) ,0);
+		KernelLog.Ln;
+
+		Indent(spaces + 4);
+		KernelLog.String("qTD Token Status: ");
+		dword := dword * QtdStatus;
+		IF dword * TdActive # {} THEN KernelLog.String("[Active]"); END;
+		IF dword * TdHalted # {} THEN KernelLog.String("[Halted]"); END;
+		IF dword * TdDataBufferError # {} THEN KernelLog.String("[DataBufferError]"); END;
+		IF dword * TdBabbleDetected # {} THEN KernelLog.String("[BabbleDetected]"); END;
+		IF dword * TdTransactionError # {} THEN KernelLog.String("[TransactionError]"); END;
+		IF dword * TdMissedMicroFrame # {} THEN KernelLog.String("[MissedMicroFrame]"); END;
+		IF dword * TdSplitTransactionState # {} THEN KernelLog.String("[SplitTransactionState]"); END;
+		IF dword * TdPingState # {} THEN KernelLog.String("[PingState]"); END;
+		KernelLog.Ln;
+
+		Indent(spaces + 4); KernelLog.String("Buffer information:");KernelLog.Ln;
+		Indent(spaces + 8); KernelLog.String("Current Buffer: ");
+		KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * QtdCurrentPage, -12)), 0);
+		IF SYSTEM.VAL(SET, qtd) * {4} # {} THEN (* Should be transfer overlay since not 32byte aligned *)
+			KernelLog.String(", Nak counter: ");
+			KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, qtd.next) * {1..3}, -1)), 0);
+		END;
+		KernelLog.Ln;
+		FOR i := 0 TO 4 DO
+			val := qtd.buffers[i];
+			Indent(spaces + 8);
+			KernelLog.String("Buffer Pointer "); KernelLog.Int(i, 0); KernelLog.String(": "); KernelLog.Address(SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, val) * {12..31}));
+
+			val := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, val) * {0..11});
+			IF i = 0 THEN
+				KernelLog.String("   Current Offset: "); KernelLog.Hex(val, 8);
+			ELSIF overlay & (i = 1) THEN
+				KernelLog.String("   C-prog-mask: "); KernelLog.Hex(val, 8);
+			ELSIF overlay & (i = 2) THEN
+				KernelLog.String("   S-Bytes / Frametag: "); KernelLog.Hex(val, 8);
+			END;
+
+			KernelLog.Ln;
+		END;
+
+		IF cap64bit THEN
+			FOR i := 0 TO 4 DO
+				val := qtd.extBuffers[i];
+				Indent(spaces + 8);
+				KernelLog.String("   ExtBufferPointer"); KernelLog.Int(i, 0); KernelLog.String(": "); KernelLog.Address(val); KernelLog.Ln;
+			END;
+		END;
+
+		Indent(spaces + 4); KernelLog.String("Alternate Next qTD Pointer: ");
+		dword := SYSTEM.VAL(SET, (*SYSTEM.GET32(adr + QtdNextQtdPointer)*) qtd.alternateNext);
+		IF dword * QhTerminate # {} THEN
+			KernelLog.String("Invalid ("); KernelLog.Address(SYSTEM.VAL(LONGINT, dword)); KernelLog.String(")");
+		ELSIF dword * {1..3} # {} THEN
+			KernelLog.String("Alignment Error ("); KernelLog.Address(SYSTEM.VAL(LONGINT, dword)); KernelLog.String(")");
+		ELSE
+			KernelLog.Address(SYSTEM.VAL(LONGINT, dword));
+		END;
+		KernelLog.Ln;
+
+		(*val := SYSTEM.VAL(ADDRESS, qtd);*)
+		Indent(spaces + 4);
+		KernelLog.String("Next qTD Pointer: ");
+		dword := SYSTEM.VAL(SET, qtd.next);
+		(*SYSTEM.GET(SYSTEM.VAL(ADDRESS, qtd) + QtdNextQtdPointer, dword);*)
+		IF dword * QhTerminate # {} THEN
+			KernelLog.String("Invalid ("); KernelLog.Address(SYSTEM.VAL(LONGINT, dword)); KernelLog.String(")");
+			qtd := 0;
+		ELSIF dword * {1..3} # {} THEN
+			KernelLog.String("Alignment Error ("); KernelLog.Address(SYSTEM.VAL(LONGINT, dword)); KernelLog.String(")");
+			qtd := 0;
+		ELSE
+			KernelLog.Address(SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, dword) * {5..31}));
+			qtd := SYSTEM.VAL(LONGINT, dword * {5..31});
+		END;
+		KernelLog.Ln;
+	END;
+END ShowQtd;
+
+(*
+PROCEDURE ShowItd(adr : LONGINT);
+VAR dword : SET; i : LONGINT;
+BEGIN
+	IF Debug.Trace THEN
+	KernelLog.String("UsbEhci: ITD at address "); KernelLog.Hex(adr, 8); KernelLog.Ln;
+	Indent(4);
+	dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr));
+	KernelLog.String("Next Link Pointer: "); KernelLog.Hex(SYSTEM.VAL(LONGINT, dword * {5..31}), 8);
+	IF dword * {0} = {} THEN KernelLog.String(" [VALID]"); ELSE KernelLog.String("[INVALID]"); END;
+	KernelLog.String(", Typ: ");
+	CASE SYSTEM.VAL(LONGINT, LSH(dword * {1..2}, -1)) OF
+		0: KernelLog.String("iTD");
+		|1 : KernelLog.String("QH");
+		|2 : KernelLog.String("siTD");
+		|3 : KernelLog.String("FSTN");
+	END;
+	dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + 24H));
+	KernelLog.String(", Device Address: "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {0..6}), 0);
+	KernelLog.String(", Endpoint: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * {8..11}, -7)), 0);
+	dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + 28H));
+	KernelLog.String(" MaxPacket: "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {0..10}), 0);
+	IF dword * {11} # {} THEN KernelLog.String(" [IN]"); ELSE KernelLog.String(" [OUT]"); END;
+	dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + 2CH));
+	KernelLog.String(", MULT: "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {0..1}), 0);
+	KernelLog.Ln;
+	FOR i := 0 TO 7 DO
+		Indent(4);
+		dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + (i+1) * 4));
+		KernelLog.String("Transaction "); KernelLog.Int(i, 0); KernelLog.String(": ");
+		KernelLog.String("Status: ");
+		IF dword * ItdStatus # {} THEN
+			IF dword * ItdActive # {} THEN KernelLog.String("[ACTIVE]"); END;
+			IF dword * ItdDataBufferError # {} THEN KernelLog.String("[DataBufferError]"); END;
+			IF dword * ItdBabbleDetected # {} THEN KernelLog.String("[Babble]"); END;
+			IF dword * ItdTransactionError # {} THEN KernelLog.String("[TransactionError]"); END;
+		ELSE
+			KernelLog.String("[Done]");
+		END;
+
+		KernelLog.String(" Length: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * {16..27}, -16)), 0);
+		KernelLog.String(", PG: "); KernelLog.Int(SYSTEM.VAL(LONGINT, LSH(dword * {12..14}, -12)), 0);
+		KernelLog.String(", Offset: "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {0..11}), 0);
+		IF dword * {15} # {} THEN KernelLog.String(" [IOC]"); END;
+		KernelLog.Ln;
+	END;
+	FOR i := 0 TO 6 DO
+		Indent(4);
+		dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + 24H + i*4));
+		KernelLog.String("Buffer Pointer Page "); KernelLog.Int(i, 0); KernelLog.String(": ");
+		KernelLog.Hex(SYSTEM.VAL(LONGINT, dword * {12..31}), 8);
+		KernelLog.Ln;
+	END;
+	END;
+END ShowItd;
+*)
+
+PROCEDURE ShowInterrupts * (s : SET);
+BEGIN
+	IF Debug.Trace THEN
+ 	IF s * StsAsyncAdvance # {} THEN KernelLog.String("[AsyncAdvance]"); END;
+	IF s * StsHostSystemError # {} THEN KernelLog.String("[HostSystemError]"); END;
+	IF s * StsFrameListRollover # {} THEN KernelLog.String("[FrameListRollover]"); END;
+	IF s * StsPortChange # {} THEN KernelLog.String("[PortChange]"); END;
+	IF s * StsUsbError # {} THEN KernelLog.String("[UsbError]"); END;
+	IF s * StsUsbInterrupt # {} THEN KernelLog.String("[UsbInterrupt]"); END;
+	END;
+END ShowInterrupts;
+
+PROCEDURE ShowItd (adr: ADDRESS; space: LONGINT);
+VAR
+	dword: SET;
+	i: LONGINT;
+BEGIN
+	IF Debug.Trace THEN
+		Indent(space);
+		IF adr = 0 THEN
+			KernelLog.String("UsbEhci: iTD at address 0, invalid address, aborting");
+			KernelLog.Ln;
+			RETURN
+		END;
+		KernelLog.String("UsbEhci: iTD at address "); KernelLog.Address(adr); KernelLog.Ln;
+
+		dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + ItdNextLinkPointer));
+		Indent(space + 4);
+		KernelLog.String("Next Link Pointer: "); KernelLog.Address(SYSTEM.VAL(ADDRESS, dword));
+		IF 0 IN dword THEN KernelLog.String(" [INVALID]") END;
+		KernelLog.String(", Type: ");
+		CASE SYSTEM.VAL(LONGINT, dword * {1 .. 2}) DIV 2 OF
+			 0: KernelLog.String("iTD")
+			|1: KernelLog.String("QH")
+			|2: KernelLog.String("siTD")
+			|3: KernelLog.String("FSTN")
+		END;
+		KernelLog.Ln;
+
+		FOR i := 0 TO 7 DO
+			Indent(space + 4);
+			KernelLog.String("Transaction "); KernelLog.Int(i, 0); KernelLog.String(": ");
+
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + ItdTransaction0 + 4 * i));
+			KernelLog.String("TX Length "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {16 .. 27}) DIV 10000H, 0);
+			KernelLog.String(", Page select "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {12 .. 14}) DIV	1000H, 0);
+			KernelLog.String(", Tx offset "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {0 .. 11}), 0);
+			IF 15 IN dword THEN KernelLog.String(" [IOC]") END;
+
+			IF 30 IN dword THEN KernelLog.String(" [DATA BUFFER ERROR]") END;
+			IF 29 IN dword THEN KernelLog.String(" [BABBLE]") END;
+			IF 28 IN dword THEN KernelLog.String(" [ERROR]") END;
+			IF 31 IN dword THEN KernelLog.String(" [ACTIVE]") ELSE KernelLog.String(" [INACTIVE]") END;
+			KernelLog.Ln
+		END;
+
+		Indent(space + 4);
+		KernelLog.String("Buffer pointer 0: ");
+		dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + ItdBufferPtr0));
+		KernelLog.String(" address "); KernelLog.Address(SYSTEM.VAL(ADDRESS, dword * {12 .. 31}));
+		KernelLog.String(", endpoint "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {8 .. 11}) DIV 100H, 0);
+		KernelLog.String(", dev address "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {0 .. 6}), 0);
+		KernelLog.Ln;
+
+		Indent(space + 4);
+		KernelLog.String("Buffer pointer 1: ");
+		dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + ItdBufferPtr1));
+		KernelLog.String(" address "); KernelLog.Address(SYSTEM.VAL(ADDRESS, dword * {12 .. 31}));
+		KernelLog.String(", max packet size "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {0 .. 10}), 0);
+		IF 11 IN dword THEN KernelLog.String(" [IN]") ELSE KernelLog.String(" [OUT]") END;
+		KernelLog.Ln;
+
+		Indent(space + 4);
+		KernelLog.String("Buffer pointer 2: ");
+		dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + ItdBufferPtr2));
+		KernelLog.String(" address "); KernelLog.Address(SYSTEM.VAL(ADDRESS, dword * {12 .. 31}));
+		KernelLog.String(", multi "); KernelLog.Int(SYSTEM.VAL(LONGINT, dword * {0 .. 1}), 0); KernelLog.String("x");
+		KernelLog.Ln;
+
+		FOR i := 3 TO 6 DO
+			Indent(space + 4);
+			KernelLog.String("Buffer pointer "); KernelLog.Int(i, 0); KernelLog.String(": ");
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(adr + ItdBufferPtr0 + 4 * i));
+			KernelLog.String(" address "); KernelLog.Address(SYSTEM.VAL(ADDRESS, dword * {12 .. 31}));
+			KernelLog.Ln
+		END
+	END
+END ShowItd;
+
+(** Prints the Framelist and its content. 'framelist' is the base address of the framelist, 'size' is the number of elements in the framelist. If 'expand's is TRUE, ITDs and QHs are also shown. *)
+PROCEDURE ShowFramelist (framelist: ADDRESS; size, hc: LONGINT; expand: BOOLEAN);
+VAR
+	i, entry: LONGINT;
+BEGIN
+	FOR i := 0 TO size - 1 DO
+		entry := SYSTEM.GET32(framelist + 4 * i);
+		KernelLog.String("Frame entry "); KernelLog.Int(i, 4);
+		KernelLog.String(" at "); KernelLog.Address(framelist + 4 * i);
+		KernelLog.String(":	"); KernelLog.Hex(entry, -8);
+		IF (hc # -1) & (i = hc) THEN KernelLog.String(" <- HC") END;
+		KernelLog.Ln;
+		IF expand & ~ODD(entry) THEN
+			ShowItd(entry, 4)
+		END
+	END;
+END ShowFramelist;
+
+(**
+ * Helper procedure to build a QH horizontal link field.
+ * Creates a horizontal link pointer field pointing to 'qh', with type 'type'.
+ * Link is marked as invalid if terminate is TRUE.
+ *)
+PROCEDURE QhHorizontalLink (qh: ADDRESS; type: LONGINT; terminate: BOOLEAN): LONGINT;
+VAR
+	dword: SET;
+BEGIN
+	IF terminate THEN
+		dword := QhTerminate
+	END;
+	dword := dword + SYSTEM.VAL(SET, qh) + SYSTEM.VAL(SET, LSH(type, 1));
+	RETURN SYSTEM.VAL(LONGINT, dword)
+END QhHorizontalLink;
+
+(* Called when this module is unloaded. Unregister all EnhancedHostController objects *)
+PROCEDURE Cleanup;
+BEGIN
+	UsbHcdi.UnRegisterHostControllers(Description);
+END Cleanup;
+
+BEGIN
+	Modules.InstallTermHandler(Cleanup);
+END UsbEhci.
+
+UsbEhci.Install ~  SystemTools.Free UsbEhci ~

source/ARM.UsbHcdi.Mod

@@ -0,0 +1,1582 @@
+MODULE UsbHcdi; (** AUTHOR "staubesv"; PURPOSE "USB Host Controller Driver Interface (HCDI)" *)
+(**
+ * Bluebottle USB Host Controller Driver Interface
+ *
+ * This is the hardware abstraction layer which enables the USB Bus Driver / Hub Driver to handle all USB Host Controller
+ * in a unique way.
+ *
+ * Overview:
+ *
+ *	HcdInterface	Interface of the hardware-specific operations to be implemented by specific USB Host Controller drivers
+ *	Hcd(Interface)	Implementation of hardware-independent functionality common to all USB Host Controllers
+ *	TransferToken	Generic representation of a USB data transfer. Used as interface between pipes and host controller drivers
+ *	Pipe			Implementation of logical communication channel between client software (i.e. device driver) and a device
+ *					endpoint.
+ *
+ * USB host controllers are controlled by operational registers and the host controller communication area in system memory.
+ *
+ * In the general design of the USB stack, device drivers use Pipe objects to communicate with a device. The Pipe in turn calls the
+ * host controller driver to schedule the USB transfer. This module implements the interface layer between a Pipe object and an
+ * HCD (host controller driver) object. The main concept of this interface is the TransferToken record, which generically represents
+ * transfers.
+ *
+ * For USB communication, a HCD presents 3 procedures:
+ *		- Schedule				execute a transfer
+ *		- Cancel				cancel a pending transfer
+ *		- UpdatePipeStatus	update status for all transfers of a pipe
+ *
+ * 3 more procedures are used for bookkeeping related to transfers:
+ *		- RegisterPipe			register a pipe to the host controller. A pipe cannot transfer data before being registered
+ *		- UnregisterPipe		unregister a pipe from the host controller. A pipe cannot transfer data after being deregistered
+ *		- CheckPipePolicy		validates the policy of an isochronous pipe. An isochronous pipe cannot transfer data before its policy being validated
+ *
+ * History:
+ *
+ *	24.11.2005	History started (staubesv)
+ *	29.11.2005	Introduced Notifier object and moved corresponding code from HC drivers to Hcd (staubesv)
+ *	12.12.2005	More options for HcdManager.Show, use exception handling for critical calls (staubesv)
+ *	13.12.2005	Fixed critical bug in RemoveQH, some cleanup (staubesv)
+ *	15.12.2005	Added pipe.CheckBuffer, removed Pipe.pid field (staubesv)
+ *	06.01.2006	Check whether USB device has been disconnected in Pipe transfer routines (staubesv)
+ *	09.01.2006	Added TraceControlData trace option (staubesv)
+ *	11.01.2006	Removed Hcd.GetPortCount, Hcd.RestartPipe, Pipe.Restart (staubesv)
+ *	12.01.2006	Implemented software scatter/gather mechanism for pipes (staubesv)
+ *	12.01.2006	Implemented transfer completion notification via interrupts (staubesv)
+ *	16.01.2006	Bugfix: Pipe.Clearhalt request must use endpoint address instead of endpoint number (staubesv)
+ *	17.01.2006	Use less memory for the TD buffer of the default pipe (staubesv)
+ *	24.01.2006	Removed offset parameter for control transfer buffer (staubesv)
+ *	25.01.2006	Unified Pipe.ControlTranfer & Pipe.Transfer, introduced TransferCompletion object (staubesv)
+ *	26.01.2006	Don't return failure codes in Hcd.GetPipe, just NIL if allocation fails (staubesv)
+ *	01.02.2006	Removed HcdManager, use Plugins mechanis instead (staubesv)
+ *	01.03.2006	Fixed broken control out transfers (staubesv)
+ *	28.06.2006	Removed procedure PrintHex (use KernelLog.Hex instead),
+ *				fixed UnRegisterHostControllers so it only removes the Controllers with the provided description (staubesv)
+ *	03.06.2006	HcdInterface.UpdatePipe removed (staubesv)
+ *	03.08.2006	Introduced HcdInterface.LinkTDsAllowed procedure (staubesv)
+ *	05.01.2007	Separatly trace shortpackets/other errors, each pipe has now a own thread that's used to call its completion handler (staubesv)
+ *	XX.04.2015	Introduced TransferToken as the interface between Pipe and HCD (tmartiel)
+ *	XX.06.2015	Moved all scatter-gather management to actual HCD implementations (tmartiel)
+ * TODOs:
+ *	- nicer design of pipes (message pipe / stream pipe)
+ *	- move pipes in separate module?
+ *	- Should control transfers also use IOC completion? No.
+ * 	- Timeout handling: Fix: Transfer could still complete when a timeout occurs
+ *)
+
+IMPORT SYSTEM, KernelLog, Machine, Plugins, Modules, Kernel, Objects, Locks, UsbBuffers, Usbdi, Debug := UsbDebug;
+
+CONST
+
+	(** Bluebottle specific hub port status bits. Not all (root) hubs do support all of the possible status bits *)
+	PortStatusDevicePresent* = {0}; 			(** USB device is attached to the port (only visible when port is powered) *)
+	PortStatusEnabled* = {1}; 				(** The port is enabled *)
+	PortStatusLowSpeed* = {2}; 				(** Connected device is a low-speed device *)
+	PortStatusFullSpeed* = {3}; 				(** Connected device is a full-speed device *)
+	PortStatusHighSpeed* = {4}; 			(** Connected device is a high-speed device *)
+	PortStatusReset* = {5}; 					(** The port is current resetting *)
+	PortStatusError* = {6}; 					(** The status request failed. *)
+	PortStatusConnectChange* = {7};		(** The connection status of the port has changed *)
+	PortStatusSuspended* = {8}; 			(** The port is suspended *)
+	PortStatusOverCurrent* = {9}; 			(** There is an overcurrent condition on this port *)
+	PortStatusPowered* = {10}; 				(** The port is powered *)
+	PortStatusEnabledChange* = {11}; 		(** The enabled/disabled status has changed *)
+	PortStatusSuspendChange* = {12}; 		(** The suspended status has changed *)
+	PortStatusOverCurrentChange* = {13}; 	(** The overcurrent status has changed *)
+	PortStatusResetChange* = {14}; 			(** The reset status has changed *)
+	PortStatusWakeOnOvercurrent* = {15}; 	(** Overcurrent conditions are a wake-up event *)
+	PortStatusWakeOnDisconnect* = {16}; 	(** Device disconnection is a wake-up event *)
+	PortStatusWakeOnConnect* = {17}; 		(** Device connection is a wake-up event *)
+	PortStatusTestControl* = {18}; 			(** The port is in test mode *)
+	PortStatusIndicatorControl* = {19}; 		(** The port supports control of its status indicator LEDs *)
+	PortStatusPortOwner* = {20}; 			(** The port is owned by the high-speed controller (only EHCI HCs) *)
+
+	(** Pipe.errors coding. Gives more detailed transfer completion status than Pipe.status *)
+	NoErrors* = {};							(** No errors occured *)
+	ShortPacket* = {1};						(** Device transferred less data than requested *)
+	Stalled* = {2};							(** Stall condition *)
+	InProgress* = {3};						(** Transfer is still in progress (timeout for blocking transfers) *)
+	Nak* = {4};								(** [UHCI] Device NAKed transfer *)
+	Crc* = {5};								(** [OHCI] CRC error *)
+	Timeout* = {6};							(** [OHCI] USB protocol timeout *)
+	CrcTimeout* = {7};						(** [UHCI] CRC or Timeout error *)
+	BitStuff* = {8};							(** Bitstuffing error *)
+	Databuffer* = {9};						(** Databuffer error *)
+	Babble* = {10};							(** Babble: Device sent more data than requested -> serious error! *)
+	UnexpectedPid* = {13};					(** [OHCI] Unexpected PID *)
+	PidCheckFailure* = {15};					(** [OHCI] PID check failure *)
+	DataToggleMismatch* = {16};			(** [OHCI] Datatoggle mismatch *)
+	DeviceNotResponding* = {17};			(** [OHCI] Device did not respond *)
+	(** Pipe.errors HCDI level errors *)
+	LinkTDsFailed* = {18};					(** Could not link TDs to QH since there are already linked TDs *)
+	OutOfTDs* = {19};						(** No more TDs available for transfer scheduling *)
+	Internal* = {11};							(** HCDI level error *)
+	TransferTooLarge* = {14};				(** HCDI level error: Transfer is too large, respectively SG list is too small *)
+	Disconnected* = {12};					(** Device has been disconnected *)
+
+	(** Coding of ORD(PipePolicy.type) field *)
+	PipeControl* =  0;
+	PipeIsochronous* = 1;
+	PipeBulk* = 2;
+	PipeInterrupt* = 3;
+
+	(* Host controller states *)
+	Undefined* = 0;
+	Initialized* = 1;
+	Operational* = 2;
+	Suspended* = 3;
+	Resuming* = 4;
+	Halted* = 5;
+	Shutdown* = 6;
+
+	(* USB transfer modes *)
+	LowSpeed* = 0;
+	FullSpeed* = 1;
+	HighSpeed* = 2;
+
+	(** Direction field for control transfers / pipes *)
+	In* = 0;		(* Device-to-Host *)
+	Out* = 1;	(* Host-to-Device *)
+
+	(** HcCapabilities powerSwitching, overCurrentDetection field values *)
+	NotAvailable* = 0; (* MUST be 0 *)
+	Global* = 1; (* MUST be 1 *)
+	PerPort* = 2; (* MUST be 2 *)
+
+	(* Port indicator colors. Don't change values! *)
+	Automatic* = 0;
+	Amber* = 1;
+	Green* = 2;
+	Off* = 3;
+
+	(** Timing [milliseconds] *)
+	PortResetTime* = 10 + 20; (* "The duration of the Resetting state is nominally 10 ms to 20 ms (10 ms is preferred)", USB2.0spec Chapter 11.5.1.5 *)
+
+	(** After a port reset, the hub should enable the port within this time *)
+	PortEnableTimeout* = 20;  (* ms *)
+
+	 (* Wait for at least 100ms to allow completion of insertion process and for power at the device to become stable. USB 2.0spec Chapter 9.1.2 *)
+	PortInsertionTime* = 100 + 50;
+
+	(* SetAddress recovery interval of 2ms, USB2.0spec Chapter 9.2.6.3 *)
+	AddressRecoveryTime* = 2 + 10;
+
+	(* Minimum time the root hub must assert the reset signal on its downstream ports *)
+	RootHubResetTime* = 100; (* >= 50 ms, USB2.0spec, p. 282  *)
+
+	(* Minimum time the HC must stay in suspend state once entered *)
+	MinSuspendTime* = 8; (* >= 5ms, OHCI specification p. 44 *)
+
+	StateDisconnected* = -1; (* MUST be equal to Usb.StateDisconnected *)
+
+	(** Max number of TDs per transfer *)
+	MaxTDs* = 1024;
+
+TYPE
+
+	(** Aligned memory space. Use data[base] as first entry. *)
+	AlignedMemSpace* = POINTER TO RECORD
+		data- : POINTER TO ARRAY OF LONGINT;
+		base- : ADDRESS;
+	END;
+
+	(** Emulated hub descriptor for supporting root hub emulation *)
+	HubDescriptor* = POINTER TO ARRAY OF CHAR;
+
+	(* Used for reporting root hub status and root hub port status changes *)
+	StatusChangeHandler* = PROCEDURE {DELEGATE} (status : Usbdi.Status; actLen : LONGINT);
+
+	(* Packet for message pipe transfers *)
+	ControlMessage* = (*RECORD ofs*: LONGINT; value*: POINTER TO ARRAY 8 + 32 OF CHAR; END;*) Usbdi.BufferPtr;
+
+TYPE
+
+	(* The HcdInterface defines the interface that must be implemented by the Host Controller Drivers *)
+	HcdInterface = OBJECT(Plugins.Plugin)
+
+		(** Root Hub Control *)
+
+		(* Port numbers: 0.. nbrOfPorts-1 *)
+
+		(**
+		 * Enable power for the specified port.
+		 * It is the hub driver that is responsible for waiting the PwrOn2PwrGood time after enabling a port.
+		 * The actual result is depending on the kind of power switching the root hub implements:
+		 * - Per port: Enable power for the specified port
+		 * - Ganged: One call will enable power for all ports, independent on the specified port number
+		 * - None: The hub driver will not call this procedure
+		 * @param port Port to enable power for
+		 *)
+		PROCEDURE EnablePortPower*(port : LONGINT);
+		BEGIN HALT(301); END EnablePortPower;
+
+		(**
+		 * Disable power for the specified port.
+		 * The actual result is depending on the kind of power switching the root hub implements:
+		 * - Per port: Disable power for the specified port
+		 * - Ganged: One call will disable power for all ports, independent on the specified port number
+		 * - None: The hub driver will not call this procedure
+		 * @param port Port to disable power for
+		 *)
+		PROCEDURE DisablePortPower*(port : LONGINT);
+		BEGIN HALT(301); END DisablePortPower;
+
+		(**	Reset and then enable the specified port.
+			Resets the port, waits until reset is complete, enables the port and then returns. The client
+			(HubDriver) is responsible for waiting after the port is enabled *)
+
+		PROCEDURE ResetAndEnablePort*(port : LONGINT) : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END ResetAndEnablePort; (* abstract *)
+
+		(** Disable the specified port. *)
+		PROCEDURE DisablePort*(port: LONGINT);
+		BEGIN HALT(301); END DisablePort; (* abstract *)
+
+		(** Suspend the specified port *)
+		PROCEDURE SuspendPort*(port: LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbHcdi: Port suspending not supported."); KernelLog.Ln; END;
+			RETURN FALSE;
+		END SuspendPort;
+
+		(** Resume the specified port *)
+		PROCEDURE ResumePort*(port: LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.Level >= Debug.Warnings  THEN KernelLog.String("UsbHcdi: Port resuming not supported."); KernelLog.Ln; END;
+			RETURN FALSE;
+		END ResumePort;
+
+		(**
+		 * Get the status of the specified port.
+		 * @param port Port to get status from
+		 * @param ack Ackknowlegde status bits that are set
+		 * @return Port status
+		 *)
+		PROCEDURE GetPortStatus*(port : LONGINT; ack : BOOLEAN) : SET;
+		BEGIN HALT(301); RETURN {}; END GetPortStatus; (* abstract *)
+
+		(** Indicate a port state using the port indicators *)
+		PROCEDURE IndicatePort*(port, indicate : LONGINT);
+		BEGIN
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbHcdi: Port indicator control not supported."); KernelLog.Ln; END;
+		END IndicatePort;
+
+		(**
+		 * Does the controller has a companion for lower-speed devices?
+		 * Not having a companion means that lower-speed devices can be connected directly to the root hub
+		 * and won't use a transaction translator.
+		 *)
+		PROCEDURE HasCompanion*(): BOOLEAN;
+		BEGIN
+			RETURN FALSE
+		END HasCompanion;
+
+		(** Route the specified port to a companion host controller if supported. *)
+		PROCEDURE RoutePortToCompanion*(port : LONGINT);
+		BEGIN
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbHcdi: Port routing not supported."); KernelLog.Ln; END;
+		END RoutePortToCompanion;
+
+		(** Updates the status field of the USB transfer request <req> *)
+		PROCEDURE UpdatePipeStatus*(pipe : Pipe);
+		BEGIN HALT(301); END UpdatePipeStatus; (* abstract *)
+
+		(** Clear halt bit if pipe is stalled. The TDs associated to the pipe will be removed from the queue *)
+		PROCEDURE ClearHalt*(pipe : Pipe);
+		BEGIN HALT(301); END ClearHalt; (* abstract *)
+
+		(**
+		 * Schedule a transfer on this hcd.
+		 * Will block until the transfer is effectively handed to hardware.
+		 * Does not wait for transfer completion.
+		 *)
+		PROCEDURE Schedule * (transfer: TransferToken);
+		BEGIN HALT(301) (* abstract *)
+		END Schedule;
+
+		(**
+		 * Cancel scheduled transfer.
+		 * Tries to cancel a scheduled transfer. Fails if transfer is completed or active.
+		 *)
+		PROCEDURE Cancel * (transfer: TransferToken): BOOLEAN;
+		BEGIN HALT(301) (* abstract *)
+		END Cancel;
+
+		(**
+		 * Checks that a periodic pipe policy is allowed for this HCD. Returns FALSE if the policy
+		 * requires too much bandwidth, TRUE if the policy is schedulable.
+		 *)
+		PROCEDURE CheckPipePolicy * (interval, size: LONGINT): BOOLEAN;
+		BEGIN HALT(301) (* abstract *)
+		END CheckPipePolicy;
+
+		(**
+		 * Some HCDIs require some pipe creations to be notified to them. This procedure is called whenever a new
+		 * pipe is created, so that the HCDI can take the necessary actions. This is needed, e.g. for bulk and control pipes
+		 * on EHCI: we build one qh per pipe.
+		 *)
+		PROCEDURE RegisterPipe * (pipe: Pipe);
+		BEGIN HALT(301) (* abstract *)
+		END RegisterPipe;
+
+		(**
+		 * Some HCDIs require some pipe freeing to be notified to them. This procedure is called whenever a pipe is freed,
+		 * so that the HCDI can take necessary steps. This is needed e.g. for bulk and control pipes on EHCI, so that the
+		 * controller can remove the queue head.
+		 *)
+		PROCEDURE UnregisterPipe * (pipe: Pipe);
+		BEGIN HALT(301) (* abstract *)
+		END UnregisterPipe;
+
+		(** Debug interface *)
+
+		(** Show the specified queue head / endpoint descriptor *)
+		PROCEDURE ShowQH*(qh, firstTD : ADDRESS);
+		BEGIN
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("QH/TD diagnostics not implemented."); KernelLog.Ln; END;
+		END ShowQH;
+
+		(** Show the data structures associated with the specified pipe *)
+		PROCEDURE ShowPipe*(pipe : Pipe);
+		BEGIN
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Pipe diagnostics not implemented."); KernelLog.Ln; END;
+		END ShowPipe;
+
+		(** Show the host controller's scheduling data structure *)
+		PROCEDURE ShowSchedule*;
+		BEGIN
+			IF Debug.Level >= Debug.Warnings THEN KernelLog.String("Schedule diagnostics not implemented."); KernelLog.Ln; END;
+		END ShowSchedule;
+
+	END HcdInterface;
+
+	(** Implements hardware-independent functionality of USB Host Controllers *)
+	Hcd* = OBJECT(HcdInterface)
+	VAR
+		(* Host controller configuration data - Consider this fields as read-only. *)
+		iobase*: Machine.Address32;
+		irq* : LONGINT;
+		portCount* : LONGINT;
+		ports* : POINTER TO ARRAY OF Machine.Address32;  (* addresses of the Port Status Control registers  *)
+
+		(* PCI specific *)
+		bus-, device-, function- : LONGINT;
+
+		(* HC capabilities (consider read-only) *)
+		DMAchaining* : BOOLEAN; 	(* Can the HC do H/W scatter/gather? *)
+		sgListSize* : LONGINT; 		(* How many entries shall the SG list support? Only valid if DMAchaining is TRUE. *)
+		isHighSpeed* : BOOLEAN;	(* Does this host controller support high-speed transfer mode? *)
+
+		(* Emulated hub device descriptor *)
+		hubDescriptor* : HubDescriptor;
+
+		(* current state of the host controller (Undefined|Initialized|Operational|Suspended|Resuming|Error) *)
+		state- : LONGINT;
+
+		(* Procedure called when the status of the root hub port changes *)
+		statusChangeHandler- : StatusChangeHandler;
+
+		(* pipes[deviceaddress][direction][endpoint],  [0][0][0] is the dummy default control pipe 	*)
+		(* control pipes (bidirectional) are stored as they had dir In (0)							*)
+		pipes- : ARRAY 128 OF ARRAY 2 OF ARRAY 16 OF Pipe;
+
+		(* These pipes have an handler associated which should be called when the pipe's transfer is finished 		*)
+		(* The Default Pipe (Address 0, ep 0) of the controller is used as list head but has no completion handler	*)
+		notifyPipes- : Pipe;
+
+		(* Keeps track which UDB device addresses are in use. *)
+		adrRange : ARRAY 128 OF BOOLEAN;
+
+
+		(* Per USB lock *)
+		buslock : LONGINT;
+
+		(* For the Wait procedure *)
+		timer : Kernel.Timer;
+
+		(* Performance monitoring *)
+		NbytesTransfered- : HUGEINT;
+
+		(* HC statistics *)
+		NnofTransfers-,
+		NnofBulkTransfers-, NnofControlTransfers-, NnofInterruptTransfers-, NnofIsochronousTransfers-,
+		NnofUnknownTransfers-,
+		NnofInterrupts*, NnofInterruptsHandled* : LONGINT;
+
+		(** Bus Locking *)
+
+		(** Lock this bus *)
+		PROCEDURE Acquire*;
+		BEGIN {EXCLUSIVE}
+			AWAIT(buslock <= 0); buslock := 1;
+		END Acquire;
+
+		(** Unlock this bus *)
+		PROCEDURE Release*;
+		BEGIN {EXCLUSIVE}
+			DEC(buslock);
+		END Release;
+
+		(** Set the state of the HC *)
+		PROCEDURE SetState*(state : LONGINT);
+		BEGIN {EXCLUSIVE}
+			SELF.state := state;
+		END SetState;
+
+		(** Root hub control *)
+
+		(**
+		 * Return the emulated hub device descriptor.
+		 * To be controllable by the hub driver, root hubs emulate a USB hub devices. This procedure returns
+		 * the emulated hub descriptor to communicate the root hubs facilities.
+		 *)
+		PROCEDURE GetHubDescriptor*() : HubDescriptor;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT(hubDescriptor # NIL); END;
+			RETURN hubDescriptor;
+		END GetHubDescriptor;
+
+		(** Set the emulated hub device descriptor. *)
+		 PROCEDURE SetHubDescriptor*(hd : HubDescriptor);
+		 BEGIN
+		 	IF Debug.StrongChecks THEN ASSERT((hd # NIL) & (LEN(hd) >= 8)); END;
+		 	hubDescriptor := hd;
+		 END SetHubDescriptor;
+
+		(**
+		 * Install a handler for root hub port status changes.
+		 * Some host controller can report changes of the port status registers via interrupts. If this is not
+		 * supported, the hub driver must poll the root hub for port status changes.
+		 * @param handler to be called when root hub port status changes occus
+		 * @return TRUE, if root hub support status change notifications, FALSE otherwise.
+		 *)
+		PROCEDURE SetStatusChangeHandler*(handler : StatusChangeHandler) : BOOLEAN;
+		BEGIN
+			statusChangeHandler := handler; RETURN TRUE;
+		END SetStatusChangeHandler;
+
+		(** Add an interrupt handler. The handler is called, when a IOC interrupt occurs and the pipe.status field is changed	*)
+		PROCEDURE AddCompletionHandler*(pipe : Pipe);
+		VAR temp : Pipe;
+		BEGIN {EXCLUSIVE}
+			IF Debug.Trace & Debug.tracePipes THEN
+				KernelLog.String("UsbHcdi: Adding completion handler for pipe (Adr: "); KernelLog.Int(pipe.address, 0);
+				KernelLog.String(", ep: "); KernelLog.Int(pipe.endpoint, 0); KernelLog.String(")"); KernelLog.Ln;
+			END;
+			temp := notifyPipes;
+			WHILE(temp.next # NIL) & (temp.next # pipe) DO temp := temp.next; END;
+			IF temp.next = NIL THEN
+				temp.next := pipe;
+			ELSIF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbHcdi: Warning: Procedure was already registered as interrupt handler"); KernelLog.Ln;
+			END;
+		END AddCompletionHandler;
+
+		(** Remove an interrupt handler. The head of this list is always the control pipe for the default address. *)
+		PROCEDURE RemoveCompletionHandler*(pipe : Pipe);
+		VAR temp : Pipe;
+		BEGIN (* only to be called from exclusive regions !! *)
+			IF Debug.Trace & Debug.tracePipes & (ADDRESSOF(pipe) > 10H)  THEN
+				KernelLog.String("UsbHcdi: Removing completion handler for pipe (Adr: "); KernelLog.Int(pipe.address, 0);
+				KernelLog.String(", ep: "); KernelLog.Int(pipe.endpoint, 0); KernelLog.String(")"); KernelLog.Ln;
+			END;
+			(* Never remove the default address control pipe *)
+			(*pipe.transferLock.AcquireWrite;*)
+			pipe.irqActive := FALSE;
+			(*pipe.transferLock.ReleaseWrite;*)
+			temp := notifyPipes;
+			WHILE (temp.next # NIL) & (temp.next # pipe) DO temp := temp.next; END;
+			IF temp.next # NIL THEN (* delete pipe in list *)
+				 temp.next := temp.next.next;
+			ELSIF Debug.Level >= Debug.Warnings THEN
+				KernelLog.String("UsbHcdi: Warning: Could not remove interrupt handler (not found)"); KernelLog.Ln;
+			END;
+		END RemoveCompletionHandler;
+
+		PROCEDURE NotifyCompletionHandlers *;
+		VAR
+			pipe: Pipe;
+			transfer: TransferToken;
+			status, actLen: LONGINT;
+			irqActive: BOOLEAN;
+		BEGIN (* concurrent insertion/removal of completion handlers is allowed *)
+			pipe := notifyPipes.next; (* notifyPipes is the Default Pipe (address 0, ep 0), skip it *)
+			WHILE pipe # NIL DO
+				pipe.lock.Acquire;
+				irqActive := pipe.irqActive;
+				IF irqActive & (pipe.mode = Usbdi.MinCpu) THEN
+					UpdatePipeStatus(pipe);
+					(* For all transfers, if finished, call completion notifier *)
+					transfer := pipe.transfers;
+					WHILE transfer # NIL DO
+						IF (transfer.status # Usbdi.InProgress) & (pipe.device.state # StateDisconnected) THEN
+							IF Debug.Trace & Debug.traceIoc THEN
+								KernelLog.String("UsbHcdi: Notify pipe adr: "); KernelLog.Int(pipe.address, 0);
+								KernelLog.String(", ep: "); KernelLog.Int(pipe.endpoint, 0); KernelLog.String(": "); ShowErrors(pipe.errors); KernelLog.Ln;
+							END;
+							IF Debug.Trace & (transfer.status # Usbdi.Ok) THEN
+								IF (transfer.status = Usbdi.ShortPacket) THEN
+									IF Debug.traceShortPackets THEN pipe.Show(TRUE); KernelLog.Ln; END;
+								ELSE
+									IF Debug.traceFailed THEN pipe.Show(TRUE); KernelLog.Ln; END;
+								END;
+							END;
+							IF Debug.PerformanceMonitoring THEN
+								IF (transfer.status = Usbdi.Ok) OR (transfer.status = Usbdi.ShortPacket) THEN
+									(* should be protected *)
+									INC(NbytesTransfered, pipe.actLen);
+								END;
+							END;
+							status := transfer.status;
+							actLen := transfer.transfered;
+							pipe.CleanupTransfer(transfer);
+							IF pipe.mode = Usbdi.MinCpu THEN pipe.completion.SetDone END;
+							IF pipe.completionHandlerCaller # NIL THEN pipe.completionHandlerCaller.Call(status, actLen); END;
+							transfer := pipe.transfers;
+						ELSE
+							transfer := transfer.next;
+						END;
+					END;
+					IF pipe.transfers = NIL THEN
+						(* No transfers scheduled: deactivate irq notifications *)
+						pipe.irqActive := FALSE
+					END;
+				END;
+				pipe.lock.Release;
+				pipe := pipe.next;
+			END;
+		END NotifyCompletionHandlers;
+
+		(* The default pipe (adr 0, ep 0) is only used to communicate with devices as long they are not in the addressed state. *)
+		PROCEDURE GetDefaultPipe*(speed, ttPort, ttAddress : LONGINT; device : Usbdi.UsbDevice) : Pipe;
+		VAR pipe : Pipe;
+		BEGIN {EXCLUSIVE}
+			IF Debug.StrongChecks THEN ASSERT(pipes[0, 0, 0] # NIL); END; (* dummy default pipe is always present *)
+			pipe := pipes[0, 0, 0];
+			pipe.speed := speed;
+			pipe.device := device;
+			pipe.completion.device := device;
+			pipe.ttPort := ttPort;
+			pipe.ttAddress := ttAddress;
+			IF (isHighSpeed) & (speed = HighSpeed) THEN
+				pipe.maxPacketSize := 64;
+			ELSE
+				pipe.maxPacketSize := 8;
+			END;
+			(*IF ~InsertQH(pipe) THEN pipe := NIL; END;*)
+			RegisterPipe(pipe);
+			RETURN pipe;
+		END GetDefaultPipe;
+
+		(** USB Pipe Handling *)
+
+		PROCEDURE GetPipe*(deviceAddress, endpointNbr : LONGINT; VAR pipe : Pipe);
+		BEGIN {EXCLUSIVE}
+			IF Debug.StrongChecks THEN
+				ASSERT((deviceAddress > 0) & (deviceAddress < 128));  (* valid USB device address *)
+				ASSERT((endpointNbr MOD 16 >=  0) & (endpointNbr MOD 16 < 16)); (* valid endpoint number *)
+				ASSERT((pipe # NIL) & (pipe.direction = In) OR (pipe.direction = Out));
+			END;
+			IF pipes[deviceAddress, pipe.direction, endpointNbr MOD 16] # NIL THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbHcdi: GetPipe: Pipe already in use."); KernelLog.Ln; END;
+				pipe := NIL; RETURN;
+			END;
+			(* Allocate the TD buffers  *)
+			(*pipe.tdBufferLen := (TDsPerPipe+1)* 16 + 128;
+			NEW(pipe.tdBuffer, pipe.tdBufferLen);*)
+			IF Debug.StrongChecks THEN
+				(*testaddr := Machine.PhysicalAdr(ADDRESSOF(pipe.tdBuffer[0]), pipe.tdBufferLen);
+				IF testaddr = Machine.NilAdr THEN
+					KernelLog.String("UsbHcdi: GetPipe: Allocated buffer not physically contiguous"); KernelLog.Ln;
+					pipe := NIL; RETURN;
+				END;*)
+			END;
+			(* TD's must be 32byte aligned for EHCI, 16byte aligned for UHCI & OHCI data structures*)
+			(* currently max qh size is below 128 bytes for EHCI, so a queue head will not cross page boundaries *)
+			(*pipe.qh := Align(ADDRESS OF pipe.tdBuffer[0], 128);
+			pipe.tdBase := pipe.qh + 32;
+			ASSERT((pipe.qh >= ADDRESSOF(pipe.tdBuffer[0])) & (pipe.tdBase <= ADDRESSOF(pipe.tdBuffer[pipe.tdBufferLen-1])));*)
+			RegisterPipe(pipe);
+			(*IF ~InsertQH(pipe) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbHcdi: GetPipe: InsertQH failed."); KernelLog.Ln; END;
+				pipe := NIL; RETURN;
+			END;*)
+			pipes[deviceAddress, pipe.direction, endpointNbr MOD 16] := pipe;
+			IF Debug.Trace & Debug.tracePipes THEN
+				KernelLog.String("UsbHcdi: GetPipe: ");
+				CASE pipe.type OF
+					|PipeControl: KernelLog.String("Control");
+					|PipeBulk: KernelLog.String("Bulk");
+					|PipeInterrupt: KernelLog.String("Interrupt");
+					|PipeIsochronous: KernelLog.String("Isochronous");
+				ELSE
+					KernelLog.String("Unknown");
+				END;
+				IF pipe.direction = In THEN KernelLog.String(" IN Pipe, ");
+				ELSIF pipe.direction = Out THEN KernelLog.String(" OUT Pipe, ");
+				ELSE KernelLog.String("Unknown direction Pipe, ");
+				END;
+				KernelLog.String(" Adr: "); KernelLog.Int(deviceAddress, 0);
+				KernelLog.String(" Ep: "); KernelLog.Int(endpointNbr, 0);
+				KernelLog.String(" established"); KernelLog.Ln;
+			END;
+		END GetPipe;
+
+		PROCEDURE FreePipe*(pipe : Pipe);
+		BEGIN {EXCLUSIVE}
+			FreePipeInternal(pipe);
+		END FreePipe;
+
+		PROCEDURE FreePipeInternal(pipe : Pipe);
+		BEGIN (* only call from exclusive regions *)
+			(*TRACE('FreePipeInternal');*)
+			IF pipe.address = 0 THEN
+				(*IF pipe.qh # 0 THEN RemoveQH(pipe); END;*)
+				UnregisterPipe(pipe);
+				IF Debug.Trace & Debug.tracePipes THEN KernelLog.String("UsbHcdi: Default pipe at adr 0 freed up."); KernelLog.Ln; END;
+			ELSIF pipes[pipe.address, pipe.direction, pipe.endpoint MOD 16] # NIL THEN
+				(* De-install interrupt handler if present *)
+				IF pipe.ioc THEN RemoveCompletionHandler(pipe); END;
+				(* Kill completion handler caller if present *)
+				IF pipe.completionHandlerCaller # NIL THEN pipe.completionHandlerCaller.Terminate; END;
+				(* Remove the pipe's queue head from the host controller's schedule *)
+				(*IF pipe.qh # 0 THEN RemoveQH(pipe); END;*)
+				UnregisterPipe(pipe);
+				(* Never free the dummy default pipe *)
+				pipes[pipe.address, pipe.direction, pipe.endpoint MOD 16] := NIL;
+				IF Debug.Trace & Debug.tracePipes THEN
+					KernelLog.String("UsbHcdi: FreePipe:"); KernelLog.String(" Adr: "); KernelLog.Int(pipe.address, 0);
+					KernelLog.String(" Ep: "); KernelLog.Int(pipe.endpoint, 0); KernelLog.String(" freed up."); KernelLog.Ln;
+				END;
+			ELSE (* Pipe not known by host controller *)
+				IF Debug.Level >= Debug.Warnings THEN
+					KernelLog.String("UsbHcdi: FreePipe: Can't free pipe... pipe not known by host controller ADR: ");
+					IF pipe # NIL THEN
+						KernelLog.Int(pipe.address, 0); KernelLog.String(", ep: "); KernelLog.Int(pipe.endpoint, 0);
+						KernelLog.String(", dir: "); KernelLog.Int(pipe.direction, 0);
+					ELSE
+						KernelLog.String("NIL");
+					END;
+					KernelLog.Ln;
+				END;
+			END;
+			(*TRACE('END FreePipeInternal');*)
+		END FreePipeInternal;
+
+		(** Free all pipes of the device with the address adr *)
+		PROCEDURE FreeAll*(adr : LONGINT);
+		VAR i, j : LONGINT;
+		BEGIN {EXCLUSIVE}
+			IF Debug.StrongChecks THEN ASSERT((adr >= 0) & (adr < 128)); END;
+			IF adr = 0 THEN RETURN (* Emulated hub device has no pipes *) END;
+			IF state = Shutdown THEN RETURN; END; (* Since the controller has been resetted, removing QHs would trap *)
+			FOR i := 0 TO 15 DO
+				FOR j := 0 TO 1 DO
+					IF pipes[adr, j, i] # NIL THEN
+						FreePipeInternal(pipes[adr, j , i]);
+					END;
+				END;
+			END;
+		END FreeAll;
+
+		(** Returns a unused address and marks it as used; address 0 is the default address, to
+		 * which unaddressed USB devices at enabled endpoints will respond.  *)
+		PROCEDURE GetFreeAddress*() : LONGINT;
+		VAR adr : LONGINT;
+		BEGIN {EXCLUSIVE}
+			FOR adr := 1 TO 127 DO
+				IF adrRange[adr] = FALSE THEN adrRange[adr] := TRUE; RETURN adr; END;
+			END;
+			RETURN 0;
+		END GetFreeAddress;
+
+		(** Marks the address <adr> as free *)
+		PROCEDURE FreeAddress*(adr : LONGINT);
+		BEGIN {EXCLUSIVE}
+			adrRange[adr] := FALSE;
+		END FreeAddress;
+
+		(** Helper: Wait for the specified number of milliseconds *)
+		PROCEDURE Wait*(ms : LONGINT);
+		BEGIN
+			timer.Sleep(ms);
+		END Wait;
+
+		PROCEDURE Cleanup*;
+		BEGIN
+			SetState(Shutdown); timer.Wakeup;
+		END Cleanup;
+
+		PROCEDURE &Default*(bus, device, function : LONGINT);
+		VAR pipe : Pipe; i : LONGINT;
+		BEGIN
+			SELF.bus := bus; SELF.device := device; SELF.function := function;
+			NEW(timer);
+			FOR i := 0 TO 127 DO adrRange[i] := FALSE; END;
+			NEW(pipe, 0, 0, SELF);
+			pipe.type := PipeControl; pipe.direction := 0;
+			(*pipe.maxPacketSize := 8; (* will be reset to 64 for EHCI HCs by hub driver *) (*! Should maybe be set in specific HCDIs *)*)
+			pipe.maxRetries := 3; pipe.timeout := 5000;
+			(* okay, now we allocate the TD buffers  *)
+			(*pipe.tdBufferLen := (TDsDefaultPipe+1) * 16 + 128; (* max alignment of 128 bytes *)
+			NEW(pipe.tdBuffer, pipe.tdBufferLen);*)
+			(* TD's must be 32byte aligned for EHCI data structures, 16byte for UHCI & OHCI data structures *)
+			(* currently max qh size is below 128 bytes for EHCI, so a queue head will not cross page boundaries *)
+			(*pipe.qh := Align(ADDRESS OF pipe.tdBuffer[0], 128);*)
+			(*pipe.tdBase := pipe.qh + 32;*)
+			IF Debug.StrongChecks THEN
+				(*ASSERT(pipe.tdBase >= ADDRESSOF(pipe.tdBuffer[0]));
+				ASSERT(pipe.tdBase <= ADDRESSOF(pipe.tdBuffer[pipe.tdBufferLen-1]));*)
+			END;
+			pipes[0, 0, 0] := pipe;
+			notifyPipes := pipe; (* Used as list head only (don't call its completion handler) *)
+		END Default;
+
+		(** Show diagnostic of host controller *)
+		PROCEDURE Diag*;
+		VAR port : LONGINT; dword : SET;
+		BEGIN
+			IF Debug.Trace THEN
+			KernelLog.String("Diagnostics of "); KernelLog.String(name);
+			KernelLog.String(" ("); KernelLog.String(desc); KernelLog.String(")");
+			KernelLog.Ln;
+			(* PCI information *)
+			KernelLog.String("   PCI bus "); KernelLog.Int(bus, 0); KernelLog.String(", device "); KernelLog.Int(device, 0);
+			KernelLog.String(", function "); KernelLog.Int(function, 0);
+			KernelLog.Ln;
+			(* IO base address and Interrupt Line *)
+			KernelLog.String("   I/O base "); KernelLog.Hex(iobase, 8); KernelLog.String("H,  Irq: "); KernelLog.Int (irq, 0);
+			KernelLog.String(", Int Counter: "); KernelLog.Int(NnofInterrupts, 0);
+			KernelLog.Ln;
+			(* Port Status *)
+			KernelLog.String("   Number of ports: "); KernelLog.Int(portCount, 0); KernelLog.String(", Port status: ");
+			KernelLog.Ln;
+			FOR port := 0 TO portCount-1 DO
+				dword := GetPortStatus(port, FALSE); KernelLog.String("      Port "); KernelLog.Int(port+1, 0); KernelLog.String(": ");
+				ShowPortStatus(dword);	KernelLog.Ln;
+			END;
+			END;
+		END Diag;
+	END Hcd;
+
+	(**
+	 * Abstract transfer descriptor.
+	 * These transfer descriptors are used only for interfacing pipes and HCDIs.
+	 * A transfer descriptor contains all necessary info for a transfer, in a transfer and hcd independent
+	 * structure. This makes a clean and extensible interface between the high-level pipe and the services it
+	 * requires from HCDIs.
+	 *)
+	TransferToken * = POINTER TO RECORD
+		(** Pipe associated with the transfer. Provides all pipe-specific information (type, packet size, direction) and completion handler. *)
+		pipe *: Pipe;
+		(** Transfer buffer. *)
+		buffer *: (*POINTER TO ARRAY OF CHAR*) ADDRESS;
+		(** ID of the transfer. Unique among all non-completed transfers of a pipe. *)
+		id *,
+		(** Offset and length of data to send/receive in the transfer buffer. *)
+		len *: LONGINT;
+		(** Status information: error, status and total transfered byte count. *)
+		errors *: SET;
+		status *, transfered *: LONGINT;
+		(** Control message. Used for control transfers only. *)
+		message *: ControlMessage;
+		(** HCD-specific data structures associated with this transfer. Used by HCDI only. Invalid after transfer completion. *)
+		tds *: ARRAY MaxTDs OF ADDRESS;
+
+		(** Chaining of transfer descriptors. *)
+		next *: TransferToken;
+	END;
+
+TYPE
+
+	(** Used for Interrupt On Completion (IOC) transfer status notification *)
+	TransferCompletion = OBJECT
+	VAR
+		done : BOOLEAN;
+		completiontimeout : BOOLEAN;
+		timer : Objects.Timer;
+		device* : Usbdi.UsbDevice;
+
+		PROCEDURE SetDone;
+		BEGIN {EXCLUSIVE}
+			done := TRUE;
+		END SetDone;
+
+		PROCEDURE SetCompletionTimeout;
+		BEGIN {EXCLUSIVE}
+			completiontimeout := TRUE;
+		END SetCompletionTimeout;
+
+		PROCEDURE AwaitDone(timeout : LONGINT) : BOOLEAN;
+		VAR result : BOOLEAN;
+		BEGIN {EXCLUSIVE}
+			Objects.SetTimeout(timer, SELF.SetCompletionTimeout, timeout);
+			done := FALSE; completiontimeout := FALSE;
+			AWAIT(done OR completiontimeout OR (device.state = StateDisconnected));
+			Objects.CancelTimeout(timer);
+			done := FALSE; result := completiontimeout; completiontimeout := FALSE;
+			RETURN ~result;
+		END AwaitDone;
+
+		PROCEDURE &New*;
+		BEGIN
+			NEW(timer);
+		END New;
+
+	END TransferCompletion;
+
+TYPE
+
+	(* Thread that calls the completion handler of a pipe (if it has one) *)
+	CompletionHandlerCaller = OBJECT
+	CONST
+		Size = 32;
+	VAR
+		completionHandler : Usbdi.CompletionHandler;
+		pipe : Pipe;
+		events: ARRAY Size OF RECORD status, actLen: LONGINT; END;
+		status, actLen: LONGINT;
+		head, len: LONGINT;
+		alive, dead : BOOLEAN;
+
+		PROCEDURE Call (status, actLen: LONGINT);
+		BEGIN {EXCLUSIVE}
+			AWAIT(len < Size);
+			events[(head + len) MOD Size].status := status;
+			events[(head + len) MOD Size].actLen := actLen;
+			INC(len);
+		END Call;
+
+		PROCEDURE HandleTimeout;
+		BEGIN{EXCLUSIVE}
+			dead := TRUE;
+		END HandleTimeout;
+
+		PROCEDURE Terminate;
+		VAR timer:Objects.Timer;
+		BEGIN
+			BEGIN {EXCLUSIVE} alive := FALSE; END;
+			(* release monitor lock *)
+			NEW(timer);
+			Objects.SetTimeout(timer, HandleTimeout, 100);
+			BEGIN {EXCLUSIVE} AWAIT(dead); END;
+		END Terminate;
+
+		PROCEDURE &New*(pipe : Pipe; c : Usbdi.CompletionHandler);
+		BEGIN
+			ASSERT(c # NIL);
+			SELF.pipe := pipe;
+			completionHandler := c;
+			alive := TRUE; dead := FALSE;
+		END New;
+
+	BEGIN {ACTIVE}
+		WHILE alive DO
+			BEGIN {EXCLUSIVE}
+				AWAIT((len > 0) OR ~alive);
+				status := events[head MOD Size].status;
+				actLen := events[head MOD Size].actLen;
+				INC(head);
+				DEC(len);
+			END;
+			IF alive THEN
+				(* notify the pipe that transfer finished to get transfer results in pipe. *)
+				(*pipe.CleanupTransfer(transfer);*)
+				completionHandler(status, actLen)
+			END;
+			(*BEGIN {EXCLUSIVE}
+				doCall := FALSE
+			END*)
+		END;
+		BEGIN {EXCLUSIVE} dead := TRUE; END;
+	END CompletionHandlerCaller;
+
+TYPE
+
+
+	(**
+	 * USB Communication Pipe
+	 * USB communication happens between buffers provided by client software and device endpoints. The association between
+	 * a client software buffer and a device endpoint is called pipe.
+	 * This is the low-level implementation of a pipe which is used by the host controller drivers. The fields declared here aren't visible
+	 * to client software which uses the interface defined by the USB Driver Interface (Usbdi).
+	 *
+	 * Concurrency:
+	 *	- concurrent calls on a pipe must preserve the transfer token list.
+	 *)
+	Pipe* = OBJECT(Usbdi.Pipe)
+	VAR
+		(* Device endpoint *)
+		address* : LONGINT; 	(* USB device address *)
+		endpoint* : LONGINT; 	(* Endpoint address *)
+		direction* : LONGINT; 	(* Endpoint direction; Usbdi.In or Usbdi.Out; not used for control transfers *)
+
+		(* Associated host controller & USB device *)
+		controller* : Hcd;
+		device* : Usbdi.UsbDevice;
+
+		(* Address of hub device that contains the transaction translator we're connected to and port of the TT *)
+		(* These fields are duplicate here (also available in Usb.UsbDevice to avoid the import of Usb.Mod in UsbEhci.Mod *)
+		ttPort*, ttAddress* : LONGINT;
+
+		(* Information from endpoint descriptor *)
+		type* : LONGINT; 		(* PipeControl, PipeBulk, PipeInterrupt or PipeIsochronous *)
+		irqInterval* : LONGINT; 	(* Interrupt interval for PipeInterrupt (interrupt transfers) *)
+		mult* : LONGINT; 		(* For high-speed interrupt/isochronous pipes: How many transactions per microframe (1,2 or 3) *)
+
+		(* Pipe specific features *)
+		speed* : LONGINT; 		 	(* LowSpeed, FullSpeed, HighSpeed *)
+		dataToggle* : BOOLEAN; 	(* 1bit sequence number *)
+(*		hostDelay, hubLsSetup : LONGINT; (* Delay introduced by host / hub in nanoseconds *)  *)
+
+		(** Transfer list. Manipulate only in EXCLUSIVE regions. *)
+		transfers*, lastTransfer*: TransferToken;
+		lock *: Locks.Lock;
+		(** Next transfer ID to use. Manipulate only in EXCLUSIVE regions. *)
+		nextId: LONGINT;
+
+		(* Periodic pipe policy *)
+		sampleInterval, sampleSize: LONGINT;
+
+		(* Transfer status information *)
+		status* : Usbdi.Status	;	(* Status of the last tranfer from/to this endpoint *)
+		errors* : SET;
+		transferLen* : LONGINT;		(* Length of the transfer in bytes *)
+		actLen* : LONGINT; 			(* how many bytes did the controller send/receive *)
+
+		(* Buffer for S/W scatter/gather support *)
+		sgBuffer : Usbdi.BufferPtr;
+		physBufferAdr- : ADDRESS;
+
+		(* Buffer for H/W scatter/gather suport *)
+		sgList- : POINTER TO ARRAY OF Machine.Range;
+
+		(* Pipe parameters set by client software *)
+		timeout* : LONGINT;
+
+		(* Data structures 																									*)
+		(* For control, bulk and interrupt transfers, each pipe has an associated queue head in the host controllers schedule.		*)
+		(* This queue head can be found in the queue <queue>. The actual USB transfers are described as linked list of 			*)
+		(* transfer descriptors (TD), which are linked to the pipe`s queue head. 												*)
+		queue* : Machine.Address32;
+		(*qh* : Machine.Address32;
+		firstTD*, lastTD* : Machine.Address32;
+
+		(* Per pipe buffer for transfer descriptors *)
+		tdBuffer* : POINTER TO ARRAY OF CHAR;
+		tdBufferLen* : LONGINT;
+		tdBase* : Machine.Address32;*)
+
+		(** Holder for controller-specific descriptors that are related to a pipe and not a transfer (e.g. queue heads in EHCI). A lock is provided for synchronization. *)
+		descriptors *: POINTER TO ARRAY OF ADDRESS;
+		(*descriptorLock *: Locks.Lock;*)
+
+		(* Transfer completion handling related *)
+		ioc* : BOOLEAN;  (* interrupt on completion enabled/disabled *)
+		completionHandlerCaller* : CompletionHandlerCaller; (* if active & ioc, the procedure interruptHandler will be called if status * ResInProgress = {} *)
+		irqActive* : BOOLEAN; (* Should the InterruptHandler be called? *)
+		completion- : TransferCompletion;
+
+		(* Pipe management *)
+		next* : Pipe;
+
+		(* Control pipes only: 8 Byte message *)
+		message : ControlMessage;
+
+		PROCEDURE &New*(adr, ep : LONGINT; hcd : Hcd);
+		BEGIN
+			NEW(completion);
+			address := adr; endpoint := ep; controller := hcd; status := Usbdi.InProgress;
+			(*IF controller.DMAchaining THEN NEW(sgList, controller.sgListSize); END;*)
+			(*NEW(descriptorLock);*)
+			NEW(lock)
+		END New;
+
+		(* For host controllers that do not support DMA chaining, the buffers must be physically contiguous. In Bluebottle, all buffers allocated 	*)
+		(* on the heap meet this requirement. Buffers allocated on the stack may, however, be physically non-contiguous. Fortunately, the 		*)
+		(* stack size is limited to 128K, so these buffers won't be bigger than 128K.																*)
+		(* This procedure...																													*)
+		(* 	- 	Returns TRUE and as a side effect copies the client buffer into the scatter/gather buffer for OUT transfers if the specified buffer	*)
+		(*		cannot directly be used by the host controller																					*)
+		(*	-	Returns FALSE when the client buffer meets the requirement of the host controller hardware									*)
+		PROCEDURE NeedSWScatterGather(direction, bufferLen, offset : LONGINT; VAR buffer : Usbdi.Buffer; VAR doCopy : BOOLEAN) : BOOLEAN;
+		VAR adr : ADDRESS;
+		BEGIN
+			doCopy := FALSE;
+			IF bufferLen = 0 THEN RETURN FALSE; END;
+			adr := Machine.PhysicalAdr(UsbBuffers.GetDataAddress(buffer), bufferLen);
+			IF adr = -1 THEN  (* buffer is not physically contiguous *)
+				IF sgBuffer = NIL THEN NEW(sgBuffer, 128*1024); END; (* 128K is stack limit *)
+				physBufferAdr := UsbBuffers.GetDataAddress(sgBuffer);
+				IF direction = In THEN
+					doCopy := TRUE;
+				ELSE
+					ASSERT(bufferLen <= 128*1024); (* If the buffer is on the heap, we don't reach this code. Stack limit is 128K *)
+					Copy(buffer, sgBuffer, offset, 0, bufferLen);
+				END;
+				RETURN TRUE;
+			ELSE
+				physBufferAdr := adr;
+				RETURN FALSE;
+			END;
+		END NeedSWScatterGather;
+
+		PROCEDURE Transfer*(bufferLen, offset : LONGINT; VAR buffer : Usbdi.Buffer) : Usbdi.Status;
+		VAR
+			copy : BOOLEAN;
+			transfer: TransferToken;
+		BEGIN
+			ASSERT(type # PipeControl);
+			ASSERT(LEN(buffer) >= bufferLen + offset);
+			IF Debug.Trace & Debug.traceTransfers THEN ShowTransfer(bufferLen, offset); END;
+
+			transfer := GetToken();
+			transfer.pipe := SELF;
+			IF ~controller.DMAchaining & NeedSWScatterGather(direction, bufferLen, 0, buffer, copy) THEN
+				transfer.buffer := UsbBuffers.GetDataAddress(sgBuffer)
+			ELSE
+				transfer.buffer := UsbBuffers.GetDataAddress(buffer)
+			END;
+			transfer.len := bufferLen;
+			transfer.status := Usbdi.InProgress;
+			IF timeout # 0 THEN status := Usbdi.InProgress END;
+
+			lock.Acquire;
+			transfer.id := nextId; INC(nextId);
+ 			IF transfers = NIL THEN
+				transfers := transfer
+			ELSE
+				lastTransfer.next := transfer
+			END;
+			lastTransfer := transfer;
+
+			IF (timeout = 0) OR (mode = Usbdi.MinCpu) THEN
+				irqActive := TRUE
+			END;
+			lock.Release;
+			controller.Schedule(transfer);
+
+			IF timeout = 0 THEN
+				RETURN transfer.id
+			ELSE
+				WaitForCompletion(transfer, buffer, copy);
+				RETURN status
+			END
+		END Transfer;
+
+		(** Waits for the completion of the next transfer. *)
+		PROCEDURE WaitForCompletion (transfer: TransferToken; buffer: Usbdi.Buffer; copy: BOOLEAN);
+		VAR
+			mtimer: Kernel.MilliTimer;
+		BEGIN
+			ASSERT(timeout # 0);
+			Kernel.SetTimer(mtimer, timeout);
+			IF mode # Usbdi.MinCpu THEN
+				Kernel.SetTimer(mtimer, timeout);
+				lock.Acquire;
+				LOOP
+					IF (transfer.status # Usbdi.InProgress) OR Kernel.Expired(mtimer) OR (device.state = StateDisconnected) THEN EXIT; END;
+					controller.UpdatePipeStatus(SELF);
+					IF mode = Usbdi.Normal THEN Objects.Yield; END;
+				END;
+				(* Copy result from transfer to pipe *)
+				CleanupTransfer(transfer);
+				lock.Release
+			ELSE
+				IF ~completion.AwaitDone(timeout) THEN
+					(* ignore *)
+				END;
+				(* Transfer cleanup is done in the interrupt handling routine. *)
+			END;
+
+			IF Debug.PerformanceMonitoring THEN
+				(* access should be protected *)
+				INC (controller.NbytesTransfered, actLen);
+			END;
+			IF copy & (status # Usbdi.InProgress) THEN (* copy data from scatter/gather buffer to client buffer *)
+				Copy(sgBuffer, buffer, 0, 0, actLen);
+			END;
+			IF Debug.Trace & Debug.traceControlData & (type = PipeControl) THEN
+				IF transfer.len > 0 THEN ShowData(transfer.buffer, transfer.len); KernelLog.Char(" "); ELSE KernelLog.String("[No Data] "); END;
+			END;
+			IF Debug.Trace & (Debug.traceTransfers OR Debug.traceControl) THEN ShowStatus(status); KernelLog.Ln; END;
+			IF Debug.Trace & (status # Usbdi.Ok) THEN
+				IF (status = Usbdi.ShortPacket) THEN
+					IF Debug.traceShortPackets THEN Show(TRUE); KernelLog.Ln; END;
+				ELSE
+					IF Debug.traceFailed THEN Show(TRUE); KernelLog.Ln; END;
+				END;
+			END;
+		END WaitForCompletion;
+
+		(** For control transfers (only for Control Pipes) *)
+		PROCEDURE Request*(bmRequestType : SET;  bRequest, wValue, wIndex, wLength : LONGINT; VAR buffer : Usbdi.Buffer) : Usbdi. Status;
+		VAR
+			dir, ofs: LONGINT;
+			transfer: TransferToken;
+			copy: BOOLEAN;
+		BEGIN
+			ASSERT(type = PipeControl);
+			ASSERT(LEN(buffer) >= wLength);
+			ASSERT(buffer # NIL);
+
+			transfer := GetToken();
+
+			(*transferLock.Acquire;*)
+			IF transfer.message = NIL THEN NEW(transfer.message, 8); END;
+			transfer.message[0] := CHR(SYSTEM.VAL(LONGINT, bmRequestType));
+			transfer.message[1] := CHR(bRequest);
+			transfer.message[2] := CHR(wValue);
+			transfer.message[3] := CHR(LSH(wValue, -8));
+			transfer.message[4] := CHR(wIndex);
+			transfer.message[5] := CHR(LSH(wIndex,-8));
+			transfer.message[6] := CHR(wLength);
+			transfer.message[7] := CHR(LSH(wLength, -8));
+			dir := direction;
+			IF bmRequestType * Usbdi.ToHost # {} THEN direction := In; ELSE 	direction := Out; END;
+			IF Debug.Trace & Debug.traceControl THEN ShowMessage(wLength, direction, transfer.message); END;
+
+			transfer.pipe := SELF;
+			IF ~controller.DMAchaining & NeedSWScatterGather(dir, wLength, 0, buffer, copy) THEN
+				transfer.buffer := UsbBuffers.GetDataAddress(sgBuffer)
+			ELSE
+				transfer.buffer := UsbBuffers.GetDataAddress(buffer)
+			END;
+			transfer.len := wLength;
+			transfer.status := Usbdi.InProgress;
+
+			lock.Acquire;
+			transfer.id := nextId; INC(nextId);
+			(* We do not allow transfer queuing on control pipes, therefore the transfer list shall be empty *)
+			ASSERT(transfers = NIL);
+			transfers := transfer;
+			(*lastTransfer := transfer;*)
+			lock.Release;
+			controller.Schedule(transfer);
+
+			WaitForCompletion(transfer, buffer, copy);
+			direction := dir;
+			RETURN status
+		END Request;
+
+		PROCEDURE ClearHalt*(): BOOLEAN;
+		CONST FsEndpointHalt = 0; SrClearFeature = 1;
+		VAR res : BOOLEAN;
+		BEGIN
+			IF Debug.Trace & Debug.tracePipes THEN
+				KernelLog.String("UsbHcdi: Clearhalt Pipe"); KernelLog.String(" Adr: ");KernelLog.Int(address, 0);
+				KernelLog.String(" Ep: "); KernelLog.Int(endpoint, 0); KernelLog.Ln;
+			END;
+			controller.ClearHalt(SELF);
+			lock.Acquire;
+			transfers := NIL;
+			lock.Release;
+			res := device.Request(Usbdi.ToDevice + Usbdi.Standard + Usbdi.Endpoint, SrClearFeature, FsEndpointHalt, endpoint, 0, Usbdi.NoData) = Usbdi.Ok;
+			IF res THEN dataToggle := FALSE; END;
+			RETURN res;
+		END ClearHalt;
+
+		PROCEDURE IsHalted*() : BOOLEAN;
+		CONST SrGetStatus = 0; Halted = {0};
+		VAR buffer : Usbdi.BufferPtr; status : SET;
+		BEGIN
+			IF Debug.Trace & Debug.tracePipes THEN
+				KernelLog.String("UsbHcdi: Get endpoint status for Adr: "); KernelLog.Int(address, 0);
+				KernelLog.String(", Ep: "); KernelLog.Int(endpoint, 0); KernelLog.Ln;
+			END;
+			NEW(buffer, 2);
+			IF device.Request(Usbdi.ToHost + Usbdi.Standard + Usbdi.Endpoint, SrGetStatus , 0, endpoint MOD 16, 2, buffer) = Usbdi.Ok THEN
+				status := SYSTEM.VAL(SET, ORD(buffer[0]) + ORD(buffer[1])*100H);
+				RETURN status * Halted # {};
+			END;
+			RETURN FALSE;
+		END IsHalted;
+
+		PROCEDURE GetActLen*() : LONGINT;
+		BEGIN
+			RETURN actLen;
+		END GetActLen;
+
+		PROCEDURE SetTimeout*(timeout : LONGINT);
+		BEGIN
+			SELF.timeout := timeout;
+		END SetTimeout;
+
+		PROCEDURE GetStatus*(VAR len : LONGINT) : Usbdi.Status;
+		BEGIN
+			controller.UpdatePipeStatus(SELF); len := actLen;
+			RETURN status;
+		END GetStatus;
+
+		PROCEDURE SetCompletionHandler*(handler: Usbdi.CompletionHandler);
+		BEGIN
+			ioc := TRUE; (* set Interrupt On Completion Bit in TD's *)
+			mode := Usbdi.MinCpu;
+			IF completionHandlerCaller # NIL THEN
+				completionHandlerCaller.Terminate;
+			END;
+			NEW(completionHandlerCaller, SELF, handler);
+			controller.AddCompletionHandler(SELF);
+		END SetCompletionHandler;
+
+		PROCEDURE GetLastTransactionId * (): LONGINT;
+		BEGIN
+		END GetLastTransactionId;
+
+		PROCEDURE CancelTransaction * (id: LONGINT): BOOLEAN;
+		VAR
+			tx: TransferToken;
+		BEGIN
+			tx := transfers;
+			WHILE (tx # NIL) & (tx.id # id) DO tx := tx.next END;
+			IF (tx = NIL) OR (tx.status = Usbdi.InProgress) THEN
+				RETURN FALSE
+			ELSE
+				RETURN controller.Cancel(tx)
+			END
+		END CancelTransaction;
+
+		PROCEDURE SetPolicy * (interval, size: LONGINT): BOOLEAN;
+		VAR
+			res: BOOLEAN;
+		BEGIN
+			IF (type = PipeControl) OR (type = PipeBulk) THEN RETURN FALSE END;
+			res := controller.CheckPipePolicy(interval, size);
+			IF res THEN
+				sampleInterval := interval;
+				sampleSize := size
+			END;
+			RETURN res
+		END SetPolicy;
+
+		(**
+		 * Cleanup a finished transfer.
+		 * This is called when a transfer finished.
+		 *)
+		PROCEDURE CleanupTransfer (transfer: TransferToken);
+		VAR
+			prev: TransferToken;
+			readLocked: BOOLEAN;
+		BEGIN
+			(*TRACE("Cleanup acquire Tk Write");*)
+			(*readLocked := transferLock.HasLock();
+			IF ~readLocked THEN transferLock.Acquire END;*)
+			(* update pipe status variables *)
+			ASSERT(transfer # NIL);
+			(*IF type = PipeIsochronous THEN TRACE(transfer, transfer.status, transfer.errors, transfer.transfered) END;*)
+			status := transfer.status;
+			errors := transfer.errors;
+			actLen := transfer.transfered;
+
+			(* remove transfer from list *)
+			(*IF type = PipeIsochronous THEN TRACE(transfer, transfers) END;*)
+			IF transfer = transfers THEN
+				transfers := transfers.next
+			ELSE
+				prev := transfers;
+				WHILE (prev # NIL) & (prev.next # NIL) & (prev.next # transfer) DO
+					prev := prev.next
+				END;
+				ASSERT(prev # NIL);
+				ASSERT(prev.next = transfer);
+				IF transfers = lastTransfer THEN
+					prev.next := NIL;
+					lastTransfer := prev
+				ELSE
+					prev.next := transfer.next
+				END
+			END;
+			(*IF ~readLocked THEN transferLock.Release END;*)
+			(*TRACE("Cleanup release Tk Write");*)
+			PutToken(transfer)
+		END CleanupTransfer;
+
+		(* Display textual representation of the transfer that will be executed. *)
+		PROCEDURE ShowTransfer(bufferLen, offset : LONGINT);
+		BEGIN
+			IF Debug.Trace THEN
+			KernelLog.String("UsbHcdi: ");
+			CASE type OF
+				PipeControl : KernelLog.String("Control Transfer???:");
+				| PipeBulk : KernelLog.String("Bulk Transfer:");
+				| PipeInterrupt : KernelLog.String("Interrupt Transfer:");
+				| PipeIsochronous : KernelLog.String("Isochronous Transfer:");
+			ELSE KernelLog.String("Unknown transfer type");
+			END;
+			KernelLog.String(" Adr: "); KernelLog.Int(address, 0);
+			KernelLog.String(" Endpoint: "); KernelLog.Int(endpoint, 0);
+			KernelLog.String(" Length: "); KernelLog.Int(bufferLen, 0); KernelLog.String(" Bytes: ");
+			END;
+		END ShowTransfer;
+
+		PROCEDURE ShowMessage(bufferLen, direction : LONGINT; CONST msg : ControlMessage);
+		VAR i : LONGINT;
+		BEGIN
+			IF Debug.Trace THEN
+			KernelLog.String("UsbHcdi: Control Transfer: ");
+			IF direction = In THEN KernelLog.String("IN");
+			ELSIF direction = Out THEN KernelLog.String("OUT");
+			ELSE KernelLog.String("ERROR");
+			END;
+			KernelLog.String(" Adr: "); KernelLog.Int(address, 0);
+			KernelLog.String(" Endpoint: "); KernelLog.Int(endpoint, 0);
+			KernelLog.String(" Length: "); KernelLog.Int(bufferLen, 0); KernelLog.String(" Bytes: ");
+			KernelLog.String(" CtrlMsg: "); 	FOR i := 0 TO 7 DO KernelLog.Hex(ORD(msg[i]), -2); KernelLog.String(" "); END;
+			END;
+		END ShowMessage;
+
+		PROCEDURE ShowData (buffer: ADDRESS; len: LONGINT);
+		VAR i : LONGINT;
+		BEGIN
+			IF Debug.Trace THEN
+			KernelLog.String("[DATA: ");
+			FOR i := 0 TO len - 1 DO
+				KernelLog.Hex(SYSTEM.GET8(buffer + i), -2);
+				IF i < len-1 THEN KernelLog.Char(" "); END;
+			END;
+			KernelLog.Char("]");
+			END;
+		END ShowData;
+
+		PROCEDURE Show*(detailed : BOOLEAN);
+		BEGIN
+			IF Debug.Trace THEN
+				CASE type OF
+					| PipeControl: KernelLog.String(" Control ");
+					| PipeInterrupt : KernelLog.String(" Interrupt ");
+					| PipeBulk : KernelLog.String(" Bulk ");
+					| PipeIsochronous : KernelLog.String(" Isochronous ");
+				ELSE
+					KernelLog.String("Unknown("); KernelLog.Int(type, 0); KernelLog.String(") ");
+				END;
+				CASE direction OF
+					| Out: KernelLog.String("OUT");
+					| In : KernelLog.String("IN");
+				ELSE
+					KernelLog.String("IN/OUT");
+				END;
+				KernelLog.String(" Pipe:"); KernelLog.String(" Adr: "); KernelLog.Int(address, 0); KernelLog.String(" Ep: "); KernelLog.Int(endpoint, 0);
+				KernelLog.String(" ");
+				IF ioc THEN KernelLog.String("[IOC]"); END;
+				IF completionHandlerCaller # NIL THEN KernelLog.String("[Handler]"); END;
+				IF irqActive THEN KernelLog.String("[IRQ_ACTIVE]"); END;
+				IF speed = LowSpeed THEN KernelLog.String("[LowSpeed]");
+				ELSIF speed = FullSpeed THEN KernelLog.String("[FullSpeed]");
+				ELSIF speed = HighSpeed THEN KernelLog.String("[HighSpeed]");
+				ELSE KernelLog.String("[ERROR: Not speed specified]");
+				END;
+				KernelLog.Ln;
+				(*KernelLog.String("    Queue: "); KernelLog.Hex(queue, 8); KernelLog.String("H");
+				KernelLog.String(", QH: "); KernelLog.Hex(qh, 8); KernelLog.String("H");
+				KernelLog.String(", firstTD: "); KernelLog.Hex(firstTD, 8); KernelLog.String("H");
+				KernelLog.String(", lastTD: "); KernelLog.Hex(lastTD, 8); KernelLog.String("H");
+				KernelLog.Ln;*)
+				IF detailed THEN
+					IF type = PipeInterrupt THEN KernelLog.String("    IRQ Interval: "); KernelLog.Int(irqInterval, 0); KernelLog.String("ms, "); ELSE KernelLog.String("    "); END;
+					KernelLog.String("Timeout: "); KernelLog.Int(timeout, 0); KernelLog.String("ms ");
+					KernelLog.String(", MaxPacketSize: "); KernelLog.Int(maxPacketSize, 0); KernelLog.String(" Bytes");
+					KernelLog.String(", MaxRetries: "); KernelLog.Int(maxRetries, 0);
+					KernelLog.String(", Mode: ");
+					CASE mode OF
+						|Usbdi.Normal: KernelLog.String("Normal");
+						|Usbdi.MaxPerformance: KernelLog.String("MaxPerformance");
+						|Usbdi.MinCpu: KernelLog.String("MinCPU");
+					ELSE
+						KernelLog.String("Undefined ("); KernelLog.Int(mode, 0); KernelLog.String(")");
+					END;
+					KernelLog.String(", Last status: "); ShowStatus(status);
+					KernelLog.String(", Last errors: "); ShowErrors(errors); KernelLog.Ln;
+				END;
+			END;
+		END Show;
+	END Pipe;
+
+TYPE
+	Registry= OBJECT(Plugins.Registry) END Registry;
+
+VAR
+	controllerCount : LONGINT; (* Only used for name creation - does not necessary reflect the actual HC count *)
+	controllers- : (*Plugins.*)Registry;
+
+	tokenPool: TransferToken;
+
+(** Copy data from array to array *)
+PROCEDURE Copy(VAR from, to: Usbdi.Buffer; fofs, tofs, len: LONGINT);
+BEGIN
+	IF Debug.Trace & Debug.traceCopying THEN KernelLog.String("UsbHcdi: SG: Copying "); KernelLog.Int(len, 0); KernelLog.String(" Bytes."); KernelLog.Ln; END;
+	IF len > 0 THEN
+		ASSERT((fofs+len <= LEN(from)) & (tofs+len <= LEN(to)));
+		SYSTEM.MOVE(UsbBuffers.GetDataAddress(from) + fofs, UsbBuffers.GetDataAddress(to) + tofs, len);
+	END;
+END Copy;
+
+(** Assign a name to the host controller and add it to the controllers registry *)
+PROCEDURE RegisterHostController*(hcd : Hcd; CONST description : Plugins.Description);
+VAR name : Plugins.Name; res : LONGINT;
+BEGIN {EXCLUSIVE}
+	name := "USBHC"; name[5] := CHR(controllerCount + 48); name[6] := 0X;
+	hcd.SetName(name); hcd.desc := description;
+	(* Register the host controller as AosPlugin *)
+	controllers.Add(hcd, res);
+	IF res # Plugins.Ok THEN (* ERROR: registering the host controller failed, should not happen *)
+		KernelLog.Enter; KernelLog.String("UsbHcdi: Error: Couldn't add host controller to registry."); KernelLog.Exit;
+	ELSE
+		INC(controllerCount);
+	END;
+END RegisterHostController;
+
+(** Remove all controllers with the specified description from the controllers registry *)
+PROCEDURE UnRegisterHostControllers*(CONST description : Plugins.Description);
+VAR table : Plugins.Table; hcd : Hcd; i : LONGINT;
+BEGIN {EXCLUSIVE}
+	controllers.GetAll(table);
+	IF table # NIL THEN
+		FOR i := 0 TO LEN(table)-1 DO
+			hcd := table[i] (Hcd);
+			IF hcd.desc = description THEN
+				hcd.Cleanup;
+				controllers.Remove(hcd);
+			END;
+		END;
+	END;
+END UnRegisterHostControllers;
+
+PROCEDURE Align*(address: ADDRESS; size: LONGINT): ADDRESS;
+BEGIN
+	RETURN address + LONGINT(-address) MOD size;
+END Align;
+
+(**
+ * Returns an AlignMemSpace with: memspace.data[memspace.base] is the first, <alignment>-aligned element of an
+ * array of LONGINTs of the size <size>;  parameters in bytes; alignmet has to be a power of two
+ *)
+PROCEDURE GetAlignedMemSpace*(size, alignment : LONGINT ) : AlignedMemSpace;
+VAR memspace : AlignedMemSpace; base, adr: ADDRESS;  len: SIZE;
+BEGIN
+	ASSERT(alignment >= 4);
+	NEW(memspace);
+	len := (size +  alignment) DIV 4;
+	NEW(memspace.data, len ); (* so we will definitly find a <alignment>-aligned memory space of the size <size> *)
+
+	adr := ADDRESSOF(memspace.data[0]);
+	(* alignment *)
+	base := adr + alignment - adr MOD alignment;
+	(*base := adr + LONGINT(-adr) MOD alignment;*)
+	ASSERT(base MOD alignment = 0);
+
+	memspace.base := (base - adr) DIV 4;
+	ASSERT(ADDRESSOF(memspace.data[memspace.base]) = base);
+	RETURN memspace;
+END GetAlignedMemSpace;
+
+(** Display textual representation of the USB tranfer status bits defined in Usbdi.Mod  *)
+PROCEDURE ShowStatus*(status : Usbdi.Status);
+BEGIN
+	IF Debug.Trace THEN
+	IF status = Usbdi.Ok THEN KernelLog.String("[Ok]"); END;
+	IF status = Usbdi.ShortPacket THEN KernelLog.String("[ShortPacket]"); END;
+	IF status = Usbdi.Stalled THEN KernelLog.String("[Stalled]"); END;
+	IF status = Usbdi.InProgress THEN KernelLog.String("[InProgress]"); END;
+	IF status = Usbdi.Error THEN KernelLog.String("[Error]"); END;
+	IF status = Usbdi.Disconnected THEN KernelLog.String("[Disconnected]"); END;
+	END;
+END ShowStatus;
+
+PROCEDURE ShowErrors*(errors : SET);
+BEGIN
+	IF Debug.Trace THEN
+	IF errors = NoErrors THEN KernelLog.String("[NoErrors]"); END;
+	IF errors * ShortPacket # {} THEN KernelLog.String("[ShortPacket]"); END;
+	IF errors * Stalled # {} THEN KernelLog.String("[Stalled]"); END;
+	IF errors * InProgress # {} THEN KernelLog.String("[InProgress]"); END;
+	IF errors * Nak # {} THEN KernelLog.String("[Nak]"); END;
+	IF errors * Crc # {} THEN KernelLog.String("[Crc]"); END;
+	IF errors * Timeout # {} THEN KernelLog.String("[Timeout]"); END;
+	IF errors * CrcTimeout # {} THEN KernelLog.String("[CRC/Timeout]"); END;
+	IF errors * BitStuff # {} THEN KernelLog.String("[Bitstuff]"); END;
+	IF errors * Databuffer # {} THEN KernelLog.String("[Databuffer]"); END;
+	IF errors * Babble # {} THEN KernelLog.String("[Babble]"); END;
+	IF errors * Internal # {} THEN KernelLog.String("[Internal]"); END;
+	IF errors * Disconnected # {} THEN KernelLog.String("[Disconnected]"); END;
+	IF errors * UnexpectedPid # {} THEN KernelLog.String("[UnexpectedPid]"); END;
+	IF errors * TransferTooLarge # {} THEN KernelLog.String("[TransferTooLarge]"); END;
+	IF errors * PidCheckFailure # {} THEN KernelLog.String("[PidCheckFailure]"); END;
+	IF errors * DataToggleMismatch # {} THEN KernelLog.String("[DatatoggleMismatch]"); END;
+	IF errors * DeviceNotResponding # {} THEN KernelLog.String("[DeviceNotResponding]"); END;
+	IF errors * LinkTDsFailed # {} THEN KernelLog.String("[TDLinkError]"); END;
+	IF errors * OutOfTDs  # {} THEN KernelLog.String("[OutOfTDs]"); END;
+	END;
+END ShowErrors;
+
+(** Display textual represenation of the port status bits defined in UsbHcdi.Mod *)
+PROCEDURE ShowPortStatus*(status : SET);
+BEGIN
+	IF Debug.Trace THEN
+	IF status * PortStatusEnabled # {} THEN KernelLog.String("[Enabled]"); ELSE KernelLog.String("[Disabled]"); END;
+	IF status * PortStatusDevicePresent # {} THEN
+		IF status * PortStatusLowSpeed # {} THEN KernelLog.String("[LowSpeed]");
+		ELSIF status * PortStatusFullSpeed # {} THEN KernelLog.String("[FullSpeed]");
+		ELSIF status * PortStatusHighSpeed # {} THEN KernelLog.String("[HighSpeed]");
+		ELSE
+			KernelLog.String("[ERROR:Device connected but no speed indication, port enabled?]");
+		END;
+	END;
+	IF status * PortStatusReset # {} THEN KernelLog.String("[Reset]"); END;
+	IF status * PortStatusDevicePresent # {} THEN KernelLog.String("[DevicePresent]"); END;
+	IF status * PortStatusError # {} THEN KernelLog.String("[Error]"); END;
+	IF status * PortStatusConnectChange # {} THEN KernelLog.String("[ConnectChange]"); END;
+	IF status * PortStatusSuspended # {} THEN KernelLog.String("[Suspended]"); END;
+	IF status * PortStatusOverCurrent # {} THEN KernelLog.String("[OverCurrent]"); END;
+	IF status * PortStatusPowered # {} THEN KernelLog.String("[Powered]"); END;
+	IF status * PortStatusEnabledChange # {} THEN KernelLog.String("[EnabledChange]"); END;
+	IF status * PortStatusSuspendChange # {} THEN KernelLog.String("[SuspendChange]"); END;
+	IF status * PortStatusOverCurrentChange # {} THEN KernelLog.String("[OverCurrentChange]"); END;
+	IF status * PortStatusWakeOnOvercurrent # {} THEN KernelLog.String("[WakeOnOvercurrent]"); END;
+	IF status * PortStatusWakeOnDisconnect # {} THEN KernelLog.String("[WakeOnDisconnect]"); END;
+	IF status * PortStatusWakeOnConnect # {} THEN KernelLog.String("[WakeOnConnect]"); END;
+	IF status * PortStatusTestControl # {} THEN KernelLog.String("[TestControl]"); END;
+	IF status * PortStatusIndicatorControl # {} THEN KernelLog.String("[IndicatorControl]"); END;
+	IF status * PortStatusPortOwner # {} THEN KernelLog.String("[PortOwner]"); END;
+	END;
+END ShowPortStatus;
+
+PROCEDURE GetToken (): TransferToken;
+VAR
+	token: TransferToken;
+	i: LONGINT;
+BEGIN {EXCLUSIVE}
+	IF tokenPool = NIL THEN
+		NEW(token)
+	ELSE
+		token := tokenPool;
+		tokenPool := tokenPool.next;
+
+		(* Clear token *)
+		(*Machine.Fill32(SYSTEM.VAL(ADDRESS, token), SIZEOF(TransferToken), 0)*)
+		token.pipe := NIL;
+		token.buffer := 0;
+		token.id := 0;
+		token.len := 0;
+		token.errors := {};
+		token.status := 0;
+		token.transfered := 0;
+		(*token.message.value := NIL;*)
+		FOR i := 0 TO MaxTDs - 1 DO token.tds[i] := 0 END;
+		token.next := NIL
+	END;
+	RETURN token
+END GetToken;
+
+PROCEDURE PutToken (token: TransferToken);
+BEGIN {EXCLUSIVE}
+	token.next := tokenPool;
+	tokenPool := token
+END PutToken;
+
+PROCEDURE Cleanup;
+BEGIN
+	Plugins.main.Remove(controllers);
+END Cleanup;
+
+BEGIN
+	Modules.InstallTermHandler(Cleanup);
+	NEW(controllers, "UsbHcdi","USB host controller drivers");
+END UsbHcdi.

source/ARM.UsbHid.Mod

@@ -0,0 +1,252 @@
+MODULE UsbHid; (** AUTHOR "staubesv"; PURPOSE "HID device class specific requests"; *)
+(**
+ * Base class for HID class driver.
+ * Implements the HID class-specific requests and parsing of the HID descriptor.
+ *
+ * References:
+ *	Device Class Definition for Human Interface Devices (HID), version 1.11, 27.06.2001, www.usb.org
+ *
+ * History:
+ *
+ *	20.11.2005	First Release (staubesv)
+ *	09.01.2006	Adapted to Usb.Mod changes (staubesv)
+ *	05.07.2006	Adapted to Usbdi (staubesv)
+ *	23.11.2006	Removed interfaceNumber parameter, added GetHidDescriptor, cleanup (staubesv)
+ *)
+
+IMPORT KernelLog, Usbdi;
+
+CONST
+
+	(** HID Descriptors Types *)
+	DescriptorHID* = 21H;
+	DescriptorReport* = 22H;
+	DescriptorPhysical* = 23H;
+
+	(** HID Report Types *)
+	ReportInput* =  01H;
+	ReportOutput* = 02H;
+	ReportFeature* = 03H;
+
+	(** HID Protocol Types *)
+	BootProtocol* = 0;
+	ReportProtocol* = 1;
+
+	(* USB HID Class Specific Request Codes, HID p. 51 *)
+	HrGetReport = 01H;
+	HrGetIdle = 02H;
+	HrGetProtocol = 03H;
+	HrSetReport = 09H;
+	HrSetIdle = 0AH;
+	HrSetProtocol = 0BH;
+	SrGetDescriptor = 6;
+
+	HidSetRequest = Usbdi.ToDevice + Usbdi.Class + Usbdi.Interface;
+	HidGetRequest = Usbdi.ToHost + Usbdi.Class + Usbdi.Interface;
+
+TYPE
+
+	(* HID descriptor according to the Device Class Definition for HID, p. 22 *)
+	HidDescriptor* = POINTER TO RECORD
+		bLength- : LONGINT;
+		bDescriptorType- : LONGINT;
+		bcdHID- : LONGINT;
+		bCountryCode- : LONGINT;
+		bNumDescriptors- : LONGINT;
+		bClassDescriptorType- : LONGINT;
+		wDescriptorLength- : LONGINT;
+		optionalDescriptors- : POINTER TO ARRAY OF OptionalDescriptor;
+	END;
+
+	OptionalDescriptor* = RECORD
+		bDescriptorType- : LONGINT;
+		wDescriptorLength- : LONGINT;
+	END;
+
+TYPE
+
+	(* Base class of HID device drivers. Provides the HID class specific requests. *)
+	HidDriver* = OBJECT(Usbdi.Driver);
+
+		(** HID class specific device requests *)
+
+		(**
+		 * The SetIdle request silinces a particular report on the Interrupt In pipe until a new event occurs or
+		 * the specified amount of time passes (HID, p.52)
+		 * @param interface Related USB device interface
+		 * @param duration: 0: infinite, 1-255: value * 4ms
+		 * @reportId 0: idle rate applies to all reports, otherwise it applies only to reports with the corresponding reportId
+		 * @return TRUE, if requests succeeds, FALSE otherwise
+		 *)
+		PROCEDURE SetIdle*(reportId, duration : LONGINT) : BOOLEAN;
+		BEGIN
+			ASSERT(interface.bInterfaceClass = 3H);
+			RETURN device.Request(HidSetRequest, HrSetIdle, reportId + duration*100H, interface.bInterfaceNumber, 0, Usbdi.NoData) = Usbdi.Ok;
+		END SetIdle;
+
+		(**
+		 * The GetIdle request reads the current idle rate for a particular input report. See HID p. 52
+		 * @param interface Related USB device interface
+		 * @param reportId
+		 * @param idle Idle rate; 0: infinite duration, otherwise: idle rate in milliseconds; Only valid then request succeeded!
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE GetIdle*(reportId : LONGINT; VAR idle : LONGINT) : BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			ASSERT(interface.bInterfaceClass = 3H);
+			NEW(buffer, 1);
+			IF device.Request(HidGetRequest, HrGetIdle, reportId, interface.bInterfaceNumber, 1, buffer) = Usbdi.Ok THEN
+				idle := 4*ORD(buffer[0]);
+				RETURN TRUE;
+			END;
+			RETURN FALSE;
+		END GetIdle;
+
+		(**
+		 * The SetProtocol request switches between the boot protocol and the report protocol (HID p. 54).
+		 * This request is only supported by devices in the boot subclass. Default is the Report Protocol.
+		 * @param interface the request should be applied to
+		 * @param protocol 0: Boot Protocol, 1: Report Protocol
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE SetProtocol*(protocol : LONGINT) : BOOLEAN;
+		BEGIN
+			ASSERT(interface.bInterfaceClass = 3H);
+			ASSERT(((protocol = BootProtocol) OR (protocol = ReportProtocol)) & (interface.bInterfaceSubClass = 1));
+			RETURN device.Request(HidSetRequest, HrSetProtocol, protocol, interface.bInterfaceNumber, 0, Usbdi.NoData) = Usbdi.Ok ;
+		END SetProtocol;
+
+		(**
+		 * The Getprotocol requests reads which protocol is currently active (HID, p. 54).
+		 * This request is only supported by devices in the boot subclass.
+		 * @param interface the request should be applied to
+		 * @param protocol 0: Boot Protocol, 1: Report Protocol (Only valid if request succeeds!)
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE GetProtocol*(VAR protocol : LONGINT) : BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			IF (interface.bInterfaceClass # 3H) OR (interface.bInterfaceSubClass #  1) THEN
+				TRACE(interface.bInterfaceClass, interface.bInterfaceSubClass); RETURN FALSE
+			END;
+			ASSERT((interface.bInterfaceClass = 3H) & (interface.bInterfaceSubClass=  1));
+			NEW(buffer, 1);
+			IF device.Request(HidGetRequest, HrGetProtocol, 0, interface.bInterfaceNumber, 1, buffer) = Usbdi.Ok THEN
+				protocol := ORD(buffer[0]);
+				RETURN TRUE;
+			END;
+			RETURN FALSE;
+		END GetProtocol;
+
+		(**
+		 * The SetReport request allows the host to send a report to the device, possibly setting the state of input,
+		 * output, or feature controls (HID, p. 52).
+		 * @param interface the request should be applied to
+		 * @param type of the report the host sends
+		 * @param id of the report the host sends
+		 * @param buffer: Buffer containing the report
+		 * @param len: Lenght of the report
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE SetReport*(type, id : LONGINT; VAR buffer: Usbdi.Buffer; len : LONGINT) : BOOLEAN;
+		BEGIN
+			ASSERT(interface.bInterfaceClass = 3H);
+			RETURN device.Request(HidSetRequest, HrSetReport, id + type*100H, interface.bInterfaceNumber, len, buffer) = Usbdi.Ok;
+		END SetReport;
+
+		(**
+		 * The GetReport request allows the host to receive a report via the Control pipe (HID, p.51)
+		 * @param interface the request should be applied to
+		 * @param type Type of the report we want
+		 * @param id of the report we want
+		 * @param buffer: Buffer to put the report into
+		 * @param len: Exspected length of the report
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		*)
+		PROCEDURE GetReport*(type, id : LONGINT; VAR buffer: Usbdi.Buffer; len : LONGINT) : BOOLEAN;
+		BEGIN
+			ASSERT(LEN(buffer) >= len);
+			ASSERT(interface.bInterfaceClass = 3H);
+			RETURN device.Request(HidGetRequest, HrGetReport, id + type*100H, interface.bInterfaceNumber, len, buffer) = Usbdi.Ok;
+		END GetReport;
+
+		(** This request returns the specified descriptor if the descriptor exists *)
+		PROCEDURE GetDescriptor*(descriptor, index, wIndex, len : LONGINT; VAR buffer : Usbdi.Buffer) : BOOLEAN;
+		BEGIN
+			ASSERT(LEN(buffer) >= len);
+			RETURN device.Request(Usbdi.ToHost + Usbdi.Standard + Usbdi.Interface, SrGetDescriptor, index + descriptor*100H, wIndex, len, buffer) = Usbdi.Ok;
+		END GetDescriptor;
+
+		(** Returns the HID descriptor of this interface or NIL if not present *)
+		PROCEDURE GetHidDescriptor*() : HidDescriptor;
+		VAR ud : Usbdi.UnknownDescriptor; hidDescriptor : HidDescriptor;
+		BEGIN
+			ASSERT(interface.bInterfaceClass = 3H);
+			(* The HID descriptor is part of the configuration descriptor and therefore already pre-parsed by the USB system *)
+			ud := interface.unknown;
+			WHILE (ud # NIL) & (ud.bDescriptorType # DescriptorHID) DO ud := ud.next; END;
+			IF ud # NIL THEN
+				hidDescriptor := ParseHidDescriptor(ud.descriptor);
+			END;
+			RETURN hidDescriptor;
+		END GetHidDescriptor;
+
+
+	END HidDriver;
+
+(* Load and parse the HID descriptor correspondig to the drivers interface *)
+PROCEDURE ParseHidDescriptor(descriptor : Usbdi.BufferPtr) : HidDescriptor;
+VAR hid : HidDescriptor;  i : LONGINT;
+BEGIN
+	IF (descriptor # NIL) & (LEN(descriptor) >= 8) THEN
+		NEW(hid);
+		hid.bLength := ORD(descriptor[0]);
+		hid.bDescriptorType := ORD(descriptor[1]);
+		hid.bcdHID := ORD(descriptor[2]) + 100H*ORD(descriptor[3]);
+		hid.bCountryCode := ORD(descriptor[4]);
+		hid.bNumDescriptors := ORD(descriptor[5]);
+		hid.bClassDescriptorType := ORD(descriptor[6]);
+		hid.wDescriptorLength := ORD(descriptor[7]);
+
+		(* Parse the optional descriptors if there are some *)
+		IF hid.bNumDescriptors > 1 THEN
+			IF LEN(descriptor) >= (3 * (hid.bNumDescriptors-2)) + 7 THEN
+				KernelLog.String("UsbHid: Warning: HID descriptor too short"); KernelLog.Ln;
+				RETURN hid;
+			END;
+			NEW(hid.optionalDescriptors, hid.bNumDescriptors-1);
+			FOR i := 0 TO hid.bNumDescriptors-2 DO
+				hid.optionalDescriptors[i].bDescriptorType := ORD(descriptor[(3 * i) + 6]);
+				hid.optionalDescriptors[i].wDescriptorLength := ORD(descriptor[(3 * i) + 7]);
+			END;
+		END;
+	END;
+	RETURN hid;
+END ParseHidDescriptor;
+
+PROCEDURE ShowHidDescriptor*(hd : HidDescriptor);
+VAR i : LONGINT;
+BEGIN
+	KernelLog.String("HID Descriptor: ");
+	IF hd = NIL THEN KernelLog.String("NIL"); END; KernelLog.Ln;
+	KernelLog.String("   bLength: "); KernelLog.Int(hd.bLength, 0); KernelLog.Ln;
+	KernelLog.String("   bDescriptorType: "); KernelLog.Int(hd.bDescriptorType, 0);
+	KernelLog.String("   bcdHID: "); KernelLog.Hex(hd.bcdHID, 0); KernelLog.Char("H"); KernelLog.Ln;
+	KernelLog.String("   bCountryCode: "); KernelLog.Hex(hd.bCountryCode, 0); KernelLog.Char("H"); KernelLog.Ln;
+	KernelLog.String("   bNumDescriptors: "); KernelLog.Int(hd.bNumDescriptors, 0); KernelLog.Ln;
+	KernelLog.String("   bClassDescriptorType: "); KernelLog.Int(hd.bClassDescriptorType, 0); KernelLog.Ln;
+	KernelLog.String("   wDescriptorLength: "); KernelLog.Int(hd.wDescriptorLength, 0); KernelLog.Ln;
+	KernelLog.String("   Optional descriptors: ");
+	IF (hd.optionalDescriptors = NIL) THEN
+		KernelLog.String("None"); KernelLog.Ln;
+	ELSE
+		FOR i := 0 TO LEN(hd.optionalDescriptors)-1 DO
+			KernelLog.String("      bDescriptorType: "); KernelLog.Int(hd.optionalDescriptors[i].bDescriptorType, 0);
+			KernelLog.String(", wDescriptorLength: "); KernelLog.Int(hd.optionalDescriptors[i].wDescriptorLength, 0);
+			KernelLog.Ln;
+		END;
+	END;
+END ShowHidDescriptor;
+
+END UsbHid.

source/ARM.UsbHidDriver.Mod

@@ -0,0 +1,1612 @@
+MODULE UsbHidDriver;  (** AUTHOR "ottigerm"; PURPOSE "USB HID Parser"; *)
+(**
+ * Bluebottle USB HID Driver
+ *
+ * This driver currently supports:
+ *
+ *	Mouse:		2 axis, 1 mouse wheel, up to 32 buttons
+ *	Keyboard	incl. consumer keys
+ *	Joystick		x, y, z, rx, ry, rz and one slider axis, one coolie hat and arbitrary many buttons
+ *  Touchscreen x,y
+ *
+ * Usage:
+ *	UsbHidDriver.Install ~ load this driver
+ *	SystemTools.Free UsbHidDriver ~ unload this driver
+ *
+ * Remarks:
+ *
+ * References:
+ *	Device Class Definition for Human Interface Devices (HID), Version 1., 27.09.2006, www.usb.org
+ *
+ * History:
+ *	21.04.2006	starting
+ *	22.01.2007	Version 1.0
+ *)
+
+IMPORT
+	SYSTEM, Machine, KernelLog, Modules, Inputs, Usbdi, UsbHid,
+	HidParser := UsbHidParser, UsbHidReport, UsagePage := UsbHidUP, UsbKeyboard,
+	Joystick := Joysticks, Kernel;
+
+CONST
+
+	Name = "UsbHid";
+	Description = "USB HID driver";
+
+	Debug 	= HidParser.Debug;
+	Trace	= HidParser.Trace;
+
+	(* use for logging the reports *)
+	ShowNoReport 			= 0;	(* do not show any reports *)
+	ShowVeryShortReport 	= 1; (* show short and non zero valued reports *)
+	ShowShortReport 		= 2; (* show short reports; only the ids with their assigned value *)
+	ShowFullReport 		= 3; (* show reports including description *)
+
+	LoggingMode = ShowNoReport;
+
+	MouseSpeed = 50;
+	MouseWheelSpeed = 3;
+	MouseAcceleration = 0;
+	
+	
+
+TYPE
+
+	MouseState = POINTER TO RECORD
+		(* mouse msg can hold up to 32 buttons *)
+		buttons:			ARRAY 32 OF UsbHidReport.UsageTuple;
+		(* identifies the last available button *)
+		buttonCount:	LONGINT;
+		buttonReport:	UsbHidReport.HidReport;
+		x:				UsbHidReport.UsageTuple;
+		y:				UsbHidReport.UsageTuple;
+		axisReport:		UsbHidReport.HidReport;
+		wheel:			UsbHidReport.UsageTuple;
+		wheelReport:	UsbHidReport.HidReport;
+		lastDx, lastDy: 	LONGINT;
+	END;
+
+	TouchscreenState = POINTER TO RECORD
+		x,y: UsbHidReport.UsageTuple;
+		minX, maxX, minY, maxY: LONGINT;
+		tipSwitch, inRange, confidence: UsbHidReport.UsageTuple;
+		prev: BOOLEAN;
+		prevx, prevy: LONGINT;
+		prevkeys: SET;
+		prevTip: BOOLEAN;
+		tipTime: LONGINT; 
+	END;
+
+	(*Handling keyboard devices*)
+	KeyboardState = OBJECT(UsbKeyboard.KeyboardBase);
+	VAR
+		modifierUsages: UsbHidReport.PtrToUsageTupleArr;     (*first ref on buffer*)
+		keycodeUsages: UsbHidReport.PtrToUsageTupleArr;
+
+		pressed* : POINTER TO ARRAY OF UsbKeyboard.Key;
+		tempPressed : POINTER TO ARRAY OF UsbKeyboard.Key;
+
+		ledStateChanged : BOOLEAN;
+
+		(*init settings*)
+		PROCEDURE Init;
+		VAR i : ADDRESS; k : ARRAY 32 OF CHAR;
+		BEGIN
+			(* Get  *)
+			Machine.GetConfig("Keyboard", k);
+			i := -1;
+			IF k # "" THEN i := TableFromFile(k); END;
+			IF i = -1 THEN (* Fallback to default *) i := UsbKeyboard.TableUS(); END;
+			SYSTEM.PUT(ADDRESSOF(keytable), i);
+
+			(* Apply Numlock boot up state *)
+			Machine.GetConfig("NumLock", k);
+			IF k[0] = "1" THEN INCL(leds, UsbKeyboard.NumLock) END;
+
+			keyDeadTime := UsbKeyboard.KeyDeadTime DIV 10;
+			keyDeadTimeRepeat := UsbKeyboard.KeyDeadTimeRepeat DIV 10;
+
+			NEW(ledBuffer, 1);
+		END Init;
+
+		(**
+		 * Sets the maximum possible amount of keys, the device is sending at one time
+		 * @param nofkeys: the number of keys maximumely sent by the device
+		 *)
+		PROCEDURE SetMaxKeycodes(nofKeys: LONGINT);
+		BEGIN
+			ASSERT(pressed=NIL);
+			ASSERT(tempPressed=NIL);
+			NEW(pressed,nofKeys);
+			NEW(tempPressed, nofKeys);
+		END SetMaxKeycodes;
+
+		(**
+		 * Handle Keyboard Report
+		 *)
+		PROCEDURE HandleKeyboardEvent;
+		VAR
+			i, j : LONGINT;
+			c : CHAR;
+			flags : SET;
+			found, kill : BOOLEAN;
+		BEGIN
+			(*KernelLog.String('handle key'); KernelLog.Ln;*)
+			(* evaluate modifier keys *)
+			msg.flags := {};
+
+			IF (modifierUsages[0].usageValue=1) THEN INCL(msg.flags, Inputs.LeftCtrl) END;
+			IF (modifierUsages[1].usageValue=1) THEN INCL(msg.flags, Inputs.LeftShift) END;
+			IF (modifierUsages[2].usageValue=1) THEN INCL(msg.flags, Inputs.LeftAlt) END;
+			IF (modifierUsages[3].usageValue=1) THEN INCL(msg.flags, Inputs.LeftMeta) END;
+			IF (modifierUsages[4].usageValue=1) THEN INCL(msg.flags, Inputs.RightCtrl) END;
+			IF (modifierUsages[5].usageValue=1) THEN INCL(msg.flags, Inputs.RightShift) END;
+			IF (modifierUsages[6].usageValue=1) THEN INCL(msg.flags, Inputs.RightAlt) END;
+			IF (modifierUsages[7].usageValue=1) THEN INCL(msg.flags, Inputs.RightMeta) END;
+
+			flags := msg.flags;
+
+			(* evaluate the six keycodes *)
+			FOR i := 2 TO 7 DO
+				c := SYSTEM.VAL(CHAR, keycodeUsages[i-2].usageValue);
+				IF c # CHR(0) THEN (* buffer[i] contains key code *)
+
+					(* check whether the key is pressed for the first time, is still being pressed or has been released *)
+					FOR j := 0 TO 5 DO
+
+						IF pressed[j].ch = c THEN (* key is still pressed *)
+							found := TRUE;
+							pressed[j].updated := TRUE;
+
+							tempPressed[i-2].counter := pressed[j].counter + 1;
+							tempPressed[i-2].ch := pressed[j].ch;
+							tempPressed[i-2].keysym := pressed[j].keysym;
+							tempPressed[i-2].updated := FALSE;
+							tempPressed[i-2].repeat := pressed[j].repeat;
+
+							IF pressed[j].repeat THEN
+								IF (keyDeadTimeRepeat # 0) & (tempPressed[i-2].counter MOD keyDeadTimeRepeat # 0) THEN (* don't send key event *) kill := TRUE; END;
+							ELSE
+								IF tempPressed[i-2].counter MOD keyDeadTime # 0 THEN (* don't send key event *)
+									kill := TRUE;
+								ELSE
+									tempPressed[i-2].repeat := TRUE;
+								END;
+							END;
+						END;
+				    	END;
+				 END;
+
+				IF ~found THEN (* the key has not been pressed down before *)
+					tempPressed[i-2].ch := c;
+					tempPressed[i-2].repeat := FALSE;
+					tempPressed[i-2].updated := FALSE;
+					tempPressed[i-2].counter := 1;
+				END;
+
+			    (* kill : Key is pressed but do not generate key event this time -> repeat rate ... *)
+			    IF (c # CHR(0)) & ~kill THEN
+			    	HandleKey(c);
+			    	tempPressed[i-2].keysym := msg.keysym; (* msg.keysym asigned by HandleKey() ... *)
+			    END;
+			END; (* FOR LOOP *)
+
+			(* update pressed array. generate keyboard.msg's for released keys *)
+			FOR i := 0 TO 5 DO
+				IF (pressed[i].updated = FALSE) & (pressed[i].ch # CHR(0)) THEN (* this key has been released *)
+					msg.flags := {};
+					INCL(msg.flags, Inputs.Release);
+					msg.ch := pressed[i].ch;
+					msg.keysym := pressed[i].keysym;
+					dkHack := deadKey;  (* value of deadKey should persist the key release event *)
+					HandleKey(c);
+					deadKey := dkHack;
+				END;
+				pressed[i].counter := tempPressed[i].counter;
+				pressed[i].ch := tempPressed[i].ch;
+				pressed[i].keysym := tempPressed[i].keysym;
+				pressed[i].repeat := tempPressed[i].repeat;
+				pressed[i].updated := FALSE;
+			END;
+
+			(* Generate events for modifiers *)
+			HandleModifiers(flags);
+
+			(* update status of the LEDs  of the keyboad if necessary *)
+			IF lastLeds # leds THEN (* LED status has changed *)
+				ledBuffer[0] := SYSTEM.VAL(CHAR, leds); lastLeds := leds;
+				ledStateChanged := TRUE;
+			END;
+		END HandleKeyboardEvent;
+	END KeyboardState;
+
+	(* When user presses button, the system has to store the pressed keys in this linked list *)
+	ConsumerKey = POINTER TO RECORD
+		key: LONGINT;
+		usagePage: LONGINT;
+		alive: BOOLEAN;
+		next: ConsumerKey;
+	END;
+
+	(*handling consumer devices*)
+	ConsumerState= OBJECT
+	VAR
+		(*where the consumer report is stored*)
+		consumerReport : UsbHidReport.HidReport;
+		first: ConsumerKey;
+
+		(**
+		 * Checks, whether the usageID with the usagePage is still pressed by the user
+		 * if found, also sets the alive flag to TRUE, such that the clean up method will not destroy it next time
+		 * @param usageID: the usageID pressed
+		 * @param usagePage: normally 0, for detected consumer devices; 9, if consumer send consumer keys as buttons
+		 * @return TRUE, if found, FALSE otherwise
+		 *)
+		PROCEDURE IsSet(usageID, usagePage: LONGINT): BOOLEAN;
+		VAR cursor:	ConsumerKey;
+		BEGIN
+			cursor := first;
+			WHILE(cursor#NIL) DO
+				IF ((cursor.key=usageID) & (cursor.usagePage=usagePage)) THEN
+					cursor.alive := TRUE;
+					RETURN TRUE;
+				END;
+				cursor := cursor.next;
+			END;
+			RETURN FALSE;
+		END IsSet;
+
+		(**
+		 * Adds the tuple.usageID and tuple.usagePage to the linked list
+		 * @param tuple: the tuple to add
+		 *)
+		PROCEDURE AddKey(tuple:UsbHidReport.UsageTuple);
+		VAR cursor: ConsumerKey;
+		BEGIN
+			IF first=NIL THEN
+				NEW(first);
+			ELSE
+				NEW(cursor);
+				cursor.next := first;
+				first := cursor;
+			END;
+			first.key := tuple.usageID;
+			first.usagePage := tuple.usagePage;
+			first.alive := TRUE;
+		END AddKey;
+
+		(**
+		 * destroys all consumerKeys whose alive flag is not set
+		 *)
+		PROCEDURE CleanUp;
+		VAR cursor, previous: ConsumerKey;
+		BEGIN
+			cursor := first;
+			WHILE(cursor#NIL) DO
+				IF cursor.alive = FALSE THEN
+					SendKeySym(cursor.key,cursor.usagePage,FALSE);
+					IF(cursor = first) THEN
+						first := first.next;
+					ELSE
+						previous.next := cursor.next;
+					END;
+				ELSE
+					cursor.alive := FALSE;
+					previous := cursor;
+				END;
+				cursor := cursor.next;
+			END;
+		END CleanUp;
+
+		(**
+		 * Checks whether the keysym is valid, if yes, it generates a keymsg and sends the key to Inputs
+		 * @param usage: the id of the keysym
+		 * @param usagePage: 0: normally, 9: if key is sent as button
+		 *)
+		PROCEDURE SendKeySym(usage, usagePage:LONGINT; pressed:BOOLEAN);
+		VAR keyMsg : Inputs.KeyboardMsg;
+		BEGIN
+			IF Debug THEN
+				KernelLog.String("Handling key");
+			END;
+			IF (usagePage=0) THEN
+				CASE usage OF
+					0B5H: 	keyMsg.keysym := Inputs.KsScanNextTrack; 		(*KernelLog.String("KsScanNextTrack");*)
+					|0B6H: 	keyMsg.keysym := Inputs.KsScanPreviousTrack; 	(*KernelLog.String("KsScanPreviousTrack");*)
+					|0B7H: 	keyMsg.keysym := Inputs.KsStopOSC; 				(*KernelLog.String("KsStopOSC");*)
+					|0CDH: 	keyMsg.keysym := Inputs.KsPlayPause; 			(*KernelLog.String("KsPlayPause");*)
+					|0E2H: 	keyMsg.keysym := Inputs.KsMute; 					(*KernelLog.String("KsMute");*)
+					|0E9H: 	keyMsg.keysym := Inputs.KsVolumeIncrement; 		(*KernelLog.String("KsVolumeIncrement");*)
+					|0EAH: 	keyMsg.keysym := Inputs.KsVolumeDecrement; 	(*KernelLog.String("KsVolumeDecrement");*)
+					|183H: 	keyMsg.keysym := Inputs.KsALConsumerControl; 	(*KernelLog.String("KsALConsumerControl");*)
+					|18AH: 	keyMsg.keysym := Inputs.KsALEmailReader; 		(*KernelLog.String("KsALEmailReader");*)
+					|221H: 	keyMsg.keysym := Inputs.KsACSearch; 				(*KernelLog.String("KsACSearch");*)
+					|223H: 	keyMsg.keysym := Inputs.KsACHome; 				(*KernelLog.String("KsACHome");*)
+					|224H: 	keyMsg.keysym := Inputs.KsACBack; 				(*KernelLog.String("KsACBack");*)
+					|225H: 	keyMsg.keysym := Inputs.KsACForward; 			(*KernelLog.String("KsACForward");*)
+					|22AH: 	keyMsg.keysym := Inputs.KsACBookmarks; 			(*KernelLog.String("KsACBookmarks");*)
+				ELSE
+					IF Trace THEN
+						KernelLog.String("Key Sym "); KernelLog.Hex(usage,0 ); KernelLog.String("H not found"); KernelLog.Ln;
+					END;
+				END;
+			ELSE
+				(*special case: when usagePage Button is used*)
+				IF (usagePage=9H) THEN
+					keyMsg.keysym := Inputs.KsConsumerButtons+usage;
+				END;
+			END;
+			IF (keyMsg.keysym#0) THEN
+				IF ~pressed THEN
+					keyMsg.flags:= {};
+					keyMsg.keysym := Inputs.KsNil;
+					INCL(keyMsg.flags, Inputs.Release);
+				END;
+				IF Debug THEN
+					IF usagePage=0 THEN
+						CASE usage OF
+							0B5H: 	KernelLog.String("KsScanNextTrack");
+							|0B6H: 	KernelLog.String("KsScanPreviousTrack");
+							|0B7H: 	KernelLog.String("KsStopOSC");
+							|0CDH:	KernelLog.String("KsPlayPause");
+							|0E2H: 	KernelLog.String("KsMute");
+							|0E9H: 	KernelLog.String("KsVolumeIncrement");
+							|0EAH: 	KernelLog.String("KsVolumeDecrement");
+							|183H: 	KernelLog.String("KsALConsumerControl");
+							|18AH: 	KernelLog.String("KsALEmailReader");
+							|192H: 	KernelLog.String("KsALCalculator");
+							|221H: 	KernelLog.String("KsACSearch");
+							|223H: 	KernelLog.String("KsACHome");
+							|224H: 	KernelLog.String("KsACBack");
+							|225H: 	KernelLog.String("KsACForward");
+							|22AH: 	KernelLog.String("KsACBookmarks");
+						ELSE
+							KernelLog.String("Key Sym not found"); KernelLog.Ln;
+						END;
+					ELSE
+						IF usagePage=9 THEN
+							KernelLog.String("KsConsumerButtons(");KernelLog.Int(usage,0); KernelLog.String(")");
+						END;
+					END;
+					IF pressed THEN
+						KernelLog.String(" pressed");
+					ELSE
+						KernelLog.String(" released");
+					END;
+					KernelLog.Ln;
+				END;
+				Inputs.keyboard.Handle(keyMsg);
+			END;
+		END SendKeySym;
+
+	END ConsumerState;
+
+	(*handle joystick devices*)
+	JoystickState = POINTER TO RECORD
+		(*use the joystick as a mouse*)
+		(*mouse msg can hold up to 32 buttons*)
+		buttons:			ARRAY 32 OF UsbHidReport.UsageTuple;
+		(*identifies the last available button*)
+		buttonCount:	LONGINT;
+		buttonReport:	UsbHidReport.HidReport;
+		x:				UsbHidReport.UsageTuple;
+		y:				UsbHidReport.UsageTuple;
+		z:				UsbHidReport.UsageTuple;
+		rx:				UsbHidReport.UsageTuple;
+		ry:				UsbHidReport.UsageTuple;
+		rz:				UsbHidReport.UsageTuple;
+		slider:			UsbHidReport.UsageTuple;
+		hatSwitch:		UsbHidReport.UsageTuple;
+
+		xReport:			UsbHidReport.HidReport;
+		yReport:			UsbHidReport.HidReport;
+		zReport:			UsbHidReport.HidReport;
+		rxReport:			UsbHidReport.HidReport;
+		ryReport:			UsbHidReport.HidReport;
+		rzReport:			UsbHidReport.HidReport;
+		sliderReport:		UsbHidReport.HidReport;
+		hatSwitchReport:	UsbHidReport.HidReport;
+
+		joystick:			Joystick.Joystick;
+	END;
+
+	(*the hid driver*)
+	HidDriver*= OBJECT (UsbHid.HidDriver);
+	VAR
+		(*itemParser is responsible for parsing the usb hid report descriptor*)
+		itemParser 	: HidParser.ItemParser;
+		endpoint	: LONGINT;
+		pipe 		: Usbdi.Pipe;
+
+		(*where the report interrupt in report is stored*)
+		reportBuffer				: Usbdi.BufferPtr;
+		reportManager			: UsbHidReport.HidReportManager;
+		hidReportItemQueue	: UsbHidReport.ReportItemQueue;
+		mouseState				: MouseState;
+		touchscreenState : TouchscreenState;
+		keyboardState			: KeyboardState;
+		consumerState			: ConsumerState;
+		joystickState			: JoystickState;
+		useReportIDMechanism	: BOOLEAN;
+		
+		recentStatus: Usbdi.Status;
+
+		(*
+		 * This procedure is called by the USB system software after an instance of this object has been passed to it via the probe procedure.
+		 * Typically, the code here sets up the communication pipe(s) use by the driver using device.GetPipe(endpointnumber).
+		 *)
+		PROCEDURE Connect() : BOOLEAN;
+		VAR
+			hidDescriptor 		: UsbHid.HidDescriptor;
+			i					: LONGINT;
+			reportDescBuffer 	: Usbdi.BufferPtr;
+			status 				: Usbdi.Status;
+			canManage			: BOOLEAN;
+		BEGIN
+			(*TestReader;*)
+			(*parse the hid report descriptor*)
+			NEW(itemParser);
+			(*get interface descriptor*)
+
+			hidDescriptor := GetHidDescriptor();
+			IF (hidDescriptor = NIL) THEN
+				RETURN FALSE;
+			END;
+
+			IF Debug THEN UsbHid.ShowHidDescriptor(hidDescriptor);	END;
+
+			NEW(reportDescBuffer, hidDescriptor.wDescriptorLength);
+			IF ~GetDescriptor(hidDescriptor.bClassDescriptorType,  0, interface.bInterfaceNumber , hidDescriptor.wDescriptorLength, reportDescBuffer) THEN
+				KernelLog.String("    Could not get reportDescriptor"); KernelLog.Ln;
+				RETURN FALSE;
+			ELSE
+				IF Debug THEN
+					(*print all all bytes of the reportDescBuffer*)
+					LayoutBuffer(reportDescBuffer, hidDescriptor.wDescriptorLength);
+				END;
+			END;
+			IF(~itemParser.ParseReportDescriptor(hidDescriptor, reportDescBuffer)) THEN
+				IF Debug THEN KernelLog.String("    Could not parse Report Descriptor correctly"); KernelLog.Ln; END;
+			END;
+			IF Trace THEN
+				(*there are cases, where the report descriptor is not set correctly, but it can be used with this errors.*)
+				itemParser.errorList.PrintAll;
+			END;
+
+			(*get reportManager and hidReportItemQueue*)
+			reportManager := itemParser.GetReportManager();
+			hidReportItemQueue := reportManager.GetReportItemQueue();
+
+			LOOP
+				IF i >= LEN(interface.endpoints) THEN EXIT; END;
+				IF (interface.endpoints[i].type = Usbdi.InterruptIn)  THEN
+					endpoint := SHORT(SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, interface.endpoints[0].bEndpointAddress) * {0,1,2,3,7}));
+					EXIT;
+				END;
+				INC(i);
+			END;
+
+			IF endpoint = 0 THEN
+				IF Debug THEN KernelLog.String("UsbMouse: No interrupt IN endpoint found."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			pipe := device.GetPipe(endpoint);
+			IF pipe = NIL THEN RETURN FALSE END;
+
+			IF InitializeTouchscreenDriver()=TRUE THEN
+				canManage := TRUE;
+			ELSIF InitializeMouseDriver() = TRUE THEN
+				canManage := TRUE;
+			END;
+
+			IF InitializeKeyboardDriver()=TRUE THEN
+				canManage := TRUE;
+			END;
+
+			IF InitializeConsumerDriver() = TRUE THEN
+				canManage := TRUE;
+			END;
+
+			IF InitializeJoystickDriver()=TRUE THEN
+				canManage := TRUE;
+			END;
+
+
+			IF (canManage) THEN
+
+				useReportIDMechanism	:= reportManager.UsesReportIDMechanism();
+
+				NEW(reportBuffer, pipe.maxPacketSize);
+
+				pipe.SetTimeout(0);
+				pipe.SetCompletionHandler(HandleHidEvent);
+
+				status := pipe.Transfer(pipe.maxPacketSize, 0, reportBuffer);
+				recentStatus := status;
+				IF Debug THEN
+					KernelLog.String("UsbHidDriver.HidDriver.Connect: Connect successfully finished"); KernelLog.Ln;
+				END;
+		(*	ELSE  NOT SUPPORTED YET, because the whole driver is removed but not only the interface
+				RETURN FALSE; *)
+			END;
+
+			RETURN TRUE;
+		END Connect;
+
+		
+
+		(*called when detaching a usb hid device*)
+		PROCEDURE Disconnect;
+		BEGIN
+			itemParser.Disconnect();
+			itemParser:=NIL;
+			(*joystick*)
+			IF (joystickState#NIL) THEN
+				IF joystickState.joystick#NIL THEN
+					Joystick.registry.Remove(joystickState.joystick);
+				END;
+			END;
+			IF Debug OR Trace THEN KernelLog.String("USB HID Device disconnected."); KernelLog.Ln; END;
+		END Disconnect;
+
+		(*is always called, when new report arrived*)
+		PROCEDURE HandleHidEvent(status : Usbdi.Status; actLen : LONGINT);
+		VAR
+			ri			: UsbHidReport.ReportItem;
+			i, bitIndex	: LONGINT;
+			reportID	: LONGINT;
+			res			: BOOLEAN;
+			usageTuple	: UsbHidReport.UsageTuple;
+
+			(*
+			* update the reportManager with the newest values
+			*)
+			PROCEDURE HandleReportItem;
+			BEGIN
+				FOR i:=0 TO (ri.reportItemCount-1) DO
+					IF(ri.values=NIL) THEN
+						(*there are no values to read, because the reportItem describes a constant field*)
+					ELSE
+						IF Debug THEN
+							KernelLog.String("HandleHidEvent: Reading..."); KernelLog.Ln;
+						END;
+						usageTuple := ri.values[i];
+						usageTuple.usageValue := ReadBits(bitIndex,ri.reportItemSize);
+						IF (LoggingMode=ShowShortReport) OR (LoggingMode=ShowVeryShortReport) THEN
+							IF((LoggingMode=ShowShortReport)OR(ri.values[i].usageValue#0)) THEN
+								KernelLog.String("usageValue for usageID ");
+								KernelLog.Int(ri.values[i].usageID,0);
+								KernelLog.String(" is: ");
+								KernelLog.Int(ri.values[i].usageValue,0);
+								KernelLog.Ln;
+							END;
+						END;
+					END;
+
+					bitIndex := bitIndex + ri.reportItemSize;
+				END;
+			END HandleReportItem;
+		BEGIN
+			(*start reportParsing*)
+
+			IF Debug THEN
+				IF((hidReportItemQueue=NIL) OR (reportManager=NIL)) THEN
+					KernelLog.String("UsbHidDriver:HidDriver.HandleHidEvent: Internal Error,hidReportItemQueue or reportManager not found"); KernelLog.Ln;
+				END;
+				LayoutBuffer(reportBuffer,actLen);
+			END;
+
+			(*fill up report buffer with new values*)
+			ri := hidReportItemQueue.first;
+
+			IF(ri=NIL) THEN
+				KernelLog.String("ri=NIL"); KernelLog.Ln;
+				RETURN;
+			END;
+			(*index in the reportBuffer*)
+			bitIndex := 0;
+			IF (useReportIDMechanism) THEN
+				reportID := ReadBits(0, 8);
+				bitIndex := bitIndex + 8;
+
+				WHILE(ri#NIL) DO
+					IF(ri.reportID=reportID) THEN
+						HandleReportItem;
+					END;
+					ri := ri.next;
+				END;
+			ELSE
+				WHILE(ri#NIL) DO
+					HandleReportItem;
+					ri := ri.next;
+				END;
+			END;
+
+			IF LoggingMode=ShowFullReport THEN reportManager.PrintReportState END;
+
+			IF (  (status = Usbdi.Ok) OR (status=Usbdi.ShortPacket)) THEN
+				recentStatus := status;
+				IF(mouseState#NIL) THEN
+					IF Debug THEN KernelLog.String("handle mouse driver"); KernelLog.Ln; END;
+					HandleMouseDriver;
+				END;
+
+				IF(keyboardState#NIL) THEN
+					IF Debug  THEN  KernelLog.String("handle keyboard driver"); KernelLog.Ln; END;
+					HandleKeyboardDriver;
+					(* update status of the LEDs  of the keyboad if necessary *)
+					IF keyboardState.lastLeds # keyboardState.leds THEN (* LED status has changed *)
+						keyboardState.ledBuffer[0] := SYSTEM.VAL(CHAR, keyboardState.leds); keyboardState.lastLeds := keyboardState.leds;
+						res := SetReport(UsbHid.ReportOutput, 0, keyboardState.ledBuffer, 1); (* ignore res *)
+					END;
+				END;
+
+				IF(consumerState#NIL) THEN
+					IF Debug THEN KernelLog.String("handle consumer driver"); KernelLog.Ln; END;
+					HandleConsumerDriver;
+				END;
+
+				IF(joystickState#NIL) THEN
+					IF Debug THEN KernelLog.String("handle custom driver"); KernelLog.Ln; END;
+					HandleJoystickDriver;
+				END;
+
+				IF touchscreenState #NIL THEN
+					IF Debug  THEN  KernelLog.String("handle touchscreen driver"); KernelLog.Ln; END;
+					HandleTouchscreenDriver
+				END;
+
+				(*get new message from hid device*)
+				status := pipe.Transfer(pipe.maxPacketSize, 0, reportBuffer);
+			ELSE
+				IF Debug THEN
+					KernelLog.String("UsbHidDriver: "); KernelLog.String(name); KernelLog.String("("); KernelLog.String(desc); KernelLog.String(")");
+					KernelLog.String(" has been disabled."); KernelLog.Ln;
+				END;
+				IF (status = Usbdi.Stalled) THEN
+					IF pipe.ClearHalt() THEN
+						IF Debug THEN KernelLog.String("UsbHidDriver: Stall on Interrupt Pipe cleared."); KernelLog.Ln; END;
+						status := pipe.Transfer(pipe.maxPacketSize, 0, reportBuffer); (* ignore status *)
+					ELSE
+						IF Debug THEN KernelLog.String("UsbHidDriver: Couldn't clear stall on interrupt pipe. Abort."); KernelLog.Ln; END;
+						device.FreePipe(pipe);
+					END;
+				END;
+				recentStatus := status;
+			END;
+		END HandleHidEvent;
+
+		PROCEDURE IsTouchscreen*(): BOOLEAN;
+		BEGIN
+			RETURN touchscreenState # NIL
+		END IsTouchscreen;
+		
+		PROCEDURE GetPipeStatus*(): Usbdi.Status;
+		VAR len: LONGINT;
+		BEGIN
+			IF pipe # NIL THEN RETURN pipe.GetStatus(len) 
+			ELSE RETURN Usbdi.Error
+			END;
+		END GetPipeStatus;
+		
+		
+		PROCEDURE GetRecentStatus*(): Usbdi.Status;
+		BEGIN
+			RETURN recentStatus;
+		END GetRecentStatus;
+		
+		
+		(**
+		 * Is called by handleHidEvent when mouse device is found
+		 *)
+		PROCEDURE HandleMouseDriver;
+		VAR
+			mm : 			Inputs.MouseMsg;
+			dx, dy,i :		LONGINT;
+			accelX, accelY : 	REAL;
+		BEGIN
+			dx := TwosComplement(mouseState.x.usageValue, mouseState.axisReport.reportSize);
+			dy := TwosComplement(mouseState.y.usageValue, mouseState.axisReport.reportSize);
+			IF Debug THEN KernelLog.String("x and y are: ");KernelLog.Int(dx,0); KernelLog.String(" and ");KernelLog.Int(dy,0); KernelLog.Ln; END;
+
+			accelX := 1.0 + ABS(dx - mouseState.lastDx) / 128 * MouseAcceleration;
+			accelY := 1.0 + ABS(dy - mouseState.lastDy) / 128 * MouseAcceleration;
+
+			mouseState.lastDx := dx;
+			mouseState.lastDy := dy;
+			(*KernelLog.String("X: "); KernelLog.Int(dx,0); KernelLog.String("  Y:"); KernelLog.Int(dy,0); KernelLog.Ln;*)
+			mm.dx := ENTIER(MouseSpeed / 50.0 *  dx * accelX);
+			mm.dy := ENTIER(MouseSpeed / 50.0 * dy * accelY);
+
+			IF (mouseState.wheel#NIL) THEN
+				mm.dz := - TwosComplement(mouseState.wheel.usageValue, mouseState.wheelReport.reportSize);
+				IF mm.dz < 0 THEN mm.dz := - MouseWheelSpeed;
+				ELSIF mm.dz>0 THEN mm.dz := + MouseWheelSpeed;
+				END;
+			END;
+
+			IF (mouseState.buttons[0].usageValue#0) THEN mm.keys := mm.keys + {0}; END;
+			IF (mouseState.buttons[1].usageValue#0) THEN mm.keys := mm.keys + {2}; END;
+			IF (mouseState.buttons[2].usageValue#0) THEN mm.keys := mm.keys + {1}; END;
+
+			FOR i:=3 TO 31 DO
+				IF (mouseState.buttons[i]#NIL) THEN
+					IF (mouseState.buttons[i].usageValue#0) THEN mm.keys := mm.keys + {i}; END;
+				END;
+			END;
+			Inputs.mouse.Handle(mm);
+		END HandleMouseDriver;
+
+		(**
+		 * Is called by handleHidEvent when mouse device is found
+		 *)
+		PROCEDURE HandleTouchscreenDriver;
+		VAR
+			mm : 			Inputs.AbsMouseMsg;
+			x,y: LONGINT; 
+			displayWidth, displayHeight: LONGINT;
+		BEGIN
+			getDisplayDimensions(displayWidth, displayHeight); 
+			x := (touchscreenState.x.usageValue-touchscreenState.minX) * displayWidth DIV (touchscreenState.maxX-touchscreenState.minX +1);
+			y := (touchscreenState.y.usageValue-touchscreenState.minY) * displayHeight DIV  (touchscreenState.maxY -touchscreenState.minY + 1);
+			
+			(*
+			TRACE(touchscreenState.inRange.usageValue);
+			IF touchscreenState.confidence # NIL THEN 
+				TRACE(touchscreenState.confidence.usageValue);
+			END;
+			*)
+			
+			IF (touchscreenState.tipSwitch.usageValue#0) THEN 
+				IF (touchConfidenceDelay#0) & ~touchscreenState.prevTip THEN
+					touchscreenState.tipTime := Kernel.GetTicks();
+					touchscreenState.prevTip := TRUE
+				ELSIF (touchConfidenceDelay =0) OR (Kernel.GetTicks()-touchscreenState.tipTime > touchConfidenceDelay) THEN
+					mm.keys := mm.keys + {0}; 
+				END
+			ELSE
+				touchscreenState.prevTip := FALSE;
+			END;
+			
+			
+			mm.x := x; mm.y := y;
+
+			IF ~(0 IN mm.keys) THEN (* up event  *)
+				touchscreenState.prev := FALSE;
+				(* 
+					There are touchscreen devices that report a (0,0) coordinate with up-events. 
+					This does not work well together with an external mouse.
+					We keep the coordinate for the time being -- with the side effect that there may be a pointer-over status after a touchscreen click
+				*)
+				IF 0 IN touchscreenState.prevkeys THEN
+					mm.x := touchscreenState.prevx;
+					mm.y := touchscreenState.prevy;
+					touchscreenState.prevkeys := mm.keys;
+					Inputs.mouse.Handle(mm);
+				END;
+			ELSIF (0 IN mm.keys) THEN (* down event or pointer move *)
+				IF ~touchscreenState.prev THEN
+					(* artificial initial up event in order to set mouse coordinates like with a "real" mouse before sending the down event *)
+					touchscreenState.prev := TRUE;
+					EXCL(mm.keys, 0);
+					Inputs.mouse.Handle(mm);
+				END;
+				(* down event or pointer event *)
+				touchscreenState.prevx := x;
+				touchscreenState.prevy := y;
+				touchscreenState.prevkeys := mm.keys;
+				Inputs.mouse.Handle(mm);
+			END;
+		END HandleTouchscreenDriver;
+
+		(**
+		 * Is called by handleHidEvent when keyboard device is found
+		 *)
+		PROCEDURE HandleKeyboardDriver;
+		VAR res : BOOLEAN;
+		BEGIN
+			keyboardState.HandleKeyboardEvent();
+			IF keyboardState.ledStateChanged THEN
+				res := SetReport(UsbHid.ReportOutput, 0, keyboardState.ledBuffer, 1); (* ignore res *)
+				keyboardState.ledStateChanged := FALSE;
+			END;
+		END HandleKeyboardDriver;
+
+		(**
+		 * Is called by handleHidEvent when consumer device is found
+		 *)
+		PROCEDURE HandleConsumerDriver;
+		VAR
+			temp: 		UsbHidReport.HidReport;
+			usageTuple:	UsbHidReport.UsageTuple;
+			i: 			LONGINT;
+			dictUsage:	LONGINT;
+		BEGIN
+			temp := consumerState.consumerReport;
+			WHILE(temp#NIL) DO
+				IF temp.usages # NIL THEN
+					FOR i:=0 TO temp.reportCount-1 DO
+						IF(temp.usages[i].usageValue#0)THEN
+							IF HidParser.IDMainIsVariable IN SYSTEM.VAL(SET,temp.mainState) THEN
+								IF Debug THEN
+									KernelLog.Int(temp.usages[i].usageID,0);
+									KernelLog.String(" ("); UsagePage.PrintUsagePage(UsagePage.ConsumerPage, temp.usages[i].usageID);
+								END;
+								IF(consumerState.IsSet(temp.usages[i].usageID,temp.usages[i].usagePage)=FALSE) THEN
+									consumerState.AddKey(temp.usages[i]);
+									consumerState.SendKeySym(temp.usages[i].usageID, temp.usages[i].usagePage, TRUE);
+								END;
+							ELSE
+								(*the data is sent in an array*)
+								IF UsbHidReport.UseUsageDictionaryExt THEN
+									IF Debug THEN
+										KernelLog.String("-> usage dictionary  index: "); KernelLog.Int(temp.usages[i].usageValue,0); KernelLog.String(";     ");
+									END;
+									usageTuple := reportManager.GetDictKey(temp.usages[i].usageValue-temp.logicalMinimum, temp.supportedUsages);
+									(*dictUsage := usageTuple.usageID;
+									KernelLog.String("usageID is "); KernelLog.Int(usageTuple.usagePage,0); KernelLog.String("  "); KernelLog.Hex(dictUsage,0); KernelLog.Ln;*)
+									IF (consumerState.IsSet(usageTuple.usageID,usageTuple.usagePage)=FALSE) THEN
+										consumerState.AddKey(usageTuple);
+										consumerState.SendKeySym(usageTuple.usageID, usageTuple.usagePage, TRUE);
+									END;
+									IF Debug THEN
+										KernelLog.Int(dictUsage,0);
+										KernelLog.String(" (");
+										IF usageTuple.usagePage # 0 THEN
+											UsagePage.PrintUsagePage(usageTuple.usagePage, dictUsage);
+										ELSE
+											UsagePage.PrintUsagePage(UsagePage.ConsumerPage, dictUsage);
+										END;
+									END;
+								ELSE
+									IF Debug THEN
+										KernelLog.Int(temp.usages[i].usageValue,0);
+										KernelLog.String(" ("); UsagePage.PrintUsagePage(UsagePage.ConsumerPage, temp.usages[i].usageValue);
+									END;
+									IF (consumerState.IsSet(temp.usages[i].usageValue,temp.usages[i].usagePage)=FALSE) THEN
+										consumerState.AddKey(temp.usages[i]);
+										consumerState.SendKeySym(temp.usages[i].usageValue,temp.usages[i].usagePage, TRUE);
+									END;
+								END;
+							END;
+							IF Debug THEN KernelLog.String(") pressed."); KernelLog.Ln; END;
+						END;
+					END;
+				END;
+				temp := temp.next;
+			END;
+			consumerState.CleanUp;
+		END HandleConsumerDriver;
+
+		(**
+		 * Is called by handleHidEvent when joystick device is found
+		 *)
+		 PROCEDURE HandleJoystickDriver;
+		VAR
+			msg : 		Joystick.JoystickDataMessage;
+			i:			LONGINT;
+		BEGIN
+			FOR i:=0 TO joystickState.buttonCount-1 DO
+				IF (joystickState.buttons[i].usageValue#0) THEN
+					msg.buttons := msg.buttons + {joystickState.buttons[i].usageID-1};
+				END;
+			END;
+
+			IF joystickState.x # NIL THEN
+				IF joystickState.xReport.logicalMinimum<0 THEN
+					msg.axis[Joystick.AxisX] := TwosComplement(joystickState.x.usageValue,joystickState.xReport.reportSize);
+				ELSE
+					msg.axis[Joystick.AxisX] := joystickState.x.usageValue;
+				END;
+			END;
+
+			IF joystickState.y # NIL THEN
+				IF joystickState.yReport.logicalMinimum<0 THEN
+					msg.axis[Joystick.AxisY] := TwosComplement(joystickState.y.usageValue,joystickState.yReport.reportSize);
+				ELSE
+					msg.axis[Joystick.AxisY] := joystickState.y.usageValue;
+				END;
+			END;
+
+			IF joystickState.z # NIL THEN
+				IF joystickState.zReport.logicalMinimum<0 THEN
+					msg.axis[Joystick.AxisZ] := TwosComplement(joystickState.z.usageValue,joystickState.zReport.reportSize);
+				ELSE
+					msg.axis[Joystick.AxisZ] := joystickState.z.usageValue;
+				END;
+			END;
+
+			IF joystickState.rx # NIL THEN
+				IF joystickState.rxReport.logicalMinimum<0 THEN
+					msg.axis[Joystick.AxisRx] := TwosComplement(joystickState.rx.usageValue,joystickState.rxReport.reportSize);
+				ELSE
+					msg.axis[Joystick.AxisRx] := joystickState.rx.usageValue;
+				END;
+			END;
+
+			IF joystickState.ry # NIL THEN
+				IF joystickState.ryReport.logicalMinimum<0 THEN
+					msg.axis[Joystick.AxisRy] := TwosComplement(joystickState.ry.usageValue,joystickState.ryReport.reportSize);
+				ELSE
+					msg.axis[Joystick.AxisRy] := joystickState.ry.usageValue;
+				END;
+			END;
+
+			IF joystickState.rz # NIL THEN
+				IF joystickState.rzReport.logicalMinimum<0 THEN
+					msg.axis[Joystick.AxisRz] := TwosComplement(joystickState.rz.usageValue,joystickState.rzReport.reportSize);
+				ELSE
+					msg.axis[Joystick.AxisRz] := joystickState.rz.usageValue;
+				END;
+			END;
+
+			IF joystickState.slider # NIL THEN
+				IF joystickState.sliderReport.logicalMinimum<0 THEN
+					msg.axis[Joystick.Slider1] := TwosComplement(joystickState.slider.usageValue,joystickState.sliderReport.reportSize);
+				ELSE
+					msg.axis[Joystick.Slider1] := joystickState.slider.usageValue;
+				END;
+			END;
+
+			IF joystickState.hatSwitch # NIL THEN
+				CASE (joystickState.hatSwitch.usageValue-joystickState.hatSwitchReport.logicalMinimum+1) OF
+					1:	msg.coolieHat[0]:= {Joystick.HatUp};
+					|2: 	msg.coolieHat[0]:= {Joystick.HatUp}+{Joystick.HatLeft};
+					|3:	msg.coolieHat[0]:= {Joystick.HatLeft};
+					|4:	msg.coolieHat[0]:= {Joystick.HatLeft}+{Joystick.HatDown};
+					|5:	msg.coolieHat[0]:= {Joystick.HatDown};
+					|6:	msg.coolieHat[0]:= {Joystick.HatDown}+{Joystick.HatRight};
+					|7:	msg.coolieHat[0]:= {Joystick.HatRight};
+					|8:	msg.coolieHat[0]:= {Joystick.HatRight}+{Joystick.HatUp};
+				ELSE
+
+				END;
+			END;
+
+			joystickState.joystick.Handle(msg);
+		END HandleJoystickDriver;
+
+		(**
+		 * checks, whether the device sends mouse informations or not
+		 *)
+		PROCEDURE InitializeMouseDriver():BOOLEAN;
+		VAR
+			mouseCollection	: UsbHidReport.HidCollection;
+			temp			: UsbHidReport.HidReport;
+			i				: LONGINT;
+			isReportProtocol: BOOLEAN;
+		BEGIN
+			(*get mouse collection: mouse collection uses UsagePage(Generic Desktop Controlsl)->1 and Usage(Mouse)->2*)
+			mouseCollection := reportManager.GetCollection(1,2);
+			IF (mouseCollection#NIL) THEN
+				NEW(mouseState);
+				mouseState.buttonCount := 0;
+				FOR i:=0 TO 31 DO
+					IF( mouseState.buttonReport = NIL) THEN
+						mouseState.buttons[i] := reportManager.GetUsage(UsagePage.ButtonPage, i+1, mouseCollection,mouseState.buttonReport);
+					ELSE
+						mouseState.buttons[i] := reportManager.GetUsage(UsagePage.ButtonPage, i+1, mouseCollection,temp);
+					END;
+					IF(mouseState.buttons[i]#NIL) THEN
+						mouseState.buttonCount := i;
+					END;
+					(*KernelLog.String("mouseState.buttons ["); KernelLog.Int(i,0); KernelLog.String("] is ");
+					KernelLog.Int(SYSTEM.VAL(LONGINT, mouseState.buttons[i]),0); KernelLog.Ln;*)
+				END;
+				mouseState.x := reportManager.GetUsage(UsagePage.GenericDesktopPage, 30H, mouseCollection,mouseState.axisReport);
+				IF(mouseState.x=NIL) THEN
+					KernelLog.String("Initialize mouse driver: error did not find x axis"); KernelLog.Ln;
+				END;
+				IF(mouseState.axisReport=NIL) THEN
+					KernelLog.String("InitializeMouseDriver: Error: Did not find axis report"); KernelLog.Ln;
+				END;
+				mouseState.y 		:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 31H, mouseCollection,temp);
+				IF(mouseState.y=NIL) THEN
+					KernelLog.String("Initialize mouse driver: error did not find y axis"); KernelLog.Ln;
+				END;
+				mouseState.wheel	:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 38H, mouseCollection,mouseState.wheelReport);
+				IF(mouseState.wheel=NIL) THEN
+					KernelLog.String("Initialize mouse driver: warning did not find wheel"); KernelLog.Ln;
+				END;
+				IF Trace THEN
+					KernelLog.String("Found Mouse Configuration"); KernelLog.Ln;
+					KernelLog.String("Mouse Driver initialized");KernelLog.Ln;
+				END;
+				isReportProtocol := SetReportProtocol();
+				RETURN TRUE;
+			ELSE
+				RETURN FALSE;
+			END;
+		END InitializeMouseDriver;
+
+		(**
+		 * checks, whether the device sends keyboard informations or not
+		 *)
+		PROCEDURE InitializeKeyboardDriver():BOOLEAN;
+		VAR
+			keyboardColl: UsbHidReport.HidCollection;
+			aUsageTuple : UsbHidReport.UsageTuple;
+			modifierReport: UsbHidReport.HidReport;
+			keycodeReport: UsbHidReport.HidReport;
+		BEGIN
+			keyboardColl:= reportManager.GetCollection(UsagePage.GenericDesktopPage,UsagePage.KeyboardPage);
+			IF(keyboardColl#NIL) THEN
+				IF ~SetIdle(0,10) THEN
+					IF Debug THEN KernelLog.String("UsbKeyboard: Error: Cannot set idle the keyboard."); KernelLog.Ln; END;
+				END;
+				NEW(keyboardState);
+				keyboardState.Init;
+
+				aUsageTuple:= reportManager.GetUsage(UsagePage.KeypadPage, 224,
+						keyboardColl, modifierReport);
+				IF (modifierReport=NIL) THEN
+					IF Debug THEN KernelLog.String("Error did not find modifier"); KernelLog.Ln; END;
+					keyboardState := NIL;
+					RETURN FALSE;
+				ELSE
+					IF (modifierReport.usages=NIL) THEN
+						IF Debug THEN KernelLog.String("Error did not find modifiers usages"); KernelLog.Ln; END;
+						keyboardState := NIL;
+						RETURN FALSE;
+					ELSE
+						IF (LEN(modifierReport.usages)<8) THEN
+							keyboardState := NIL;
+							RETURN FALSE;
+						END;
+					END;
+					keyboardState.modifierUsages := modifierReport.usages;
+				END;
+
+				(*assume that the keycodes always begin at usage 0*)
+				aUsageTuple:= reportManager.GetUsage(UsagePage.KeypadPage, 0,
+						keyboardColl, keycodeReport);
+				IF(keycodeReport=NIL) THEN
+					IF Debug THEN KernelLog.String("Error did not find keycodeReport"); KernelLog.Ln; END;
+					keyboardState := NIL;
+					RETURN FALSE;
+				ELSE
+					IF(keycodeReport.usages=NIL) THEN
+						keyboardState := NIL;
+						IF Debug THEN KernelLog.String("Error did not find keycodeReports usages"); KernelLog.Ln; END;
+						RETURN FALSE;
+					(*ELSE
+						IF (LEN(modifierReport.usages)<8) THEN
+							keyboardState := NIL;
+							RETURN FALSE;
+						END;*)
+					END;
+				END;
+				keyboardState.keycodeUsages := keycodeReport.usages;
+				keyboardState.SetMaxKeycodes(LEN(keycodeReport.usages));
+				RETURN SetReportProtocol();
+			ELSE
+				RETURN FALSE;
+			END;
+		END InitializeKeyboardDriver;
+
+		(**
+		 * checks, whether the device sends consumer informations or not
+		 *)
+		PROCEDURE InitializeConsumerDriver():BOOLEAN;
+		VAR
+			consumerColl	: UsbHidReport.HidCollection;
+			temp			: UsbHidReport.HidReport;
+			usageCounter	: LONGINT;
+		BEGIN
+			consumerColl := reportManager.GetCollection(UsagePage.ConsumerPage, 1H);
+			IF consumerColl # NIL THEN
+				NEW(consumerState);
+				consumerState.consumerReport := consumerColl.firstReport;
+				IF consumerState.consumerReport # NIL THEN
+					temp := consumerState.consumerReport;
+					WHILE (temp # NIL) DO
+							usageCounter := usageCounter + temp.reportCount;
+						temp := temp.next;
+					END;
+					temp := consumerState.consumerReport;
+					RETURN TRUE;
+				ELSE
+					consumerState := NIL;
+				END;
+			END;
+			RETURN FALSE;
+		END InitializeConsumerDriver;
+
+		(**
+		 * checks, whether the device sends joystick informations or not
+		 *)
+		 PROCEDURE InitializeJoystickDriver():BOOLEAN;
+		VAR
+			joystickColl		: UsbHidReport.HidCollection;
+			temp			: UsbHidReport.HidReport;
+			res,i			: LONGINT;
+		BEGIN
+			(*get joystick collection: joystick collection uses UsagePage(Generic Desktop Controlsl)->1 and Usage(Joystick)->4*)
+			joystickColl := reportManager.GetCollection(UsagePage.GenericDesktopPage,4);
+			IF (joystickColl#NIL) THEN
+				NEW(joystickState);
+				joystickState.buttonCount := 0;
+				FOR i:=0 TO 31 DO
+					IF( joystickState.buttonReport = NIL) THEN
+						joystickState.buttons[i] := reportManager.GetUsage(UsagePage.ButtonPage, i+1, joystickColl,joystickState.buttonReport);
+						(*KernelLog.String("Found button report"); KernelLog.Ln;*)
+					ELSE
+						joystickState.buttons[i] := reportManager.GetUsage(UsagePage.ButtonPage, i+1, joystickColl,temp);
+					END;
+					IF(joystickState.buttons[i]#NIL) THEN
+						joystickState.buttonCount := joystickState.buttonCount +1;
+						(*KernelLog.String(" button ");
+						KernelLog.Int(joystickState.buttonReport.usages[i].usageID,0);  KernelLog.Ln;*)
+					END;
+				END;
+
+				IF Debug THEN
+					KernelLog.String("Found Joystick Configuration"); KernelLog.Ln;
+				END;
+
+				NEW(joystickState.joystick,joystickState.buttonCount);
+				joystickState.joystick.desc := "USBHIDJoystick";
+
+				joystickState.x := reportManager.GetUsage(UsagePage.GenericDesktopPage, 30H, joystickColl,joystickState.xReport);
+				IF(joystickState.x#NIL) THEN
+					joystickState.joystick.AddAxis(Joystick.AxisX, joystickState.xReport.logicalMinimum, joystickState.xReport.logicalMaximum);
+				END;
+
+				joystickState.y 		:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 31H, joystickColl,joystickState.yReport);
+				IF(joystickState.y#NIL) THEN
+					joystickState.joystick.AddAxis(Joystick.AxisY, joystickState.yReport.logicalMinimum, joystickState.yReport.logicalMaximum);
+				END;
+
+				joystickState.z 		:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 32H, joystickColl,joystickState.zReport);
+				IF(joystickState.z#NIL) THEN
+					joystickState.joystick.AddAxis(Joystick.AxisZ, joystickState.zReport.logicalMinimum, joystickState.zReport.logicalMaximum);
+				END;
+
+				joystickState.rx 		:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 33H, joystickColl,joystickState.rxReport);
+				IF(joystickState.rx#NIL) THEN
+					joystickState.joystick.AddAxis(Joystick.AxisRx, joystickState.rxReport.logicalMinimum, joystickState.rxReport.logicalMaximum);
+				END;
+
+				joystickState.ry 		:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 34H, joystickColl,joystickState.ryReport);
+				IF(joystickState.ry#NIL) THEN
+					joystickState.joystick.AddAxis(Joystick.AxisRy, joystickState.ryReport.logicalMinimum, joystickState.ryReport.logicalMaximum);
+				END;
+
+				joystickState.rz 		:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 35H, joystickColl,joystickState.rzReport);
+				IF(joystickState.rz#NIL) THEN
+					joystickState.joystick.AddAxis(Joystick.AxisRz, joystickState.rzReport.logicalMinimum, joystickState.rzReport.logicalMaximum);
+				END;
+
+				joystickState.slider	:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 36H, joystickColl,joystickState.sliderReport);
+				IF(joystickState.slider#NIL) THEN
+					joystickState.joystick.AddAxis(Joystick.Slider1, joystickState.sliderReport.logicalMinimum, joystickState.sliderReport.logicalMaximum);
+				END;
+
+				joystickState.hatSwitch:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 39H, joystickColl,joystickState.hatSwitchReport);
+				IF(joystickState.hatSwitch#NIL) THEN
+					IF (joystickState.hatSwitchReport.logicalMaximum-joystickState.hatSwitchReport.logicalMinimum=7) THEN
+						joystickState.joystick.AddCoolieHat();
+					ELSE
+						KernelLog.String("HatSwitch found, but not compatible. HatSwitch events are not sent to Joysticks.."); KernelLog.Ln;
+					END;
+				END;
+
+				Joystick.registry.Add(joystickState.joystick,res);
+				RETURN TRUE;
+			ELSE
+				RETURN FALSE;
+			END;
+		END InitializeJoystickDriver;
+
+		(**
+		 * checks, whether the device sends mouse informations or not
+		 *)
+		PROCEDURE InitializeTouchscreenDriver():BOOLEAN;
+		VAR
+			mouseCollection	: UsbHidReport.HidCollection;
+			temp			: UsbHidReport.HidReport;
+			i				: LONGINT;
+			isReportProtocol: BOOLEAN;
+		BEGIN
+			(*get mouse collection: mouse collection uses UsagePage(Generic Desktop Controlsl)->1 and Usage(Mouse)->2*)
+			mouseCollection := reportManager.GetCollection(0DH, 04H);
+			IF (mouseCollection#NIL) THEN
+				NEW(touchscreenState);
+				touchscreenState.prev := FALSE;
+				touchscreenState.x 		:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 30H, mouseCollection,temp);
+				touchscreenState.minX := temp.logicalMinimum;
+				touchscreenState.maxX := temp.logicalMaximum;
+				touchscreenState.y 		:= reportManager.GetUsage(UsagePage.GenericDesktopPage, 31H, mouseCollection,temp);
+				touchscreenState.minY := temp.logicalMinimum;
+				touchscreenState.maxY := temp.logicalMaximum;
+				touchscreenState.tipSwitch := reportManager.GetUsage(0DH, 042H, mouseCollection,temp);
+				touchscreenState.inRange := reportManager.GetUsage(0DH, 032H, mouseCollection,temp);
+				touchscreenState.confidence := reportManager.GetUsage(0DH, 047H, mouseCollection, temp); 
+				KernelLog.String("Touchscreen device registered with ");
+				KernelLog.String("minX = "); KernelLog.Int(touchscreenState.minX,1); 
+				KernelLog.String(", maxX = "); KernelLog.Int(touchscreenState.maxX,1); 
+				KernelLog.String(", minY = "); KernelLog.Int(touchscreenState.minY,1); 
+				KernelLog.String(", maxY = "); KernelLog.Int(touchscreenState.maxY,1); 
+				IF touchscreenState.confidence # NIL THEN KernelLog.String(", confidence"); END; 
+				KernelLog.Ln;
+				RETURN TRUE;
+			ELSE
+				RETURN FALSE;
+			END;
+		END InitializeTouchscreenDriver;
+
+
+		(**
+		 * Reads bitlen bits from a position index
+		 * @param index[in bits]: where to start reading [1..32]
+		 * @param bitLen: the amount of bits to read
+		 * @return value
+		 *)
+		 PROCEDURE ReadBits(index, bitLen: LONGINT):LONGINT;
+		VAR rv : LONGINT;
+		BEGIN
+			rv := ReadBitsBuffer(index,bitLen,reportBuffer);
+			RETURN rv;
+		END ReadBits;
+
+		 (**
+		 * Reads bitlen bits from a position index
+		 * @param index[in bits]: where to start reading [1..32]
+		 * @param bitLen: the amount of bits to read
+		 * @param localBuf: the buffer to read from
+		 * @return value
+		 *)
+		PROCEDURE ReadBitsBuffer(index, bitLen: LONGINT; localBuf: Usbdi.BufferPtr):LONGINT;
+		VAR
+			endIndex	: LONGINT;
+			rv			: LONGINT;
+			temp		: LONGINT;
+			indexEightAligned : LONGINT;
+			bitsToShift	: LONGINT;
+			set			: SET;
+		BEGIN
+			endIndex := index + bitLen-1;
+
+			IF bitLen<=0 THEN RETURN 0 END;
+
+			IF Debug THEN KernelLog.String("read bits from "); KernelLog.Int(index,0); KernelLog.String(" to "); KernelLog.Int(endIndex,0); KernelLog.Ln; END;
+
+			IF(endIndex>=(8*LEN(localBuf))) THEN
+				IF Debug THEN KernelLog.String("ReadBits: Buffer overflow, endindex is out of localBuf"); KernelLog.Ln; END;
+				RETURN 0;
+			END;
+
+			IF (bitLen=1) THEN
+				(*simply get the bit*)
+				set := SYSTEM.VAL(SET, localBuf[index DIV 8]);
+				IF (index MOD 8) IN set THEN
+					rv := 1;
+				ELSE
+					rv := 0;
+				END;
+				RETURN rv;
+			END;
+
+			IF ((index DIV 8) = (endIndex DIV 8)) THEN
+				(*detect reading simple byte*)
+
+				temp := SYSTEM.VAL(LONGINT, ORD(localBuf[index DIV 8]));
+
+				IF (bitLen=8) THEN
+					rv:= temp;
+					IF Debug THEN
+						KernelLog.String("the byte value is: "); KernelLog.Int(rv,0); KernelLog.Ln;
+					END;
+					RETURN rv;
+				ELSE
+					(*simply read in the byte index DIV 8*)
+					IF Debug THEN
+						KernelLog.Ln;
+						KernelLog.String("       the value of the byte is: "); KernelLog.Int(temp,0); KernelLog.Ln;
+						KernelLog.String(" (");KernelLog.Bits(SYSTEM.VAL(SET, temp),0,8); KernelLog.String(")"); KernelLog.Ln;
+						KernelLog.String("       read in the byte from "); KernelLog.Int(index MOD 8,0); KernelLog.String(" to ");
+						KernelLog.Int(endIndex MOD 8,0); KernelLog.String(")"); KernelLog.Ln;
+					END;
+
+					temp := SYSTEM.VAL(LONGINT,(SYSTEM.VAL(SET, temp) * {(index MOD 8)..(endIndex MOD 8)}));
+					IF Debug THEN
+						KernelLog.String("        the value of the byte after masking: "); KernelLog.Bits(SYSTEM.VAL(SET, temp),0,8); KernelLog.Ln;
+					END;
+
+					bitsToShift := index MOD 8;
+					IF Debug THEN
+						KernelLog.String("        bits to shift: "); KernelLog.Int(bitsToShift,0); KernelLog.Ln;
+					END;
+
+					rv := SYSTEM.VAL(LONGINT,LSH(SYSTEM.VAL(CHAR,temp),-bitsToShift));
+
+					IF Debug THEN
+						KernelLog.String("        the value of the byte after shifting: "); KernelLog.Bits(SYSTEM.VAL(SET, rv),0,8); KernelLog.Ln;
+					END;
+
+				END;
+			ELSE
+				(* the index and the endIndex are not in the same byte
+
+					block position k of index is k="index DIV 8"
+					so endBit in the same block is   eb=k * 8 + 7
+					ex: given: index := 27;
+					asked:	how many bits to shift the current rv to left
+
+							k := 27 div 8
+							k := 3;
+							eb := 3 * 8 + 7= 31
+
+				*)
+				indexEightAligned := SYSTEM.VAL(LONGINT,SYSTEM.VAL(SET,index)+{0..2});
+
+				IF Debug THEN
+					KernelLog.String("index, indexEightAligned, endIndex");
+					KernelLog.Int(index,6);KernelLog.Int(indexEightAligned,6);KernelLog.Int(endIndex,6); KernelLog.Ln;
+				END;
+
+				temp := ReadBitsBuffer(indexEightAligned+1,endIndex-indexEightAligned, localBuf);
+				temp := LSH(temp,indexEightAligned-index+1);
+				rv := temp + ReadBitsBuffer(index, indexEightAligned-index+1,localBuf);
+			END;
+			RETURN rv;
+		END ReadBitsBuffer;
+
+		(**
+		 * for testing the readBits Procedure
+
+		PROCEDURE TestReader;
+		VAR
+			myBuf:	Usbdi.BufferPtr;
+			test:	LONGINT;
+		BEGIN
+			NEW(myBuf,8);
+
+			myBuf[0] := CHR(5H);
+			myBuf[1] := CHR(7H);
+			myBuf[2] := CHR(55H);
+			myBuf[3] := CHR(3H);
+			myBuf[4] := CHR(5H);
+			myBuf[5] := CHR(7H);
+			myBuf[6] := CHR(0FFH);
+			myBuf[7] := CHR(0FFH);
+
+
+			KernelLog.Ln;
+			KernelLog.String("Initialize TestReader"); KernelLog.Ln;
+
+			KernelLog.String("myBuf[0]: "); KernelLog.Int(SYSTEM.VAL(LONGINT,ORD(myBuf[0])),0); KernelLog.Ln;
+			KernelLog.String("myBuf[1]: "); KernelLog.Int(SYSTEM.VAL(LONGINT,ORD(myBuf[1])),0); KernelLog.Ln;
+			KernelLog.String("myBuf[2]: "); KernelLog.Int(SYSTEM.VAL(LONGINT,ORD(myBuf[2])),0); KernelLog.Ln;
+			KernelLog.String("myBuf[3]: "); KernelLog.Int(SYSTEM.VAL(LONGINT,ORD(myBuf[3])),0); KernelLog.Ln;
+			KernelLog.String("myBuf[4]: "); KernelLog.Int(SYSTEM.VAL(LONGINT,ORD(myBuf[4])),0); KernelLog.Ln;
+			KernelLog.String("myBuf[5]: "); KernelLog.Int(SYSTEM.VAL(LONGINT,ORD(myBuf[5])),0); KernelLog.Ln;
+			KernelLog.String("myBuf[6]: "); KernelLog.Int(SYSTEM.VAL(LONGINT,ORD(myBuf[6])),0); KernelLog.Ln;
+			KernelLog.String("myBuf[7]: "); KernelLog.Int(SYSTEM.VAL(LONGINT,ORD(myBuf[7])),0); KernelLog.Ln;
+
+			IF FALSE THEN
+			KernelLog.Ln;
+			KernelLog.String("Starting Testcases"); KernelLog.Ln;
+			KernelLog.String("Reading every bit from myBuf[0]: "); KernelLog.Bits(SYSTEM.VAL(SET,myBuf[0]),0,8); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit from 0:  "); KernelLog.Int(ReadBitsBuffer(0,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit from 1:  "); KernelLog.Int(ReadBitsBuffer(1,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit from 2:  "); KernelLog.Int(ReadBitsBuffer(2,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit from 3:  "); KernelLog.Int(ReadBitsBuffer(3,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit from 4:  "); KernelLog.Int(ReadBitsBuffer(4,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit from 5:  "); KernelLog.Int(ReadBitsBuffer(5,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit from 6:  "); KernelLog.Int(ReadBitsBuffer(6,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit from 7:  "); KernelLog.Int(ReadBitsBuffer(7,1,myBuf),0); KernelLog.Ln;
+			KernelLog.Ln;
+
+
+			KernelLog.String("Reading 1-8 bits from myBuf[1]: "); KernelLog.Bits(SYSTEM.VAL(SET,myBuf[1]),0,8); KernelLog.Ln;
+			KernelLog.String("    Read 1 bits from 8:  "); KernelLog.Int(ReadBitsBuffer(8,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 2 bits from 8:  "); KernelLog.Int(ReadBitsBuffer(8,2,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 3 bits from 8:  "); KernelLog.Int(ReadBitsBuffer(8,3,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 4 bits from 8:  "); KernelLog.Int(ReadBitsBuffer(8,4,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 5 bits from 8:  "); KernelLog.Int(ReadBitsBuffer(8,5,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 6 bits from 8:  "); KernelLog.Int(ReadBitsBuffer(8,6,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 7 bits from 8:  "); KernelLog.Int(ReadBitsBuffer(8,6,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 8 bits from 8:  "); KernelLog.Int(ReadBitsBuffer(8,8,myBuf),0); KernelLog.Ln;
+			KernelLog.Ln;
+
+			KernelLog.String("Reading 1-7 bits from myBuf[2]: "); KernelLog.Bits(SYSTEM.VAL(SET,myBuf[2]),0,8); KernelLog.Ln;
+			KernelLog.String("    Read 1 bit   from 17:  "); KernelLog.Int(ReadBitsBuffer(17,1,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 2 bits from 17:  "); KernelLog.Int(ReadBitsBuffer(17,2,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 3 bits from 17:  "); KernelLog.Int(ReadBitsBuffer(17,3,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 4 bits from 17:  "); KernelLog.Int(ReadBitsBuffer(17,4,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 5 bits from 17:  "); KernelLog.Int(ReadBitsBuffer(17,5,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 6 bits from 17:  "); KernelLog.Int(ReadBitsBuffer(17,6,myBuf),0); KernelLog.Ln;
+			KernelLog.String("    Read 7 bits from 17:  "); KernelLog.Int(ReadBitsBuffer(17,6,myBuf),0); KernelLog.Ln;
+			KernelLog.Ln;
+
+
+			KernelLog.String("Read 8 bits from 0:  "); KernelLog.Int(ReadBitsBuffer(0,8,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 9 bits from 0:  "); KernelLog.Int(ReadBitsBuffer(0,9,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 10 bits from 0:  "); KernelLog.Int(ReadBitsBuffer(0,10,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 11 bits from 0:  "); KernelLog.Int(ReadBitsBuffer(0,11,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 12 bits from 0:  "); KernelLog.Int(ReadBitsBuffer(0,12,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 13 bits from 0:  "); KernelLog.Int(ReadBitsBuffer(0,13,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 14 bits from 0:  "); KernelLog.Int(ReadBitsBuffer(0,14,myBuf),0); KernelLog.Ln;
+			KernelLog.Ln;
+
+			KernelLog.String("Read 7 bits from 1:  "); KernelLog.Int(ReadBitsBuffer(1,7,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 8 bits from 1:  "); KernelLog.Int(ReadBitsBuffer(1,8,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 9 bits from 1:  "); KernelLog.Int(ReadBitsBuffer(1,9,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 10 bits from 1:  "); KernelLog.Int(ReadBitsBuffer(1,10,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 11 bits from 1:  "); KernelLog.Int(ReadBitsBuffer(1,11,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 12 bits from 1:  "); KernelLog.Int(ReadBitsBuffer(1,12,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 13 bits from 1:  "); KernelLog.Int(ReadBitsBuffer(1,13,myBuf),0); KernelLog.Ln;
+			KernelLog.Ln;
+
+			KernelLog.String("Read 8 bits from 32:  "); KernelLog.Int(ReadBitsBuffer(32,8,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 9 bits from 32:  "); KernelLog.Int(ReadBitsBuffer(32,9,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 10 bits from 32:  "); KernelLog.Int(ReadBitsBuffer(32,10,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 11 bits from 32:  "); KernelLog.Int(ReadBitsBuffer(32,11,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 12 bits from 32:  "); KernelLog.Int(ReadBitsBuffer(32,12,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 13 bits from 23:  "); KernelLog.Int(ReadBitsBuffer(32,13,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 14 bits from 32:  "); KernelLog.Int(ReadBitsBuffer(32,14,myBuf),0); KernelLog.Ln;
+			KernelLog.Ln;
+
+			KernelLog.String("Read 7 bits from 33:  "); KernelLog.Int(ReadBitsBuffer(33,7,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 8 bits from 33:  "); KernelLog.Int(ReadBitsBuffer(33,8,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 9 bits from 33:  "); KernelLog.Int(ReadBitsBuffer(33,9,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 10 bits from 33:  "); KernelLog.Int(ReadBitsBuffer(33,10,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 11 bits from 33:  "); KernelLog.Int(ReadBitsBuffer(33,11,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 12 bits from 33:  "); KernelLog.Int(ReadBitsBuffer(33,12,myBuf),0); KernelLog.Ln;
+			KernelLog.String("Read 13 bits from 33:  "); KernelLog.Int(ReadBitsBuffer(33,13,myBuf),0); KernelLog.Ln;
+			KernelLog.Ln;
+			END;
+			KernelLog.String("TwosComplement of 6 in (4 Bits): "); KernelLog.Int(TwosComplement(6,4),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 7 in (4 Bits): "); KernelLog.Int(TwosComplement(7,4),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 8 in (4 Bits): "); KernelLog.Int(TwosComplement(8,4),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 13 in (4 Bits): "); KernelLog.Int(TwosComplement(13,4),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 14 in (4 Bits): "); KernelLog.Int(TwosComplement(14,4),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 15 in (4 Bits): "); KernelLog.Int(TwosComplement(15,4),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 15 in (5 Bits): "); KernelLog.Int(TwosComplement(15,5),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 16 in (5 Bits): "); KernelLog.Int(TwosComplement(16,5),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 28 in (5 Bits): "); KernelLog.Int(TwosComplement(28,5),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 29 in (5 Bits): "); KernelLog.Int(TwosComplement(29,5),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 30 in (5 Bits): "); KernelLog.Int(TwosComplement(30,5),0); KernelLog.Ln;
+			KernelLog.String("TwosComplement of 31 in (5 Bits): "); KernelLog.Int(TwosComplement(31,5),0); KernelLog.Ln;
+			KernelLog.Ln;
+
+			test := ReadBitsBuffer(48,12, myBuf);
+			KernelLog.String("Reading 12 Bits at block 6 (starting at 48, 12 bits: "); KernelLog.Int(test,0); KernelLog.Ln;
+			KernelLog.String("      ?= 4095. Twos Complement should be -1: "); KernelLog.Int(TwosComplement(test,12),0); KernelLog.Ln;
+
+			test := ReadBitsBuffer(49,12, myBuf);
+			KernelLog.String("Reading 12 Bits at block 6 (starting at 49, 12 bits: "); KernelLog.Int(test,0); KernelLog.Ln;
+			KernelLog.String("      ?= 4095. Twos Complement should be -1: "); KernelLog.Int(TwosComplement(test,12),0); KernelLog.Ln;
+
+
+		END TestReader;
+		*)
+
+		(**
+		 * returns the twos complement of a value by the predicted bitLen
+		 * @param value: the value to convert
+		 * @param bitLen: the bit length of the value it can have
+		 * @return twos complement of value
+		 *)
+		PROCEDURE TwosComplement(value: LONGINT; bitLen: LONGINT) : LONGINT;
+		VAR toMuch : LONGINT;
+		BEGIN
+			IF(bitLen<32) & (bitLen>0) THEN
+				IF ((bitLen-1) IN SYSTEM.VAL(SET,value)) THEN
+					toMuch:= SYSTEM.VAL(LONGINT,{bitLen});
+					value := value - toMuch;
+				END;
+			END;
+			RETURN value;
+		END TwosComplement;
+
+		(**
+		 * Sets the device into normal mode (report protocol mode)
+		 *)
+		PROCEDURE SetReportProtocol():BOOLEAN;
+		VAR
+			(*0: if boot protocol, 1: if report protocol*)
+			bootFlag:	LONGINT;
+		BEGIN
+			IF (GetProtocol(bootFlag)) THEN
+				IF(bootFlag=0) THEN
+					IF Debug THEN
+						KernelLog.String("UsbHidDriver:HidDriver.Connect: GetProtocol returned boot protocol, set to report protocol"); KernelLog.Ln;
+					END;
+					IF(SetProtocol(1)=FALSE) THEN
+						KernelLog.String("UsbHidDriver:HidDriver.Connect: SetProtocol to report failed"); KernelLog.Ln;
+						RETURN FALSE;
+					END;
+				ELSE
+					IF Debug THEN
+						KernelLog.String("UsbHidDriver:HidDriver.Connect: GetProtocol returned report protocol"); KernelLog.Ln;
+					END;
+				END;
+			END;
+			RETURN TRUE;
+		END SetReportProtocol;
+
+	END HidDriver;
+
+(*used for debug output. lists the report descriptor as described in Device Class Definition for Human Interface Devices,
+	f.e. page 61 Appendix A, B.2 Protocol 2 (Mouse)*)
+PROCEDURE LayoutBuffer*(CONST buf : Usbdi.Buffer;  len : LONGINT);
+VAR temp : LONGINT;
+BEGIN
+	KernelLog.String("Buffer Outline:"); KernelLog.Ln;
+	FOR temp := 0 TO len-1 DO
+		IF (temp MOD 2 = 0) THEN
+			KernelLog.Ln();
+			KernelLog.Int(temp, 4);
+			KernelLog.String("    ");
+			KernelLog.Hex(ORD(buf[temp]), -2);
+		ELSE
+			KernelLog.String("    ");
+			KernelLog.Hex(ORD(buf[temp]), -2);
+		END;
+	END;
+	KernelLog.Ln(); KernelLog.Ln();
+END LayoutBuffer;
+
+(*check, whether the device is a hid device
+ *	return 	HidDriver, if hid device found, NIL otherwise
+ *)
+PROCEDURE Probe(dev : Usbdi.UsbDevice; if : Usbdi.InterfaceDescriptor) : Usbdi.Driver;
+VAR hidDriver : HidDriver;
+BEGIN
+	IF if.bInterfaceClass # 3 THEN RETURN NIL END;
+	NEW(hidDriver);
+	RETURN hidDriver;
+END Probe;
+
+(* Called, when detaching the UsbHidDriver *)
+PROCEDURE Cleanup;
+BEGIN
+	Usbdi.drivers.Remove(Name);
+END Cleanup;
+
+PROCEDURE Install*;
+END Install;
+
+TYPE 
+	DimensionGetter= PROCEDURE {DELEGATE} (VAR w,h: LONGINT);
+VAR
+	getDisplayDimensions: DimensionGetter;
+	touchConfidenceDelay: LONGINT;
+
+PROCEDURE DefaultDisplayDimensions(VAR w,h: LONGINT);
+BEGIN
+	w := 1024; h := 768;
+END DefaultDisplayDimensions;
+
+PROCEDURE InstallDisplayDimensions*(poller: DimensionGetter); (* for touchscreen coordinate transformation*)
+BEGIN
+	IF poller # NIL THEN
+		getDisplayDimensions := poller;
+	ELSE 
+		getDisplayDimensions := DefaultDisplayDimensions;
+	END;
+END InstallDisplayDimensions;
+
+
+PROCEDURE Setup;
+VAR s: ARRAY 32 OF CHAR; p: LONGINT;
+BEGIN
+	getDisplayDimensions := DefaultDisplayDimensions;
+	Machine.GetConfig("TouchDelay", s);
+	IF s[0] # 0X THEN
+		p := 0;  touchConfidenceDelay := Machine.StrToInt(p, s)
+	ELSE
+		touchConfidenceDelay := 0; 
+	END;
+	Usbdi.drivers.Add(Probe, Name, Description, 10);
+END Setup;
+
+
+BEGIN
+	Modules.InstallTermHandler(Cleanup);
+	Setup;
+END UsbHidDriver.
+
+UsbHidDriver.Install ~  SystemTools.Free UsbHidDriver UsbHidParser UsbHidErrors UsbHidParserExt UsbHidReport UsbHidUP~

source/ARM.UsbHubDriver.Mod

@@ -0,0 +1,1566 @@
+MODULE UsbHubDriver; (** AUTHOR "staubesv"; PURPOSE "USB 2.0 Hub Driver"; *)
+(**
+ * Bluebottle USB 2.0 Hub Driver
+ *
+ * The hub driver is actually part of the USB Bus Driver and has direct access to the functionality offered by Usb.Mod.
+ *
+ * Usage:
+ *
+ *	UsbHubDriver.Install ~ will load this device driver
+ *	SystemTools.Free UsbHubDriver ~ unloads it
+ *
+ * Overview:
+ *
+ *	HubDriverInterface(Usbdi.Driver)		Abstract class defining the interface to USB hub devices and USB root hubs
+ *	HubDriver(HubDriverInterface)			Actual hub driver for both USB hub devices and USB root hubs,
+ * 											based on HubDriverInterface
+ *	UsbHubDriver(HubDriver)				Implements HubDriverInterface for USB hub devices
+ *	UsbRootHubDriver(HubDriver)			Implements HubDriverInterface for USB root hubs
+ *
+ * References:
+ *
+ *	Universal Serial Bus Specification, Revision 2.0 (available at www.usb.org)
+ *
+ * History:
+ *
+ *	24.11.2005	First release (staubesv)
+ *	12.12.2005	Force check port status for USB hub devices to enumerate attached devices, use exception handling (staubesv)
+ *	29.06.2006	Show overcurrent conditions on kernel log (staubesv)
+ *	30.06.2006	Made HubDriver.Wait procedure EXCLUSIVE to protect timer from concurrent use (staubesv)
+ *	03.07.2006	Bugfix: Correct address and port of transaction translator for low-/fullspeed devices connected to high-speed busses (staubesv)
+ *	04.07.2006	UsbHubDriver.GetPortStatus: First ackknowledge change bits then evaluate port status (staubesv)
+ *	02.08.2006	Bugfix in HubDriver.HandlePortStatusChange, adaptions to Usbdi (staubesv)
+ *
+ * TODOs:
+ *	- power management/saving
+ *	- overcurrent handling
+ *	- correct TT support
+ *	- device driver connect procedure blocks HubDriver.HandlePortStatus change -> shouldn't do that,
+ *		hubdriver should call connect itself, not as sideeffect of installing a driver via driver manager
+ *)
+
+IMPORT SYSTEM, KernelLog, Usb, UsbHcdi, Usbdi, UsbBuffers, Kernel, Modules, Debug := UsbDebug;
+
+CONST
+
+	(* Name and description of the integrated USB (root) hub driver *)
+	Name = "UsbHub";
+	Description = "USB Hub Driver";
+
+	AllowSuspend = FALSE;
+
+	(* Interval in milliseconds the root hubs are polled when interrupt notification is not supported. *)
+	PollingInterval = 200;
+
+	(* Hub class port status bits *)
+	PsCurrentConnectStatus = {0};
+	PsPortEnabled = {1};
+	PsSuspend = {2};
+	PsOverCurrent = {3};
+	PsReset = {4};
+	PsPortPower = {8};
+	PsLowSpeed = {9}; (* IF status * {9, 10} = {} THEN Fullspeed *)
+	PsHighSpeed = {10};
+	PsPortTestMode = {11};
+	PsPortIndicators = {12};
+	PsConnectStatusChange = {16};
+	PsPortEnabledChange = {17};
+	PsSuspendChange = {18};
+	PsOvercurrentChange = {19};
+	PsResetChange = {20};
+	PsChangeMask = {16..20};
+
+	(* Hub class hub status bit *)
+	HsLocalPowerLost = {0};
+	HsOvercurrent = {1};
+	HsLocalPowerSourceChange = {16};
+	HsOvercurrentChange = {17};
+
+	(* Hub Class Request Codes (USB2.0, p. 421) *)
+	GetStatus = 0;
+	ClearFeature = 1;
+	SetFeature = 3;
+	GetDescriptor = 6;
+	SetDescriptor = 7;
+	ClearTtBuffer = 8;
+	ResetTt = 9;
+	GetTtState = 10;
+	StopTt = 11;
+
+	(* Hub Class Feature Selectors (USB2.0, p 421- 422) *)
+	HubLocalPowerChange = 0;
+	HubOverCurrentChange = 1;
+	PortConnection = 0;
+	PortEnable = 1;
+	PortSuspend = 2;
+	PortOverCurrent = 3;
+	PortReset = 4;
+	PortPower = 8;
+	PortLowSpeed = 9;
+	PortConnectionChange = 16;
+	PortEnableChange = 17;
+	PortSuspendChange = 18;
+	PortOverCurrentChange = 19;
+	PortResetChange = 20;
+	PortTest = 21;
+	PortIndicator =22;
+
+	(* Descriptor types *)
+	DescriptorHub = 29H;
+	DescriptorDevice = 1;
+
+	(* UsbHubDriver.powerSwitching & UsbHubDriver.ocProtection values *)
+	NotAvailable = UsbHcdi.NotAvailable; (* MUST be 0 *)
+	Global = UsbHcdi.Global; (* MUST be 1 *)
+	PerPort = UsbHcdi.PerPort; (* MUST be 2 *)
+
+	(* Format of Setup Data *)
+	ToDevice = Usbdi.ToDevice;
+	ToHost = Usbdi.ToHost;
+	Class = Usbdi.Class;
+	Device = Usbdi.Device;
+	Other = Usbdi.Other;
+
+	(* HubDriver.EnablePortPower parameter *)
+	AllPorts = -1;
+
+	(* Device attachement/removal *)
+	DeviceAttached = 0;
+	DeviceRemoved = 1;
+
+	(* Number of times the status pipe of USB hub devices is tried to be restarted when errors occur *)
+	StatusPipeMaxRetries = 5;
+
+TYPE
+
+	(* Interface to be implemented for both USB hub devices and USB root hubs *)
+	HubInterface = OBJECT(Usbdi.Driver)
+	VAR
+
+		(** Note: Port numbers: 0..nbrOfPorts-1 *)
+
+		(*
+		 * This hub class specific request returns the hub descriptor.
+		 * @param type: Descriptor type
+		 * @param index: Descriptor index
+		 * @param length: Number of bytes to load
+		 * @param buffer: Buffer where to put the descriptor in (at offset 0)
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE GetHubDescriptor(type, index, length : LONGINT; VAR buffer : Usbdi.Buffer) : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END GetHubDescriptor; (* abstract *)
+
+		(*
+		 * This hub class specific request overrides the hub descriptor.
+		 * Note that this request is optional. It will be stalled by the USB hub device is not supported. As all USB
+		 * device descriptors, its first byte is its length in bytes and its second bytes the descriptor type.
+		 * @param type: Descriptor type
+		 * @param index: Descriptor index
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		 PROCEDURE SetHubDescriptor(type, index : LONGINT; buffer : Usbdi.Buffer) : BOOLEAN;
+		 BEGIN HALT(301); RETURN FALSE; END SetHubDescriptor; (* abstract *)
+
+		(*
+		 * This hub class request resets a value reported in the hub status.
+		 * @param feature: Feature selector (HubLocalPower or HubOvercurrent)
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE ClearHubFeature(feature : LONGINT) : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END ClearHubFeature; (* abstract *)
+
+		(*
+		 * This hub class request sets a value reported in the hub status.
+		 * @param feature: Feature selector (HubLocalPowerChange or HubOvercurrentChange)
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE SetHubFeature(feature : LONGINT) : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END SetHubFeature; (* abstract *)
+
+		(*
+		 * This hub class request resets a value reported in the port status.
+		 * @param feature: Feature to be reset
+		 * @port: Port number
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE ClearPortFeature(feature, port,  selector : LONGINT) : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END ClearPortFeature; (* abstract *)
+
+		(*
+		 * This hub class request sets a value reported in the hub status.
+		 * @param port Port number
+		 * @param feature Feature to be resetted (HubLocalPower or HubOvercurrent)
+		 * @param selector
+		 * @return Status of the control transfer
+		 *)
+		PROCEDURE SetPortFeature(feature, port, selector : LONGINT) : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END SetPortFeature; (* abstract *)
+
+		(*
+		 * This hub class request returns the current hub status and the states that have change since the
+		 * previous acknowledgment.
+		 * @param hubstatus
+		 * @return TRUE, if status request succeeded, FALSE otherwise.
+		 *)
+		PROCEDURE GetHubStatus(VAR hubstatus : SET) : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END GetHubStatus; (* abstract *)
+
+		(*
+		 * This hub class request returns the current port status and the current value of the port status
+		 * change bits.
+		 * @param port Port number
+		 * @param ack Acknowledge status change bits
+		 * @return Status of the specified port
+		 *)
+		PROCEDURE GetPortStatus(port : LONGINT; ack : BOOLEAN) : SET;
+		BEGIN HALT(301); RETURN {}; END GetPortStatus; (* abstract *)
+
+		(*
+		 * Transaction Translator (TT) control.
+		 * High-speed capable USB hub devices can operate at full- or highspeed. When connected to a highspeed host
+		 * controller, the communication between the hub device and the host is always at highspeed. When low- or fullspeed
+		 * USB devices are attached to a USB hub device operating at highspeed, the split transaction protocol is used.
+		 * The low-/fullspeed USB transactions are sent at highspeed from the host  to the hub device, which has one (single-TT)
+		 * or more (multi-TT) transaction tranlators. These translate the transaction into a low-/fullspeed transaction.
+		 *)
+
+		(*
+		 * This hub class specific request clears the state of the Transaction Translator (TT) bulk/control transfer after
+		 * it has been left in a busy state due to high-speed errors. This request is only defined for non-periodic endpoints.
+		 * @param dev : Low-/Fullspeed USB device
+		 * @param endpoint : Endpoint number
+		 * @param port: If the hub supports a TT per port, this is the port number of the TT that encountered the high-speed errors.
+		 * @return TRUE, if request succeeded, FALSE otherwiese
+		 *)
+		PROCEDURE ClearTTBuffer(dev : Usb.UsbDevice; endpoint, port : LONGINT) : BOOLEAN;
+		BEGIN
+			RETURN FALSE;
+		END ClearTTBuffer;
+
+		(*
+		 * This hub class specific request returns the internal state of the Transaction Translator (TT) in a vendor specific format.
+		 * A TT receiving this request must have first been stopped using the StopTTRequest.
+		 * @flags Vendor specific usage
+		 * @port If the hub supports multiple TTs, specify the port number of the TT that will return TT_State. Must be one for single-TT hubs.
+		 * @return TRUE, if the request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE GetTTState(flags, port, len : LONGINT; VAR buffer : Usbdi.Buffer) : BOOLEAN;
+		BEGIN
+			RETURN FALSE;
+		END GetTTState;
+
+		(*
+		 * This hub class specific request returns the Transaction Translator (TT) in a hub to a known state.
+		 * After the reset is completed, the TT can resume its normal operation.
+		 * @param port: If the hub supports multiple TTs, specify the port number of the TT that is to be reset (Must be 1 for single-TT hubs).
+		 * @return TRUE, if the request succeeded, FALSE otherwise
+		 *)
+		 PROCEDURE ResetTT(port : LONGINT) : BOOLEAN;
+		 BEGIN
+			RETURN FALSE;
+		 END ResetTT;
+
+		(*
+		 * This hub class specific request stops the normal execution of the Transaction Translator (TT) so that the internal
+		 * state can be retrieved via GetTTState. This request is provided for debugging purposes.
+		 * @param port: If the hub supports multiple TTs, the port number of the TT that is being stopped must be specified (1 for single-TT hubs).
+		 * @return TRUE, if request succeeded, FALSE otherwise
+		 *)
+		PROCEDURE StopTT(port : LONGINT) : BOOLEAN;
+		BEGIN
+			RETURN FALSE;
+		END StopTT;
+
+		(*
+		 * Perform initialization of USB hub device or root hub
+		 * @return TRUE, if initialization succeeded, FALSE otherwise
+		 *)
+		PROCEDURE Initialize() : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END Initialize; (* abstract *)
+
+	END HubInterface;
+
+TYPE
+
+	(* Integrated USB Hub Driver. *)
+	HubDriver = OBJECT (HubInterface)
+	VAR
+	 	(* Associated USB hub device *)
+	 	hub : Usb.UsbDevice;
+
+		(* Information from hub descriptor *)
+		nbrOfPorts : LONGINT;			(* Number of downstream ports *)
+	 	pwrOn2pwrGood : LONGINT;	(* Power on to power good wait time [ms] *)
+	 	powerSwitching : LONGINT;		(* Supported power switching modes (NotAvailable, Global or PerPort) *)
+	 	isCompound : BOOLEAN; 		(* Is this hub part of a compound device? *)
+	 	ocProtection : LONGINT; 		(* Supported overcurrent protection (NotAvailable, Global or PerPort) *)
+	 	thinkTime : LONGINT; 			(* 0..4 *)
+	 	portIndicators : BOOLEAN; 		(* Is port indicator control support available? *)
+	 	ctrlCurrent : LONGINT;
+	 	deviceRemovable : POINTER TO ARRAY OF BOOLEAN;
+
+		timer : Kernel.Timer;
+
+		(* Enable power on the specified port (or on all ports if AllPorts is used as parameter) *)
+		PROCEDURE EnablePortPower(port : LONGINT) : BOOLEAN;
+		VAR i : LONGINT;
+		BEGIN
+			IF port = AllPorts THEN (* Only wait for power on to power good once *)
+				IF Debug.Trace & Debug.traceHubRequests THEN Show("Enable power on all ports"); KernelLog.Ln; END;
+				FOR i := 0 TO nbrOfPorts-1 DO
+					IF ~SetPortFeature(PortPower, i, 0) THEN
+						IF Debug.Level >= Debug.Errors THEN Show("Could not enable power on port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+						RETURN FALSE;
+					END;
+				END;
+				Wait(pwrOn2pwrGood);
+				RETURN TRUE;
+			ELSE
+				IF Debug.Trace & Debug.traceHubRequests THEN Show("Enable power on port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+				IF SetPortFeature(PortPower, port, 0) THEN
+					Wait(pwrOn2pwrGood);
+					RETURN TRUE;
+				END;
+			END;
+			RETURN FALSE;
+		END EnablePortPower;
+
+		(* Disable power on the specified port *)
+		PROCEDURE DisablePortPower(port : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Disable power on port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+			RETURN ClearPortFeature(PortPower, port, 0);
+		END DisablePortPower;
+
+		(* Enable the specified port *)
+		PROCEDURE ResetAndEnablePort(port :LONGINT) : BOOLEAN;
+		VAR status : SET; timer : Kernel.Timer;
+		BEGIN
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Enable port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+			IF SetPortFeature(PortReset, port, 0) THEN (* Hub will enable port after reset *)
+				NEW(timer); timer.Sleep(100);
+				status := GetPortStatus(port, FALSE);
+				IF status * UsbHcdi.PortStatusError # {} THEN
+					IF Debug.Level >= Debug.Errors THEN Show("Cannot get port status after enabling."); KernelLog.Ln; END;
+					RETURN FALSE;
+				ELSIF status * UsbHcdi.PortStatusReset # {} THEN
+					IF Debug.Level >= Debug.Errors THEN Show("Port still in reset (after 50ms!)"); KernelLog.Ln; END;
+					RETURN FALSE;
+				ELSIF status * UsbHcdi.PortStatusEnabled = {} THEN
+					IF Debug.Level >= Debug.Errors  THEN Show("Could not enable port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+					RETURN FALSE;
+				END;
+			ELSE
+				RETURN FALSE;
+			END;
+			RETURN TRUE;
+		END ResetAndEnablePort;
+
+		(* Disable the specified port *)
+		PROCEDURE DisablePort(port : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Disable port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+			RETURN ClearPortFeature(PortEnable, port, 0);
+		END DisablePort;
+
+		(* Selectively suspend the specified port *)
+		PROCEDURE SuspendPort(port : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.Trace & (Debug.traceHubRequests OR Debug.traceSuspend) THEN Show("Suspend port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+			IF SetPortFeature(PortSuspend, port, 0) THEN
+				hub.deviceAtPort[port].SetState(Usb.StateSuspended);
+				RETURN TRUE;
+			ELSIF Debug.Level >= Debug.Errors THEN Show("Failed to suspend port"); KernelLog.Int(port+1, 0); KernelLog.Ln;
+			END;
+			RETURN FALSE;
+		END SuspendPort;
+
+		(* Resume a selectively suspended port *)
+		PROCEDURE ResumePort(port : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.Trace & (Debug.traceHubRequests OR Debug.traceSuspend) THEN Show("Resume port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+			IF ClearPortFeature(PortSuspend, port, 0) THEN
+				hub.deviceAtPort[port].SetState(Usb.StateConfigured);
+				RETURN TRUE;
+			ELSIF Debug.Level >= Debug.Errors THEN Show("Failed to resume port "); KernelLog.Int(port+1, 0); KernelLog.Ln;
+			END;
+			RETURN FALSE;
+		END ResumePort;
+
+		(* Are there any device drivers associated to the specified device? *)
+		PROCEDURE DriversInstalled(dev : Usb.UsbDevice) : BOOLEAN;
+		VAR intf : Usb.InterfaceDescriptor; i : LONGINT;
+		BEGIN (* locking? *)
+			FOR i := 0 TO LEN(dev.actConfiguration.interfaces)-1 DO
+				intf := dev.actConfiguration.interfaces[i] (Usb.InterfaceDescriptor);
+				IF intf.driver # NIL THEN RETURN TRUE; END;
+			END;
+			RETURN FALSE;
+		END DriversInstalled;
+
+		(* If the hub supports port indicator control, set the port indcator to Automatic, Green, Amber or Off. *)
+		PROCEDURE Indicate(port, ledstatus : LONGINT);
+		BEGIN
+			IF Debug.StrongChecks THEN
+				ASSERT((ledstatus = UsbHcdi.Automatic) OR (ledstatus =UsbHcdi. Green) OR (ledstatus = UsbHcdi.Amber) OR (ledstatus = UsbHcdi.Off));
+			END;
+			IF portIndicators THEN (* Port Indicator Control supported *)
+				IF Debug.Trace & Debug.traceHubRequests THEN
+					Show("Set port indicator of port "); KernelLog.Int(port+1, 0); KernelLog.String(" to "); KernelLog.Int(ledstatus, 0); KernelLog.Ln;
+				END;
+				IF SetPortFeature(PortIndicator, port, ledstatus) THEN
+				ELSIF Debug.Level >= Debug.Errors THEN Show("Could not control port indicator."); KernelLog.Ln;
+				END;
+			END;
+		END Indicate;
+
+		(* How much current (mA) is available for this hub. *)
+		PROCEDURE AvailableCurrent() : LONGINT;
+		VAR hubstatus : SET; current : LONGINT;
+		BEGIN
+			current := 0;
+			IF GetHubStatus(hubstatus) THEN
+				IF hubstatus * HsLocalPowerLost = {} THEN (* Hub is in self-powered mode *)
+				ELSE
+				END;
+			ELSE
+			END;
+			RETURN current;
+		END AvailableCurrent;
+
+		(* Hub may report power source changes (self-powered vs. bus-powered) and overcurrent changes (if not reported per port). *)
+		PROCEDURE HandleHubStatusChange;
+		VAR hubstatus : SET; ignore : BOOLEAN;
+		BEGIN
+			IF Debug.Trace & Debug.traceConnects THEN Show("Handling hub status change."); KernelLog.Ln; END;
+			IF GetHubStatus(hubstatus) THEN
+				IF hubstatus * HsLocalPowerLost # {} THEN
+					IF Debug.Level >= Debug.Default THEN Show("Hub hast lost power supplier"); KernelLog.Ln; END;
+				END;
+				IF hubstatus * HsOvercurrent # {} THEN
+					IF Debug.Level >= Debug.Default THEN Show("Hub reports overcurrent condition"); KernelLog.Ln END;
+				END;
+				(* Ackknowledge status changes *)
+				IF hubstatus * HsLocalPowerSourceChange # {} THEN
+					ignore := ClearHubFeature(HubLocalPowerChange);
+				END;
+				IF hubstatus * HsOvercurrentChange # {} THEN
+					ignore := ClearHubFeature(HubOverCurrentChange);
+				END;
+			ELSIF Debug.Level >= Debug.Errors THEN Show("Hub status change but could not get hub status."); KernelLog.Ln;
+			END;
+		END HandleHubStatusChange;
+
+		PROCEDURE LookForDevices;
+		VAR i : LONGINT; trap : BOOLEAN;
+		BEGIN
+			IF nbrOfPorts > 0 THEN
+				FOR i := 0 TO nbrOfPorts-1 DO (* Check and handle status of all ports *)
+					HandlePortStatusChange(i);
+				END;
+			END;
+		FINALLY
+			IF trap & (Debug.Level >= Debug.Warnings) THEN KernelLog.String("UsbHubDriver: TRAP catched."); KernelLog.Ln; END;
+		END LookForDevices;
+
+		(* Remove device and its driver instance from the specified port *)
+		PROCEDURE RemoveDeviceFromPort(port : LONGINT);
+		BEGIN
+			IF hub.deviceAtPort[port] # NIL THEN (* remove device and its driver instance from port *)
+				hub.deviceAtPort[port].SetState(Usb.StateDisconnected);
+				hub.deviceAtPort[port].Remove;
+				hub.deviceAtPort[port] := NIL;
+			END;
+		END RemoveDeviceFromPort;
+
+		(*	Poll the status of this hub and look for connect changes. If a connect change occured, i.e. a USB device
+			has been attached or detached to/from a port, call FindNewDevice or remove the device dependent data structures *)
+		PROCEDURE HandlePortStatusChange(port : LONGINT);
+		CONST MaxPortStatusErrors = 10;
+		VAR dev : Usb.UsbDevice; status : SET; i : LONGINT; res : BOOLEAN;
+		BEGIN
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Handling port status change for port "); KernelLog.Int(port + 1, 0); KernelLog.Ln; END;
+			status := GetPortStatus(port, TRUE);
+
+			IF status * UsbHcdi.PortStatusError # {} THEN
+				INC(hub.portErrors[port]);
+				IF hub.portErrors[port] >= MaxPortStatusErrors THEN
+					IF Debug.Level >= Debug.Errors THEN Show("Error: Could not get status of port "); KernelLog.Int(port + 1, 0); KernelLog.Ln; END;
+					RemoveDeviceFromPort(port);
+					Indicate(port, UsbHcdi.Amber);
+				END;
+				RETURN;
+			ELSE
+				hub.portErrors[port] := 0;
+			END;
+
+			IF status * UsbHcdi.PortStatusOverCurrent # {} THEN
+				IF Debug.Level >= Debug.Default THEN Show("Warning: Overcurrent detected on port "); KernelLog.Int(port + 1, 0); KernelLog.Ln; END;
+			END;
+
+			IF status * UsbHcdi.PortStatusConnectChange # {} THEN (* Connection Status of port has changed *)
+
+				IF status * UsbHcdi.PortStatusDevicePresent # {} THEN (* A device has been attached to this port *)
+
+					IF Debug.Trace & Debug.traceConnects THEN Show("Looking at device at port "); KernelLog.Int(port + 1, 0); KernelLog.Ln; END;
+
+					(* I've seen devices that disconnect under error conditions and then reconnect again. Therefore,
+					we first check whether the USB system has already an attached device on the port *)
+					IF hub.deviceAtPort[port] # NIL THEN
+						IF Debug.Level >= Debug.Warnings THEN Show("Device already present. Remove it."); KernelLog.Ln; END;
+						RemoveDeviceFromPort(port);
+					END;
+
+					(* Note: PortStatusConnectChange is reset by GetPortStatus() *)
+					IF  ~hub.portPermanentDisabled[port] THEN
+						(* There mustn't be more than one enabled port with an unaddressed USB device
+						connected to a single USB. Otherwise, multiple devices could respond to the default address 0 *)
+						Wait(UsbHcdi.PortInsertionTime); (* >= 100ms, USBspec *)
+						hub.controller.Acquire;
+						res := ResetAndEnablePort(port);
+
+						IF res THEN (* Try to connect to attached USB device *)
+							i := 0;
+							LOOP
+								dev := GetAddressedDevice(port);
+								IF dev # NIL THEN (* Device found *) EXIT; END;
+								IF Debug.Trace & Debug.traceConnects THEN Show("Retrying to connect device."); KernelLog.Ln;END;
+								res := ResetAndEnablePort(port);
+								Wait(100 + i * 50); (* eventually the USB device reacts to slowly *)
+								INC(i);
+								IF i >=4 THEN EXIT END;
+							END;
+
+							IF dev = NIL THEN (* ERROR: USB device attached but not found using GetAddressedDevice *)
+								res := DisablePort(port); (* ignore res *)
+								hub.controller.Release;
+								status := GetPortStatus(port, FALSE);
+								IF status * UsbHcdi.PortStatusDevicePresent = {} THEN (* Bad timing... device is not present anymore *)
+									Indicate(port, UsbHcdi.Off);
+								ELSE (* There is a device attached but we can't handle it *)
+									IF Debug.Level >= Debug.Default THEN
+										Show("Cannot access device. Permanently disabled port "); KernelLog.Int(port+1, 0);
+										KernelLog.String(". Replug connector of device!"); KernelLog.Ln;
+									END;
+									hub.portPermanentDisabled[port] := TRUE;
+									Indicate(port, UsbHcdi.Amber);
+								END;
+							ELSE (* New device found & addressed *)
+								hub.controller.Release;
+								IF InquiryDevice(dev) THEN
+									dev.Register(hub, port);
+									IF Debug.Verbose THEN ShowDevice(DeviceAttached, port+1, dev); END;
+									(* Try to install an appropriate USB device driver. If a driver is found, its Connect() procedure is called. *)
+									Usb.drivers.ProbeDevice(dev);
+									IF ~DriversInstalled(dev) THEN
+										(* We don't have a driver for this device. Suspend it. *)
+										(* res := SuspendPort(port); *)
+									END;
+									Indicate(port, UsbHcdi.Green);
+								ELSE
+									IF Debug.Level >= Debug.Default THEN Show("Failed to inquiry addressed device at port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+									IF ~DisablePort(port) THEN (* ignore res *) END;
+									Indicate(port, UsbHcdi.Amber);
+								END;
+							END;
+						ELSE (* ERROR: Couldn't enable port  *)
+							hub.controller.Release;
+							IF (hub.parent = hub) & hub.controller.isHighSpeed THEN
+								(* Lowspeed or fullspeed device connected to highspeed controller root hub? *)
+								status := GetPortStatus(port, FALSE);
+								IF (status * UsbHcdi.PortStatusEnabled = {}) & (status * UsbHcdi.PortStatusDevicePresent # {}) THEN
+								    	hub.controller.RoutePortToCompanion(port);
+								END;
+							ELSE
+								IF Debug.Level >= Debug.Default THEN Show("Could not enable port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+								hub.portPermanentDisabled[port] := TRUE;
+								Indicate(port, UsbHcdi.Amber);
+							END;
+						END;
+					ELSE
+						IF Debug.Level >= Debug.Default THEN Show("Device connected to permanently disabled port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+					END;
+
+				ELSE (* Device has been removed from port *)
+					IF hub.deviceAtPort[port] # NIL THEN (* Remove device and its driver instance from port *)
+						IF Debug.Verbose THEN ShowDevice(DeviceRemoved, port, hub.deviceAtPort[port]); END;
+						RemoveDeviceFromPort(port);
+					END;
+					res := DisablePort(port); (* ignore res *)
+					hub.portPermanentDisabled[port] := FALSE; (* Reset disabled status *)
+					hub.portErrors[port] := 0;
+					Indicate(port, UsbHcdi.Off);
+				END;
+			END;
+
+			(* sanity checks *)
+
+			status := GetPortStatus(port, FALSE);
+			IF status * UsbHcdi.PortStatusDevicePresent = {} THEN
+				IF hub.deviceAtPort[port] # NIL THEN
+					IF Debug.Level >= Debug.Warnings THEN Show("Port indicates no device present, but USB driver has one."); KernelLog.Ln; END;
+					RemoveDeviceFromPort(port);
+					hub.portPermanentDisabled[port] := FALSE; (* Reset disabled status *)
+				END;
+			END;
+
+			IF status * UsbHcdi.PortStatusEnabled # {} THEN (*Port is enabled -> a device should be connected to this port *)
+				IF hub.deviceAtPort[port] = NIL THEN
+					IF Debug.Level >= Debug.Warnings THEN Show("Port was enabled, but USB software did not know it!"); KernelLog.Ln; END;
+					RemoveDeviceFromPort(port);
+					res := DisablePort(port);
+					Indicate(port, UsbHcdi.Off);
+				END;
+			END;
+		END HandlePortStatusChange;
+
+		(**	Traverses the bus topology towards the root hub starting at the device associated to the specified pipe. *)
+		PROCEDURE GetTransactionTranslator(device : Usb.UsbDevice) : BOOLEAN;
+		VAR dev : Usb.UsbDevice;
+		BEGIN
+			dev := device;
+			IF dev.controller.isHighSpeed & (dev.speed # Usb.HighSpeed) THEN
+				(* Low-/Fullspeed device connected to high-speed bus via high-speed hub device. Find the high-speed hub device. *)
+				WHILE (dev.parent # NIL) & (dev.parent.speed # Usb.HighSpeed) DO dev := dev.parent; END;
+				IF dev # NIL THEN
+					device.ttAddress := dev.parent.address; device.ttPort := dev.port;
+					IF Debug.Trace & Debug.traceConnects THEN
+						Show("TT Address: "); KernelLog.Int(device.ttAddress, 0); KernelLog.String(", TT Port: "); KernelLog.Int(device.ttPort, 0); KernelLog.Ln;
+					END;
+					RETURN TRUE;
+				ELSIF (SELF IS RootHubDriver) & (device.parent = NIL) & ~device.controller.HasCompanion() THEN
+					device.ttAddress := 0; device.ttPort := 0;
+					IF Debug.Level >= Debug.Warnings THEN Show("Not assigning TT for device connected to root hub"); KernelLog.Ln; END;
+					RETURN TRUE
+				ELSE
+					IF Debug.Level >= Debug.Errors THEN Show("Could not find transaction translator."); KernelLog.Ln; END;
+					RETURN FALSE;
+				END;
+			ELSE
+				device.ttAddress := 0; device.ttPort := 0;
+				RETURN TRUE;
+			END;
+		END GetTransactionTranslator;
+
+		(*
+		 * When entering this procedure, the USB device is already in the default state, i.e. it is attached and powered.
+		 * This procedure will assign a USB device address to the device.
+		 * @param port where the USB device is attached to
+		 * @return USB device in addressed state
+		 *)
+		PROCEDURE GetAddressedDevice(port : LONGINT) : Usb.UsbDevice;
+		VAR
+			dev : Usb.UsbDevice; defaultpipe : UsbHcdi.Pipe;
+			descriptor : Usb.DeviceDescriptor;
+			adr : LONGINT;
+			status : SET;
+		BEGIN
+			IF Debug.Trace & Debug.traceConnects THEN Show("Assign address to device at port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+			status := GetPortStatus(port, FALSE);
+			IF status * UsbHcdi.PortStatusError # {} THEN
+				IF Debug.Level >= Debug.Errors THEN Show("GetAddressedDevice: Cannot get status of port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+				RETURN NIL;
+			ELSIF status * UsbHcdi.PortStatusDevicePresent = {} THEN
+				IF Debug.Level >= Debug.Errors  THEN Show("GetAddressedDevice: Device no more present ??"); KernelLog.Ln; END;
+				RETURN NIL;
+			END;
+
+			(* Create a new USB device object*)
+			NEW(dev);  NEW(descriptor);
+			dev.descriptor := descriptor;
+			dev.address := 0; (* Default address, since we did not yet assign an address to the device *)
+			dev.controller := hub.controller;
+			dev.parent := hub;
+			dev.port := port;
+			dev.SetState(Usb.StateDefault);
+
+			IF status * UsbHcdi.PortStatusLowSpeed # {} THEN
+				dev.speed := UsbHcdi.LowSpeed;
+			ELSIF status * UsbHcdi.PortStatusFullSpeed # {} THEN
+				dev.speed := UsbHcdi.FullSpeed;
+			ELSIF status * UsbHcdi.PortStatusHighSpeed # {} THEN
+				dev.speed := UsbHcdi.HighSpeed;
+			ELSE
+				IF Debug.Level >= Debug.Errors THEN Show("Device speed error"); KernelLog.Ln; END;
+				RETURN NIL;
+			END;
+
+			IF ~GetTransactionTranslator(dev) THEN
+				RETURN NIL;
+			END;
+
+			(* We link the default control pipe of the device that we're installing to the dummy default control pipe provided by the controller *)
+			defaultpipe := hub.controller.GetDefaultPipe(dev.speed, dev.ttPort, dev.ttAddress, dev);
+			IF defaultpipe = NIL THEN
+				IF Debug.Level >= Debug.Errors THEN Show("Couldn't get default pipe."); KernelLog.Ln; END;
+				RETURN NIL;
+			END;
+
+			(* Assign a USB device address to the device *)
+			adr := hub.controller.GetFreeAddress();
+			IF adr = 0 THEN (* Sorry, bus is full *)
+				KernelLog.String("Usb: Cannot configure device:  No free device addresses. "); KernelLog.Ln;
+				dev.FreePipe(defaultpipe);
+				RETURN NIL;
+			END;
+
+			dev.defaultpipe := defaultpipe;
+			(* SetAddress will set dev.address as side-effect *)
+			IF ~dev.SetAddress(adr) THEN
+				dev.FreePipe(dev.defaultpipe);
+				hub.controller.FreeAll(adr);
+				hub.controller.FreeAddress(adr);
+				IF Debug.Level >= Debug.Warnings THEN Show("Address Setup failed."); KernelLog.Ln;END;
+				RETURN NIL;
+			END;
+
+			(* Note that device is now in the "address" state. The SetAddress procedure has updated the dev.address field. *)
+			Wait(UsbHcdi.AddressRecoveryTime); (* 2ms recovery interval [USB2.0spec, p. 246]  *)
+			dev.SetState(Usb.StateAddress);
+
+			(* We don't need the dummy control pipe anymore... free it up... *)
+			dev.FreePipe(dev.defaultpipe);
+
+			RETURN dev;
+		END GetAddressedDevice;
+
+		(*
+		 * When entering this procedure, the USB device is already in the addressed state, i.e. it is attached, powered and
+		 * addressed. This procedure will read in all descriptors of the device and then configure the device, so when this procedure
+		 * is left, the device is in the state configured and can be used by USB device drivers.
+		 * @param dev
+		 * @return TRUE, if operation succeeded, FALSE otherwise
+		 *)
+		PROCEDURE InquiryDevice(dev : Usb.UsbDevice) : BOOLEAN;
+		VAR
+			defaultpipe, tempPipe : UsbHcdi.Pipe;
+			buffer : Usbdi.BufferPtr;
+		BEGIN
+			(* Okay. we have to build the default control pipe from the device now... *)
+			NEW(defaultpipe, dev.address, 0, dev.controller);
+			dev.defaultpipe := defaultpipe;
+			dev.defaultpipe.device := dev;
+			dev.defaultpipe.completion.device := dev;
+			dev.defaultpipe.address := dev.address;
+			dev.defaultpipe.maxRetries := 3;
+			dev.defaultpipe.type := UsbHcdi.PipeControl;
+			dev.defaultpipe.maxPacketSize := 8; 	(* Not yet known *)
+			dev.defaultpipe.speed := dev.speed;
+			dev.defaultpipe.timeout := Usb.DefaultTimeout;
+
+			IF GetTransactionTranslator(dev) THEN
+				dev.defaultpipe.ttAddress := dev.ttAddress;
+				dev.defaultpipe.ttPort := dev.ttPort;
+			ELSE
+				hub.controller.FreeAll(dev.address);
+				hub.controller.FreeAddress(dev.address);
+				RETURN FALSE;
+			END;
+
+			(* Register the default control pipe *)
+			hub.controller.GetPipe(dev.address, 0, dev.defaultpipe);
+			IF dev.defaultpipe = NIL THEN
+				IF Debug.Level >= Debug.Errors THEN Show("InquiryDevice: Could not register the default control pipe"); KernelLog.Ln; END;
+				hub.controller.FreeAll(dev.address);
+				hub.controller.FreeAddress(dev.address);
+				RETURN FALSE;
+			END;
+
+			(* We are only allowed to read 8 bytes until now - otherwise there could happen a babble error *)
+			NEW(buffer, 8);
+			IF ~dev.GetDescriptor(DescriptorDevice, 0, 0, 8, buffer) THEN
+				IF Debug.Level >= Debug.Errors THEN Show("InquiryDevice: Read first 8 bytes of device descriptor failed."); KernelLog.Ln; END;
+				hub.controller.FreeAll(dev.address);
+				hub.controller.FreeAddress(dev.address);
+				RETURN FALSE;
+			END;
+
+			dev.defaultpipe.maxPacketSize := ORD(buffer[7]);
+
+			(* We don't need the dummy control pipe anymore... free it up... *)
+			tempPipe := dev.defaultpipe;
+			dev.FreePipe(dev.defaultpipe);
+			tempPipe.device := dev; (* has been removed by FreePipe *)
+
+			hub.controller.GetPipe(dev.address, 0, tempPipe);
+
+			IF tempPipe = NIL THEN
+				IF Debug.Level >= Debug.Errors THEN Show("InquiryDevice: Could not register the default control pipe"); KernelLog.Ln; END;
+				hub.controller.FreeAll(dev.address);
+				hub.controller.FreeAddress(dev.address);
+				RETURN FALSE;
+			END;
+
+			dev.defaultpipe := tempPipe;
+			dev.defaultpipe.completion.device := dev;
+
+			(* okay, device is in adressed state...  we now parse the device descriptor *)
+			IF ~dev.GetDeviceDescriptor() OR ~dev.GetConfigurations()THEN
+				hub.controller.FreeAll(dev.address);
+				hub.controller.FreeAddress(dev.address);
+				IF Debug.Level >= Debug.Errors THEN Show("Parsing descriptors failed."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			(* If the attached device is USB2.0 complaint, we also load and parse the Device Qualifier and
+		 	the Other Speed Configurations *)
+		 	IF dev.descriptor.bcdUSB >= 0200H THEN
+		 		IF Debug.Trace & Debug.traceConnects THEN Show("Get device qualifier."); KernelLog.Ln; END;
+		 		IF ~dev.GetDeviceQualifier() THEN
+		 			IF Debug.Level >= Debug.Errors THEN Show("Couldn't get device qualifier."); KernelLog.Ln; END;
+		 		ELSIF dev.GetOtherSpeedConfigurations() THEN
+		 			IF ~dev.controller.isHighSpeed THEN
+		 				KernelLog.String("UsbHubDriver: Warning: Connected high-speed capable device to low-/full-speed controller."); KernelLog.Ln;
+		 			END;
+		 		ELSE
+		 			IF Debug.Level >= Debug.Errors THEN Show("Couldn't get other speed configurations"); KernelLog.Ln; END;
+		 		END;
+		 	END;
+
+		 	(* Check whether topology constrains are met and enough power is available *)
+		 	IF ~ValidTopology(dev, hub) THEN
+				hub.controller.FreeAll(dev.address);
+				hub.controller.FreeAddress(dev.address);
+				Show("Topology constraints violated. Cannot configure device."); KernelLog.Ln;
+				RETURN FALSE;
+			END;
+
+		 	IF ~EnoughPower(dev, hub) THEN
+				hub.controller.FreeAll(dev.address);
+				hub.controller.FreeAddress(dev.address);
+				Show("Not enough power available. Cannot configure device."); KernelLog.Ln;
+				RETURN FALSE;
+		 	END;
+
+			(* Enough bandwidth available? *)
+
+			(* Set Configuration *)
+			IF ~dev.SetConfiguration(0) THEN
+				hub.controller.FreeAll(dev.address);
+				hub.controller.FreeAddress(dev.address);
+				IF Debug.Level >= Debug.Errors THEN Show("Could not set configuration"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			dev.SetState(Usb.StateConfigured);
+
+		(*	IF AllowSuspend THEN (* Enable remote wakeup if supported. *)
+				IF ~dev.hubFlag & (dev.descriptor.bDeviceClass # 09H) & dev.actConfiguration(Usb.ConfigurationDescriptor).remoteWakeup THEN
+					IF ~dev.SetFeature(Device, 0, Usb.FsDeviceRemoteWakeup) THEN
+						IF Debug THEN Show("Warning: Could not enable remote wakeup."); END;
+					END;
+				END;
+			END; *)
+
+			(* Get sManufacturer, sProduct and sSerialNumber strings & interface/configurations descriptors *)
+			 Usb.GetStrings(dev);
+
+			RETURN TRUE;
+		END InquiryDevice;
+
+		PROCEDURE ParseHubDescriptor(buffer : Usbdi.Buffer) : BOOLEAN;
+		VAR i : LONGINT;
+		BEGIN
+			IF (LEN(buffer) < 2) OR (ORD(buffer[0]) < 7) OR (ORD(buffer[1]) # DescriptorHub) THEN RETURN FALSE; END;
+			nbrOfPorts := ORD(buffer[2]);
+			i := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, ORD(buffer[3])) * {0..1});
+			CASE i OF
+				0 : powerSwitching := Global;
+				|1 : powerSwitching := PerPort;
+			ELSE
+				powerSwitching := NotAvailable;
+			END;
+
+			IF SYSTEM.VAL(SET, ORD(buffer[3])) * {2} # {} THEN isCompound := TRUE; END;
+			IF SYSTEM.VAL(SET, ORD(buffer[3])) * {7} # {} THEN portIndicators := TRUE; END;
+
+			i := SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, ORD(buffer[3])) * {3..4}, -3));
+			CASE i OF
+				0 : ocProtection := Global;
+				|1 : ocProtection := PerPort;
+			ELSE
+				ocProtection := NotAvailable;
+			END;
+
+		 	thinkTime := SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, ORD(buffer[3])) * {3..4}, -3));
+			pwrOn2pwrGood := ORD(buffer[5]) * 2; (* PowerOn 2 PowerGood measured in 2ms steps *)
+		 	ctrlCurrent := ORD(buffer[6]);
+
+		(* 	IF (ORD(buffer[2]) - 7) > (nbrOfPorts DIV 8 + 1) THEN
+		 		NEW(deviceRemovable, nbrOfPorts);
+		 		FOR i := 0 TO nbrOfPorts - 1 DO
+			 		IF (SYSTEM.VAL(SET, ORD(buffer[7 + i DIV 8])) * SYSTEM.VAL(SET, i MOD 8) = {}) THEN
+			 			deviceRemovable[i] := TRUE;
+			 		END;
+			 	END;
+		 	END; *)
+		 	RETURN TRUE;
+		 END ParseHubDescriptor;
+
+		(* Load and parse the hub descriptor, power on all ports *)
+		PROCEDURE Connect() : BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr; len : LONGINT;
+		BEGIN
+			hub := device (Usb.UsbDevice);
+
+			(* First get the first 8 bytes of the hub descriptor to get its length and then load the full length hub desriptor *)
+			NEW(buffer, 2);
+			IF ~GetHubDescriptor(DescriptorHub, 0, 2, buffer) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbHubDriver: Could not get first two bytes of hub descriptor."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			len := ORD(buffer[0]); NEW(buffer, len);
+		 	IF ~GetHubDescriptor(DescriptorHub, 0, SYSTEM.VAL(LONGINT, len), buffer) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbHubDriver: Could not get hub descriptor."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			IF ~ParseHubDescriptor(buffer) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbHubDriver: Failed to parse hub descriptor."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+		 	hub.hubFlag := TRUE;
+			hub.nbrOfPorts := nbrOfPorts;
+			NEW(hub.deviceAtPort, nbrOfPorts);
+			NEW(hub.portPermanentDisabled, nbrOfPorts);
+			NEW(hub.portErrors, nbrOfPorts);
+
+			IF Debug.Trace & Debug.traceInfo THEN ShowInfo; END;
+
+			IF ~EnablePortPower(AllPorts) THEN
+				IF Debug.Level >= Debug.Errors THEN Show("Error: Could not enable port power"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			IF Debug.Verbose THEN Show(""); KernelLog.Int(nbrOfPorts, 0); KernelLog.String(" ports detected."); KernelLog.Ln; END;
+			RETURN Initialize();
+		END Connect;
+
+		PROCEDURE ValidTopology(dev, parent : Usb.UsbDevice) : BOOLEAN;
+		VAR segments : LONGINT; temp : Usb.UsbDevice;
+		BEGIN (* Should lock topology !! *)
+			IF dev.hubFlag THEN
+				(* Count cable segments between dev and the host *)
+				temp := dev;
+				WHILE temp.parent # dev DO
+					INC(segments);
+					temp := temp.parent;
+				END;
+				(* No more than 6 cable segments allowed between device and host (USB2.0, p ??) *)
+				IF segments > 6 THEN
+					Show("Bus topology constraint not met: maximum of 6 cable segment between device and host."); KernelLog.Ln;
+					RETURN FALSE;
+				END;
+			END;
+			RETURN TRUE;
+		END ValidTopology;
+
+		(* Can the hub parent provide enough power for the device dev ? *)
+		PROCEDURE EnoughPower(dev, parent : Usb.UsbDevice) : BOOLEAN;
+		VAR status : SET;
+		BEGIN
+			IF dev.GetStatus(Device, 0, status) THEN
+				(* TODO: Implement *)
+
+				(* Unfortunately, most hubs claim to be self-powered even when it's not the case... *)
+				IF status * Usb.SelfPowered # {} THEN
+					IF Debug.Trace & Debug.traceConnects THEN Show(""); dev.ShowName; KernelLog.String(" is self-powered."); KernelLog.Ln; END;
+				ELSE
+					IF Debug.Trace & Debug.traceConnects THEN Show(""); dev.ShowName; KernelLog.String(" is bus-powered."); KernelLog.Ln; END;
+				END;
+			ELSE
+				IF Debug.Level >= Debug.Errors THEN Show("GetStatus request failed."); KernelLog.Ln; END;
+			END;
+			RETURN TRUE;
+		END EnoughPower;
+
+		PROCEDURE Disconnect;
+		BEGIN
+			IF Debug.Verbose THEN Show(" disconnected."); KernelLog.Ln;END;
+		END Disconnect;
+
+		PROCEDURE Wait(ms : LONGINT);
+		BEGIN {EXCLUSIVE}
+			timer.Sleep(ms)
+		END Wait;
+
+		PROCEDURE &New*;
+		BEGIN
+			NEW(timer); (* Used by Wait *)
+		END New;
+
+		PROCEDURE ShowDevice(mode, port : LONGINT; dev : Usb.UsbDevice);
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT((dev # NIL) & ((mode = DeviceAttached) OR (mode = DeviceRemoved))); END;
+			KernelLog.String("UsbHubDriver: ");	dev.ShowName;
+			IF mode = DeviceAttached THEN
+				KernelLog.String(" attached to "); KernelLog.String(hub.controller.name); KernelLog.String(" port ");
+				KernelLog.Int(port, 0); KernelLog.String("."); KernelLog.Ln;
+			ELSE
+				KernelLog.String(" has been detached."); KernelLog.Ln;
+			END;
+		END ShowDevice;
+
+		PROCEDURE ShowInfo;
+		VAR i : LONGINT;
+		BEGIN
+			IF Debug.Trace THEN
+			Show(" Capabilities:"); KernelLog.Ln;
+			KernelLog.String("    Compound device: "); IF isCompound THEN KernelLog.String("Yes"); ELSE KernelLog.String("No"); END;
+			KernelLog.String(", Port indicator control: "); IF portIndicators THEN KernelLog.String("Yes"); ELSE KernelLog.String("No"); END;
+			KernelLog.String(", Power switching support: ");
+			IF powerSwitching = NotAvailable THEN KernelLog.String("n/a");
+			ELSIF powerSwitching = Global THEN KernelLog.String("Global");
+			ELSIF powerSwitching = PerPort THEN KernelLog.String("Per port");
+			ELSE KernelLog.String("Error: "); KernelLog.Int(powerSwitching, 0);
+			END;
+			KernelLog.String(", Overcurrent protection: ");
+			IF ocProtection = NotAvailable THEN KernelLog.String("n/a");
+			ELSIF ocProtection = Global THEN KernelLog.String("Global");
+			ELSIF ocProtection = PerPort THEN KernelLog.String("Per port");
+			ELSE KernelLog.String("Error: "); KernelLog.Int(ocProtection, 0);
+			END;
+			KernelLog.Ln;
+			KernelLog.String("    Power On 2 Power Good: "); KernelLog.Int(pwrOn2pwrGood, 0); KernelLog.String(" ms");
+			KernelLog.String(", Control logic current: "); KernelLog.Int(ctrlCurrent, 0); KernelLog.String(" mA");
+			KernelLog.String(", Think time: "); KernelLog.Int(thinkTime, 0); KernelLog.String(" ms"); KernelLog.Ln;
+			KernelLog.String("    Number of downstream ports: "); KernelLog.Int(nbrOfPorts, 0); KernelLog.Ln;
+			FOR i := 0 TO nbrOfPorts-1 DO
+				KernelLog.String("        Port "); KernelLog.Int(i, 0); KernelLog.String(": ");
+				IF (deviceRemovable # NIL) & deviceRemovable[i] THEN KernelLog.String("[Removable]"); END;
+				UsbHcdi.ShowPortStatus(GetPortStatus(i, FALSE));
+				KernelLog.Ln;
+			END;
+			KernelLog.Ln;
+			END;
+		END ShowInfo;
+
+		(* Displays message containing a description of this hub and the specified text to kernel log *)
+		PROCEDURE Show(CONST text : ARRAY OF CHAR);
+		BEGIN
+			KernelLog.String("UsbHubDriver: Hub "); hub.ShowName;
+			KernelLog.String(" attached to "); KernelLog.String(hub.controller.name); KernelLog.String(" port ");
+			KernelLog.Int(hub.port + 1, 0); KernelLog.String(": "); KernelLog.String(text);
+		END Show;
+
+	END HubDriver;
+
+TYPE
+
+	(* Implementation of the USB hub device specific parts of the Hub Driver *)
+	UsbHubDriver = OBJECT(HubDriver)
+	VAR
+		(* Hub device status pipe for status notifications *)
+		statusPipe : Usbdi.Pipe;
+		statusBuffer : Usbdi.BufferPtr;
+		statusPipeRetries : LONGINT;
+
+		(* This hub class specific request returns the hub descriptor. *)
+		PROCEDURE GetHubDescriptor(type, index, length : LONGINT; VAR buffer : Usbdi.Buffer) : BOOLEAN;
+		BEGIN
+			ASSERT(length >= 2);
+			RETURN (hub.defaultpipe.Request(ToHost + Class + Device, GetDescriptor, index + type*100H, 0, length, buffer) = Usbdi.Ok) &
+					 (ORD(buffer[1]) = type);
+		END GetHubDescriptor;
+
+		(* This hub class specific request overrides the hub descriptor. *)
+		 PROCEDURE SetHubDescriptor(type, index : LONGINT; buffer : Usbdi.Buffer) : BOOLEAN;
+		 BEGIN
+			ASSERT((LEN(buffer) >= 2) & (ORD(buffer[0]) = LEN(buffer)) & (ORD(buffer[1]) = type));
+		 	RETURN hub.defaultpipe.Request(ToDevice + Class + Device, SetDescriptor, index + type*100H, 0, LEN(buffer), buffer) = Usbdi.Ok;
+		 END SetHubDescriptor;
+
+		(* This hub class request resets a value reported in the hub status. *)
+		PROCEDURE ClearHubFeature(feature : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT((feature = HubLocalPowerChange) OR (feature = HubOverCurrentChange)); END; (* Valid feature selector *)
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Clear hub feature "); KernelLog.Int(feature, 0); KernelLog.Ln; END;
+			RETURN hub.defaultpipe.Request(ToDevice + Class + Device, ClearFeature, feature, 0, 0, Usbdi.NoData) = Usbdi.Ok;
+		END ClearHubFeature;
+
+		(* This hub class request sets a value reported in the hub status. *)
+		PROCEDURE SetHubFeature(feature : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT((feature = HubLocalPowerChange) OR (feature = HubOverCurrentChange)); END; (* Valid feature selector *)
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Set hub feature "); KernelLog.Int(feature, 0); KernelLog.Ln; END;
+			RETURN hub.defaultpipe.Request(ToDevice + Class + Device, SetFeature, feature, 0, 0, Usbdi.NoData) = Usbdi.Ok;
+		END SetHubFeature;
+
+		(* This hub class request resets a value reported in the port status. *)
+		PROCEDURE ClearPortFeature(feature, port,  selector : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.StrongChecks THEN
+				ASSERT(((feature # PortTest) & (feature # PortIndicator)) OR (selector = 0));
+				ASSERT((feature > 0) & (feature <= 22) & (feature # PortConnection)); (* Valid feature selector *)
+			END;
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Port "); KernelLog.Int(port + 1, 0); KernelLog.String(": Clear feature "); KernelLog.Int(feature, 0); KernelLog.Ln; END;
+			RETURN hub.defaultpipe.Request(ToDevice + Class + Other, ClearFeature, feature, (port + 1) + LSH(selector, 8), 0, Usbdi.NoData) = Usbdi.Ok;
+		END ClearPortFeature;
+
+		(* This hub class request sets a value reported in the hub status. *)
+		PROCEDURE SetPortFeature(feature, port, selector : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.StrongChecks THEN
+				ASSERT(((feature = PortTest) OR (feature = PortIndicator)) OR (selector = 0));
+				ASSERT((feature > 0) & (feature <= 22) & (feature # PortConnection)); (* Valid feature selector *)
+			END;
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Port "); KernelLog.Int(port + 1, 0); KernelLog.String(": Set feature "); KernelLog.Int(feature, 0); KernelLog.Ln; END;
+			RETURN hub.defaultpipe.Request(ToDevice + Class + Other, SetFeature, feature, (port + 1) + LSH(selector, 8), 0, Usbdi.NoData) = Usbdi.Ok;
+		END SetPortFeature;
+
+		(* This hub class request returns the current hub status and the states that have change since the previous acknowledgment. *)
+		PROCEDURE GetHubStatus(VAR hubstatus : SET) : BOOLEAN;
+		VAR buffer : Usbdi.BufferPtr;
+		BEGIN
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Get Hub Status."); KernelLog.Ln; END;
+			NEW(buffer, 4);
+			IF hub.defaultpipe.Request(ToHost + Class + Device, GetStatus, 0, 0, 4, buffer) = Usbdi.Ok THEN
+				hubstatus := SYSTEM.VAL(SET, SYSTEM.GET32(UsbBuffers.GetDataAddress(buffer)));
+				RETURN TRUE;
+			END;
+			RETURN FALSE;
+		END GetHubStatus;
+
+		(* This hub class request returns the current port status and the current value of the port status change bits.  *)
+		PROCEDURE GetPortStatus(port : LONGINT; ack : BOOLEAN) : SET;
+		VAR buffer : Usbdi.BufferPtr; s, portstatus : SET;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT(port >= 0); END;
+			IF Debug.Trace & Debug.traceHubRequests THEN
+				Show("Get port status of port "); KernelLog.Int(port + 1, 0); IF ack THEN KernelLog.String(" (ACK)"); END; KernelLog.Ln;
+			END;
+			NEW(buffer, 4);
+			IF hub.defaultpipe.Request(ToHost + Class + Other, GetStatus, 0, port+1, 4, buffer) = Usbdi.Ok THEN
+				s := SYSTEM.VAL(SET, SYSTEM.GET32(UsbBuffers.GetDataAddress(buffer)));
+
+				IF ack & (s * PsChangeMask # {}) THEN (* Acknowledge the changes *)
+					IF s * PsConnectStatusChange # {} THEN
+						IF ~ClearPortFeature(PortConnectionChange, port, 0) THEN
+							RETURN UsbHcdi.PortStatusError;
+						END;
+					END;
+					IF s * PsPortEnabledChange # {} THEN
+						IF ~ClearPortFeature(PortEnableChange, port, 0) THEN
+							RETURN UsbHcdi.PortStatusError;
+						END;
+					END;
+					IF s * PsSuspendChange # {} THEN
+						IF ~ClearPortFeature(PortSuspendChange, port, 0) THEN
+							RETURN UsbHcdi.PortStatusError;
+						END;
+					END;
+					IF s * PsOvercurrentChange # {} THEN
+						IF ~ClearPortFeature(PortOverCurrentChange, port, 0) THEN
+							RETURN UsbHcdi.PortStatusError;
+						END;
+					END;
+					IF s * PsResetChange # {} THEN
+						IF ~ClearPortFeature(PortResetChange, port, 0) THEN
+							RETURN UsbHcdi.PortStatusError;
+						END;
+					END;
+				END;
+
+				IF s * PsCurrentConnectStatus # {} THEN
+					portstatus := portstatus + UsbHcdi.PortStatusDevicePresent;
+					IF s * PsPortEnabled # {} THEN
+						portstatus := portstatus + UsbHcdi.PortStatusEnabled;
+						IF s * PsLowSpeed # {} THEN
+							portstatus := portstatus + UsbHcdi.PortStatusLowSpeed;
+						ELSIF s * PsHighSpeed # {} THEN
+							portstatus := portstatus + UsbHcdi.PortStatusHighSpeed;
+						ELSE
+							portstatus := portstatus + UsbHcdi.PortStatusFullSpeed;
+						END;
+					END;
+				END;
+
+				IF s * PsSuspend # {} THEN portstatus := portstatus + UsbHcdi.PortStatusSuspended; END;
+				IF s * PsOverCurrent # {} THEN portstatus := portstatus + UsbHcdi.PortStatusOverCurrent; END;
+				IF s * PsReset # {} THEN portstatus := portstatus + UsbHcdi.PortStatusReset; END;
+				IF s * PsPortPower # {} THEN portstatus := portstatus + UsbHcdi.PortStatusPowered; END;
+				IF s * PsPortTestMode # {} THEN portstatus := portstatus + UsbHcdi.PortStatusTestControl; END;
+				IF s * PsPortIndicators # {} THEN portstatus := portstatus + UsbHcdi.PortStatusIndicatorControl; END;
+				IF s * PsConnectStatusChange # {} THEN portstatus := portstatus + UsbHcdi.PortStatusConnectChange; END;
+				IF s * PsPortEnabledChange # {} THEN portstatus := portstatus + UsbHcdi.PortStatusEnabledChange; END;
+				IF s * PsSuspendChange # {} THEN portstatus := portstatus + UsbHcdi.PortStatusSuspendChange; END;
+				IF s * PsOvercurrentChange # {} THEN portstatus := portstatus + UsbHcdi.PortStatusOverCurrentChange; END;
+				IF s * PsResetChange # {} THEN portstatus := portstatus + UsbHcdi.PortStatusResetChange; END;
+				IF Debug.Trace & Debug.traceHubRequests THEN
+					Show("Status of port "); KernelLog.Int(port + 1, 0); UsbHcdi.ShowPortStatus(portstatus); KernelLog.Ln;
+				END;
+				RETURN portstatus;
+			ELSE
+				IF Debug.Level >= Debug.Errors THEN Show("Can't get port status of port "); KernelLog.Int(port+1, 0); KernelLog.Ln; END;
+				RETURN UsbHcdi.PortStatusError;
+			END;
+		END GetPortStatus;
+
+		(*
+		 * This handler is called when the hub's interrupt IN status pipe reports a change of either
+		 * the hub status or the status of a hub port.
+		 *)
+		PROCEDURE HandleStatusChange(status : Usbdi.Status; actLen : LONGINT);
+		VAR ignore : Usbdi.Status; i, port : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceConnects THEN
+				Show("Hub reports status change: "); FOR i := 0 TO LEN(statusBuffer)-1 DO KernelLog.Hex(ORD(statusBuffer[i]), -2); END; KernelLog.Ln;
+			END;
+			IF (status = Usbdi.Ok) OR ((status = Usbdi.ShortPacket) & (actLen > 0)) THEN
+				IF SYSTEM.VAL(SET, statusBuffer[0]) * {0} # {} THEN (* Hub status changed *)
+					IF Debug.Trace & Debug.traceConnects THEN Show("Hub status changed."); END;
+					statusBuffer[0] := SYSTEM.VAL(CHAR, SYSTEM.VAL(SET, statusBuffer[0]) - {0}); (* Clear hub status change bit *)
+					HandleHubStatusChange;
+				END;
+				(* Look for port status changes *)
+				FOR i := 0 TO actLen-1 DO
+					FOR port := 0 TO 7 DO
+						IF SYSTEM.VAL(SET, statusBuffer[i]) * {port} # {} THEN
+							HandlePortStatusChange(port + i * 8 - 1);
+						END;
+					END;
+				END;
+				ignore := statusPipe.Transfer(statusPipe.maxPacketSize, 0, statusBuffer);
+				statusPipeRetries := 0;
+			ELSE
+				IF statusPipeRetries > StatusPipeMaxRetries THEN
+					IF Debug.Level >= Debug.Errors THEN Show("Status pipe error "); UsbHcdi.ShowStatus(status); KernelLog.Ln; END;
+					RETURN; (* give up *)
+				END;
+				IF (status = Usbdi.Stalled) THEN
+					IF ~statusPipe.ClearHalt() THEN
+						IF Debug.Level >= Debug.Errors THEN Show("Could not recover from status pipe error."); KernelLog.Ln; END;
+						RETURN;
+					END;
+				ELSIF (status = Usbdi.Disconnected) THEN
+					RETURN;
+				END;
+				ignore := statusPipe.Transfer(statusPipe.maxPacketSize, 0, statusBuffer);
+				INC(statusPipeRetries);
+			END;
+		END HandleStatusChange;
+
+		(* USB hub device specific initialization *)
+		PROCEDURE Initialize() : BOOLEAN;
+		VAR endpoint : Usbdi.EndpointDescriptor; ignore : Usbdi.Status;
+		BEGIN
+			(* Look for the hub's interrupt endpoint which is used to communicate status changes *)
+			endpoint := hub.actConfiguration.interfaces[0].endpoints[0];
+			ASSERT(endpoint.type = Usbdi.InterruptIn);
+			statusPipe := hub.GetPipe(endpoint.bEndpointAddress);
+			IF statusPipe = NIL THEN
+				IF Debug.Level >= Debug.Errors THEN Show("Could not establish status pipe."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			NEW(statusBuffer, statusPipe.maxPacketSize);
+			statusPipe.SetTimeout(0); (* Non-blocking pipe *)
+			statusPipe.SetCompletionHandler(HandleStatusChange);
+			ignore := statusPipe.Transfer(statusPipe.maxPacketSize, 0, statusBuffer);
+
+			RETURN TRUE;
+		END Initialize;
+
+		(*
+		 * This hub class specific request clears the state of the Transaction Translator (TT) bulk/control transfer after
+		 * it has been left in a busy state due to high-speed errors. This request is only defined for non-periodic endpoints.
+		 *)
+		PROCEDURE ClearTTBuffer(dev : Usb.UsbDevice; endpoint, port : LONGINT) : BOOLEAN;
+		VAR intf : Usb.InterfaceDescriptor; endp : Usb.EndpointDescriptor; wValue : SET; i, e : LONGINT;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT((dev.speed # Usb.HighSpeed) & (dev.parent.speed = Usb.HighSpeed)); END;
+			(* Get the endpoint *)
+			LOOP (* Search all interfaces *)
+				IF i > dev.actConfiguration.bNumInterfaces-1 THEN EXIT END;
+				intf := dev.actConfiguration.interfaces[i] (Usb.InterfaceDescriptor);
+				FOR e := 0 TO LEN(intf.endpoints)-1 DO (* Search all endpoints *)
+					IF intf.endpoints[e].bEndpointAddress = endpoint THEN (* Endpoint found *)
+						endp := intf.endpoints[e] (Usb.EndpointDescriptor);
+					END;
+				END;
+				IF endp # NIL THEN EXIT END;
+				INC(i);
+			END;
+			IF endp = NIL THEN (* Endpoint not found *) RETURN FALSE END;
+			IF (endp.bmAttributes * {0,1} # {}) OR (endp.bmAttributes * {0,1} # {1}) THEN
+				IF Debug.Level >= Debug.Warnings THEN Show("ClearTTBuffer error: Only allowed for non-periodic endpoints"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			(* wValue: {0..3}: Endpoint Number, {4..10}: Device Address, {11..12}: Endpoint Type, {13..13}: Reserved, {15}: Endpoint Direction *)
+			wValue := SYSTEM.VAL(SET, endp.bEndpointAddress) * {0..3} + LSH(SYSTEM.VAL(SET, dev.address), 4) * {4..10};
+			wValue := wValue + LSH(endp.bmAttributes ,11) * {11..12} + LSH(SYSTEM.VAL(SET, endp.bEndpointAddress) * {7}, 8);
+			RETURN hub.defaultpipe.Request(ToDevice + Class + Other, ClearTtBuffer, SYSTEM.VAL(LONGINT, wValue), (port + 1), 0, Usbdi.NoData) = Usbdi.Ok;
+		END ClearTTBuffer;
+
+		(*
+		 * This hub class specific request returns the internal state of the Transaction Translator (TT) in a vendor specific format.
+		 * A TT receiving this request must have first been stopped using the StopTTRequest.
+		 *)
+		PROCEDURE GetTTState(flags, port, len : LONGINT; VAR buffer : Usbdi.Buffer) : BOOLEAN;
+		BEGIN
+			RETURN hub.defaultpipe.Request(ToDevice + Class + Other, GetTtState, flags, (port + 1), len, buffer) = Usbdi.Ok;
+		END GetTTState;
+
+		(*
+		 * This hub class specific request returns the Transaction Translator (TT) in a hub to a known state.
+		 * After the reset is completed, the TT can resume its normal operation.
+		 *)
+		 PROCEDURE ResetTT(port : LONGINT) : BOOLEAN;
+		 BEGIN
+		 	RETURN hub.defaultpipe.Request(ToDevice + Class + Other, ResetTt, 0, (port + 1), 0, Usbdi.NoData) = Usbdi.Ok;
+		 END ResetTT;
+
+		(*
+		 * This hub class specific request stops the normal execution of the Transaction Translator (TT) so that the internal
+		 * state can be retrieved via GetTTState. This request is provided for debugging purposes.
+		 *)
+		PROCEDURE StopTT(port : LONGINT) : BOOLEAN;
+		BEGIN
+			RETURN hub.defaultpipe.Request(ToDevice + Class + Other, StopTt, 0, (port + 1), 0, Usbdi.NoData) = Usbdi.Ok;
+		END StopTT;
+
+	END UsbHubDriver;
+
+TYPE
+
+	(* Implementation of the USB root hub specific parts of the Hub Driver *)
+	RootHubDriver = OBJECT (HubDriver)
+	VAR
+		(* Root hub management *)
+		next: RootHubDriver;
+
+		(* Active object handling *)
+		timerRH : Kernel.Timer;
+		alive, dead, statusChange : BOOLEAN;
+		pollingInterval : LONGINT;
+
+		(* Will be true when Connect() returns. Used to synchronize active body *)
+		initialized : BOOLEAN;
+
+		(* Get the emulated hub descriptor. Ignore type and index parameters. *)
+		PROCEDURE GetHubDescriptor(type, index, length : LONGINT;  VAR buffer : Usbdi.Buffer) : BOOLEAN;
+		VAR i : LONGINT; hd : UsbHcdi.HubDescriptor;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT(LEN(buffer) <= length); END;
+			hd := device(Usb.UsbDevice).controller.GetHubDescriptor();
+			IF hd = NIL THEN RETURN FALSE END;
+			IF length > LEN(hd) THEN length := LEN(hd); END;
+			FOR i := 0 TO length-1 DO buffer[i] := hd[i]; END;
+			RETURN TRUE;
+		END GetHubDescriptor;
+
+		(* Overwrites the emulated hub descriptor. Ignore type and index paramters. *)
+		 PROCEDURE SetHubDescriptor(type, index : LONGINT; buffer : Usbdi.Buffer) : BOOLEAN;
+		 VAR i : LONGINT; hd : UsbHcdi.HubDescriptor;
+		 BEGIN
+		 	IF Debug.StrongChecks THEN ASSERT((LEN(buffer)>=8) & (ORD(buffer[0])=LEN(buffer)) & (ORD(buffer[1])=type)); END;
+		 	NEW(hd, LEN(buffer));
+		 	FOR i := 0 TO LEN(buffer)-1 DO hd[i] := buffer[i]; END;
+		 	device(Usb.UsbDevice).controller.SetHubDescriptor(hd);
+		 	RETURN TRUE;
+		 END SetHubDescriptor;
+
+		(* Clear a root hub feature. *)
+		PROCEDURE ClearHubFeature(feature : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT((feature = HubLocalPowerChange) OR (feature = HubOverCurrentChange)); END; (* Valid feature selector *)
+			(* TODO: Do nothing? *)
+			RETURN TRUE;
+		END ClearHubFeature;
+
+		(* Set a root hub feature *)
+		PROCEDURE SetHubFeature(feature : LONGINT) : BOOLEAN;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT((feature = HubLocalPowerChange) OR (feature = HubOverCurrentChange)); END; (* Valid feature selector *)
+			(* TODO: Do nothing? *)
+			RETURN TRUE;
+		END SetHubFeature;
+
+		(* Clear a root hub port feature. *)
+		PROCEDURE ClearPortFeature(feature, port, selector : LONGINT) : BOOLEAN;
+		VAR res : BOOLEAN;
+		BEGIN
+			IF Debug.StrongChecks THEN
+				ASSERT((port >= 0) & (port < nbrOfPorts));
+				ASSERT(((feature # PortTest) & (feature # PortIndicator)) OR (selector = 0));
+				ASSERT((feature > 0) & (feature <= 22) & (feature # PortConnection)); (* Valid feature selector *)
+			END;
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Port "); KernelLog.Int(port + 1, 0); KernelLog.String(": Clear feature "); KernelLog.Int(feature, 0); KernelLog.Ln; END;
+			CASE feature OF
+				PortEnable : hub.controller.DisablePort(port); res := TRUE;
+				| PortSuspend : res := hub.controller.ResumePort(port);
+				| PortPower: hub.controller.DisablePortPower(port); res := TRUE;
+				| PortIndicator: hub.controller.IndicatePort(port, selector); res := TRUE;
+				| PortConnectionChange:
+				| PortResetChange:
+				| PortEnableChange:
+				| PortSuspendChange:
+				| PortOverCurrentChange:
+			ELSE
+				IF Debug.Level >= Debug.Warnings THEN Show("Clearing of Feature "); KernelLog.Int(feature, 0); KernelLog.String(" not supported"); KernelLog.Ln; END;
+			END;
+			RETURN res;
+		END ClearPortFeature;
+
+		(* Set a root hub port feature *)
+		PROCEDURE SetPortFeature(feature, port, selector : LONGINT) : BOOLEAN;
+		VAR res : BOOLEAN;
+		BEGIN
+			IF Debug.StrongChecks THEN
+				ASSERT((port >= 0) & (port < nbrOfPorts));
+				ASSERT(((feature = PortTest) OR (feature = PortIndicator)) OR (selector = 0));
+				ASSERT((feature > 0) & (feature <= 22) & (feature # PortConnection)); (* Valid feature selector *)
+			END;
+			IF Debug.Trace & Debug.traceHubRequests THEN Show("Port "); KernelLog.Int(port + 1, 0); KernelLog.String(": Set feature "); KernelLog.Int(feature, 0); KernelLog.Ln; END;
+			CASE feature OF
+				PortEnable : res := hub.controller.ResetAndEnablePort(port);
+				| PortSuspend : res := hub.controller.SuspendPort(port);
+				| PortPower: hub.controller.EnablePortPower(port); res := TRUE;
+				| PortReset: res := hub.controller.ResetAndEnablePort(port);
+				| PortTest:
+				| PortIndicator: hub.controller.IndicatePort(port, selector); res := TRUE;
+				| PortConnectionChange:
+				| PortResetChange:
+				| PortEnableChange:
+				| PortSuspendChange:
+				| PortOverCurrentChange:
+			ELSE
+				IF Debug.Level >= Debug.Warnings THEN Show("Request not supported"); KernelLog.Ln; END;
+			END;
+			RETURN res;
+		END SetPortFeature;
+
+		(* Return the root hubs status. Reported: Local power supply good & Overcurrent *)
+		PROCEDURE GetHubStatus(VAR hubstatus : SET) : BOOLEAN;
+		BEGIN
+			(* HsLocalPowerLost and HsLocalPowerSourceChange are never set since root hubs cannot not loose power *)
+			hubstatus := {};
+			(* TODO: report global overcurrent here *)
+			RETURN TRUE;
+		END GetHubStatus;
+
+		(* Get the status of the specifed root hub port. Note that the HCD is responsible for acknowledging changes. *)
+		PROCEDURE GetPortStatus(port : LONGINT; ack : BOOLEAN) : SET;
+		BEGIN
+			IF Debug.StrongChecks THEN ASSERT((port >= 0) & (port < nbrOfPorts)); END;
+			RETURN hub.controller.GetPortStatus(port, ack);
+		END GetPortStatus;
+
+		(* Root hubs that support interrupt notification for port status changes will call this
+		handler when a corresponding interrupt occurs. The parameters are ignored. *)
+		PROCEDURE HandleStatusChange(status : Usbdi.Status; actLen : LONGINT);
+		BEGIN {EXCLUSIVE}
+			statusChange := TRUE;
+		END HandleStatusChange;
+
+		(* How much current (mA) is available for this hub? *)
+		PROCEDURE AvailableCurrent() : LONGINT;
+		BEGIN
+			RETURN 500; (* High power port delivers 500mA *)
+		END AvailableCurrent;
+
+		(* Active object control *)
+		PROCEDURE Terminate; BEGIN {EXCLUSIVE} alive:=FALSE; timerRH.Wakeup; END Terminate;
+		PROCEDURE SetDead; BEGIN {EXCLUSIVE} dead := TRUE; END SetDead;
+		PROCEDURE AwaitDead;	BEGIN {EXCLUSIVE} AWAIT(dead); END AwaitDead;
+
+		(* Root hub specific initialization *)
+		PROCEDURE Initialize() : BOOLEAN;
+		BEGIN
+			IF hub.controller.SetStatusChangeHandler(HandleStatusChange) THEN
+				(* Root hub driver will be wake up via interrupt notification *)
+				pollingInterval := 0;
+			END;
+			BEGIN {EXCLUSIVE} initialized := TRUE; END;
+			RETURN TRUE;
+		END Initialize;
+
+		(* Displays message containing a description of this hub and the specified text to kernel log *)
+		PROCEDURE Show(CONST text : ARRAY OF CHAR);
+		BEGIN
+			KernelLog.String("UsbHubDriver: Root Hub "); hub.ShowName; KernelLog.String(": "); KernelLog.String(text);
+		END Show;
+
+		PROCEDURE Disconnect;
+		BEGIN
+			Terminate; AwaitDead;
+			IF Debug.Verbose THEN Show("Disconnected."); KernelLog.Ln; END;
+		END Disconnect;
+
+		PROCEDURE &New*;
+		BEGIN
+			New^; NEW(timerRH);
+			alive := TRUE; dead := FALSE; initialized := FALSE;
+			pollingInterval := PollingInterval;
+		END New;
+
+		BEGIN {ACTIVE}
+			(* Root hubs use a different way to communicate root hub port status changes. Either, they cannot 			*)
+			(* report there changes at all and must be polled (e.g. UHCI host controllers), or they use interrupt driven	*)
+			(* global status change notification (e.g. OHCI and EHCI host controllers). 									*)
+			BEGIN {EXCLUSIVE} AWAIT(initialized OR ~alive); END;
+			WHILE alive DO
+				(* The first time we poll the bus (force bus enumeration) *)
+				LookForDevices;
+				IF pollingInterval = 0 THEN (* Use interrupt handler port status change notification *)
+					BEGIN {EXCLUSIVE}
+						AWAIT((alive = FALSE) OR (statusChange = TRUE));
+						statusChange := FALSE;
+					END;
+				ELSE (* Use polling *)
+					timerRH.Sleep(pollingInterval);
+				END;
+			END;
+			SetDead;
+		END RootHubDriver;
+
+VAR
+	(* This is a linked list of all running root hub drivers. It's only used by the module termination handler. *)
+	rootHubs : RootHubDriver;
+
+(* This is the Probe procedure of the internal USB hub driver / root hub driver. *)
+PROCEDURE Probe(dev : Usbdi.UsbDevice; id : Usbdi.InterfaceDescriptor) : Usbdi.Driver;
+VAR hubDriver : UsbHubDriver; rootHubDriver : RootHubDriver;
+BEGIN
+	IF dev.descriptor.bNumConfigurations # 1 THEN RETURN NIL; END;
+	IF dev.configurations[0].bNumInterfaces # 1 THEN RETURN NIL; END;
+
+	IF id.bInterfaceClass # 9 THEN RETURN NIL; END;
+	IF id.bInterfaceSubClass # 0 THEN RETURN NIL; END;
+	IF id.bNumEndpoints # 1 THEN RETURN NIL; END;
+
+	IF dev(Usb.UsbDevice).parent = dev THEN (* It's a root hub *)
+		NEW(rootHubDriver);
+		(* Insert at head of root hub driver linked list *)
+		rootHubDriver.next := rootHubs; rootHubs := rootHubDriver;
+		RETURN rootHubDriver;
+	ELSE (* It's a hub device attached to the bus *)
+		NEW(hubDriver);
+		RETURN hubDriver;
+	END;
+END Probe;
+
+PROCEDURE Cleanup;
+VAR rh : RootHubDriver;
+BEGIN
+	rh := rootHubs;
+	WHILE(rh # NIL) DO
+		IF Debug.Verbose THEN rh.Show("Shutting down... "); KernelLog.Ln; END;
+		rh.Terminate; rh.AwaitDead;
+		rh := rh.next;
+	END;
+	Usbdi.drivers.Remove(Name);
+	IF Debug.Verbose THEN KernelLog.Enter; KernelLog.String("UsbHubDriver: Removed hub driver."); KernelLog.Exit; END;
+END Cleanup;
+
+(** Install the USB Hub Driver *)
+PROCEDURE Install*;
+END Install;
+
+BEGIN
+	Modules.InstallTermHandler(Cleanup);
+	Usbdi.drivers.Add(Probe, Name, Description, 10);
+END UsbHubDriver.
+
+UsbHubDriver.Install ~  SystemTools.Free UsbHubDriver ~

source/ARM.UsbKeyboard.Mod

@@ -0,0 +1,758 @@
+MODULE UsbKeyboard; (** AUTHOR "cplattner; staubesv"; PURPOSE "Bluebottle USB Keyboard Driver" *) (** non-portable **)
+(**
+ * Bluebottle USB Keyboard Driver (HID boot protocol)
+ *
+ * Usage:
+ *
+ *	UsbKeyboard.Install ~ loads this driver		SystemTools.Free UsbKeyboard ~ unloads it
+ *
+ *	UsbKeyboard.SetLayout dev file ~ sets the keyboard layout
+ * 	UsbKeyboard.SetLayout UsbKeyboard00 KeyCH.Bin ~ sets the CH keyboard layout, for example
+ *
+ * References:
+ *
+ *	Device Class Definition for Human Interface Devices (HID), version 1.11
+ *	HID Usage Tables, version 1.11
+ *
+ *	References are available at http://www.usb.org
+ *
+ * History:
+ *
+ *	30.09.2000 	cp first release
+ *	18.10.2000 	cp fix size of interrupt endpoint and add warning message if keyboard fails
+ *	27.02.2006	Correct handling for modifier keys (also generate event if only a modifier key is pressed) (staubesv)
+ *	01.03.2006	Added SetLayout & KeyboardDriver.SetLayout (staubesv)
+ *	22.01.2007	Splitted up Keyboard Driver for HID compatibility (ottigerm)
+ *	26.06.2014	Basic port to ARM (tmartiel)
+ *)
+
+IMPORT SYSTEM, Machine, Files, Inputs, Commands, KernelLog, Streams, Plugins, Modules, Usb, Usbdi, UsbHid;
+
+CONST
+
+	Name = "UsbKeyboard";
+	Description = "USB Keyboard Driver";
+	Priority = 10;
+
+	NumLock* = 0;
+	CapsLock* = 1;
+	ScrollLock* = 2;
+	(* Compose & kana not yet implemented *)
+	Compose = 3;
+	Kana = 4;
+
+	(* If you press a key and hold it down, the following will happen:					*)
+	(* 1. A Inputs.KeyboardMsg is sent											*)
+	(* 2. No further messages are sent until the period KeyDeadTime expires			*)
+	(* 3. Further messages are sent with the interval KeyDeadTimeRepeat				*)
+	(*																				*)
+	(* A release event is sent when you release the key.								*)
+	(* The values KeyDeadTime and KeyDeadTimeRepeat are set in milliseconds. 		*)
+	(*																				*)
+	KeyDeadTime* = 100;
+	KeyDeadTimeRepeat* = 0;  (* 10 <= value < infinity  && value mod 10 = 0 *)
+
+	TraceKeys* = FALSE; (* Displays scan code of pressed key on KernelLog if TRUE *)
+	Debug* = TRUE;
+
+TYPE
+
+	Key* = RECORD
+		ch* : CHAR;
+		keysym* : LONGINT;
+		counter* : LONGINT;
+		repeat* : BOOLEAN;
+		updated* : BOOLEAN;
+	END;
+
+TYPE
+
+	KeyboardBase*=OBJECT
+	VAR
+		msg*, lastMsg : Inputs.KeyboardMsg;
+		lastFlags : SET;
+
+		numKeyVal : LONGINT;
+		deadKey* : LONGINT;
+		dkHack* : LONGINT;  (* deadKey value should persist Release events ... *)
+
+		(* Status of NumLock,ScrollLock,CapsLock,Compose & Kana *)
+		leds*, lastLeds* : SET;
+		ledBuffer* : Usbdi.BufferPtr;
+
+		keyboardFileTable : POINTER TO ARRAY OF CHAR;
+		keytable* : ADDRESS; (* used as pointer to keyboardFileTable[0] *)
+
+		keyDeadTime*, keyDeadTimeRepeat* : LONGINT;
+
+
+		PROCEDURE HandleKey*(c : CHAR);
+		VAR k : LONGINT;
+		BEGIN
+			(* map USB Usage ID to keysym: Only non-alphanumeric keys are mapped by Keysym()  *)
+			msg.keysym := KeySym(c, leds);
+			IF TraceKeys THEN KernelLog.String("USB Usage ID: "); KernelLog.Hex(ORD(c), -3); END;
+			(* map USB Usage ID to Oberon key code *)
+			SYSTEM.GET(UsbScanTab() + ORD(c), c);
+			IF TraceKeys THEN KernelLog.String(" -> Oberon key code: "); KernelLog.Hex(ORD(c), -3) END;
+
+			IF c = CHR(58) THEN  leds := leds / {CapsLock};
+			ELSIF c = CHR(69) THEN leds := leds / {NumLock};
+			ELSIF c = CHR(70) THEN leds := leds / {ScrollLock};
+			ELSE
+				k := Translate(msg.flags, leds, c, keytable, deadKey, numKeyVal);
+				IF TraceKeys THEN KernelLog.String(" translated into: "); KernelLog.Char(CHR(k)); END;
+				(* if c is an ASCII character, then map c to keysym *)
+				IF (k  >= 1) & (k  <= 126) & (msg.keysym = Inputs.KsNil)  THEN msg.keysym := k; END;
+				IF k >= 0 THEN msg.ch := CHR(k) ELSE msg.ch := 0X END;
+				IF TraceKeys THEN
+					KernelLog.String(" Aos Keysym: "); IF msg.keysym = Inputs.KsNil THEN KernelLog.String("No Key"); ELSE KernelLog.Hex(msg.keysym, 9); END;
+					KernelLog.Ln; ShowFlags(msg.flags, leds); KernelLog.Ln;
+				END;
+				(* build up message for this event *)
+				IF (msg.flags # lastMsg.flags) OR (msg.ch # 0X) OR (msg.keysym # Inputs.KsNil) THEN
+					Inputs.keyboard.Handle(msg);
+				END;
+				lastMsg := msg;
+			END;
+		END HandleKey;
+
+		PROCEDURE HandleModifiers*(flags : SET);
+		VAR i : LONGINT;
+		BEGIN
+			IF flags # lastFlags THEN
+				msg.flags := {}; msg.ch := 0X; msg.keysym := Inputs.KsNil;
+				FOR i := 0 TO MAX(SET) DO
+					IF (i IN flags) & ~(i IN lastFlags) THEN (* modifier key pressed for the first time *)
+						msg.flags := {i}; msg.keysym := GetModifierKeysym(i);
+						Inputs.keyboard.Handle(msg);
+					ELSIF ~(i IN flags) & (i IN lastFlags) THEN (* modifier key released *)
+						msg.flags := {Inputs.Release}; msg.keysym := GetModifierKeysym(i);
+						Inputs.keyboard.Handle(msg);
+					END;
+				END;
+			END;
+			lastFlags := flags;
+		END HandleModifiers;
+
+		PROCEDURE TableFromFile*(CONST name: ARRAY OF CHAR): ADDRESS;
+		VAR f: Files.File; r: Files.Rider; len: LONGINT;
+		BEGIN
+			KernelLog.String("UsbKeyboard: "); KernelLog.String(" Loading layout "); KernelLog.String(name); KernelLog.Ln;
+			f := Files.Old(name);
+			IF f # NIL THEN
+				len := f.Length();
+				IF len MOD 4 = 0 THEN
+					NEW(keyboardFileTable, len+1);
+					f.Set(r, 0); f.ReadBytes(r, keyboardFileTable^, 0, len);
+					IF r.res = 0 THEN
+						keyboardFileTable[len] := 0FFX;
+						RETURN ADDRESSOF(keyboardFileTable[0])
+					ELSIF Debug THEN KernelLog.String("UsbKeyboard: TableFromFile: Error: res="); KernelLog.Int(r.res, 1); KernelLog.Ln;
+					END
+				ELSIF Debug THEN KernelLog.String("UsbKeyboard: TableFromFile: Error: len="); KernelLog.Int(len, 1); KernelLog.Ln;
+				END
+			ELSIF Debug THEN KernelLog.String("UsbKeyboard: TableFromFile: Error: File not found."); KernelLog.Ln;
+			END;
+			RETURN -1;
+		END TableFromFile;
+
+		PROCEDURE SetLayout*(CONST name : ARRAY OF CHAR);
+		VAR adr : ADDRESS;
+		BEGIN
+			IF name = "KeyUS.Bin" THEN adr := TableUS();
+			ELSE adr := TableFromFile(name);
+			END;
+			IF adr = -1 THEN (* Leave the current setting *)
+			ELSE SYSTEM.PUT(ADDRESSOF(keytable), adr);
+			END;
+		END SetLayout;
+
+	END KeyboardBase;
+
+	KeyboardDriver = OBJECT (UsbHid.HidDriver)
+	VAR
+		pipe : Usbdi.Pipe;
+
+ 		(* buffer[0] : modifier byte					*)
+		(* buffer[1] : reserved						*)
+		(* buffer[2]-buffer[7] : 6 one byte key codes  	*)
+		buffer : Usbdi.BufferPtr;
+
+		base : KeyboardBase;
+
+		(*for keeing the pressed keys in mind*)
+		pressed* : ARRAY 6 OF Key;
+
+		PROCEDURE &Init*;
+		BEGIN
+			NEW(base);
+		END Init;
+
+		PROCEDURE EventHandler(status : Usbdi.Status; actLen : LONGINT);
+		VAR
+			i, j : LONGINT;
+			c : CHAR;
+			modifiers, flags : SET;
+			res : BOOLEAN;
+			tempPressed : ARRAY 6 OF Key;
+			found, kill : BOOLEAN;
+		BEGIN
+			IF (status=Usbdi.Ok) OR ((status = Usbdi.ShortPacket) & (actLen >= 8)) THEN
+
+				(* evaluate modifier keys *)
+				base.msg.flags := {};
+				modifiers := SYSTEM.VAL(SET, buffer[0]);
+				IF modifiers * {0} # {} THEN INCL(base.msg.flags, Inputs.LeftCtrl) END;
+				IF modifiers * {1} # {} THEN INCL(base.msg.flags, Inputs.LeftShift) END;
+				IF modifiers * {2} # {} THEN INCL(base.msg.flags, Inputs.LeftAlt) END;
+				IF modifiers * {3} # {} THEN INCL(base.msg.flags, Inputs.LeftMeta) END;
+				IF modifiers * {4} # {} THEN INCL(base.msg.flags, Inputs.RightCtrl) END;
+				IF modifiers * {5} # {} THEN INCL(base.msg.flags, Inputs.RightShift) END;
+				IF modifiers * {6} # {} THEN INCL(base.msg.flags, Inputs.RightAlt) END;
+				IF modifiers * {7} # {} THEN INCL(base.msg.flags, Inputs.RightMeta) END;
+				flags := base.msg.flags;
+
+				(* evaluate the six keycodes *)
+				FOR i := 2 TO 7 DO
+					c := buffer[i];
+					IF c # CHR(0) THEN (* buffer[i] contains key code *)
+
+						(* check whether the key is pressed for the first time, is still being pressed or has been released *)
+						FOR j := 0 TO 5 DO
+
+							IF pressed[j].ch = c THEN (* key is still pressed *)
+								found := TRUE;
+								pressed[j].updated := TRUE;
+
+								tempPressed[i-2].counter := pressed[j].counter + 1;
+								tempPressed[i-2].ch := pressed[j].ch;
+								tempPressed[i-2].keysym := pressed[j].keysym;
+								tempPressed[i-2].updated := FALSE;
+								tempPressed[i-2].repeat := pressed[j].repeat;
+
+								IF pressed[j].repeat THEN
+									IF (base.keyDeadTimeRepeat # 0) & (tempPressed[i-2].counter MOD base.keyDeadTimeRepeat # 0) THEN (* don't send key event *) kill := TRUE; END;
+								ELSE
+									IF tempPressed[i-2].counter MOD base.keyDeadTime # 0 THEN (* don't send key event *)
+										kill := TRUE;
+									ELSE
+										tempPressed[i-2].repeat := TRUE;
+									END;
+								END;
+							END;
+					    	END;
+					 END;
+
+					IF ~found THEN (* the key has not been pressed down before *)
+						tempPressed[i-2].ch := c;
+						tempPressed[i-2].repeat := FALSE;
+						tempPressed[i-2].updated := FALSE;
+						tempPressed[i-2].counter := 1;
+					END;
+
+				    (* kill : Key is pressed but do not generate key event this time -> repeat rate ... *)
+				    IF (c # CHR(0)) & ~kill THEN
+				    	base.HandleKey(c);
+				    	tempPressed[i-2].keysym := base.msg.keysym; (* base.msg.keysym asigned by HandleKey() ... *)
+				    END;
+				END; (* FOR LOOP *)
+
+				(* update pressed array. generate keyboard.base.msg's for released keys *)
+				FOR i := 0 TO 5 DO
+					IF (pressed[i].updated = FALSE) & (pressed[i].ch # CHR(0)) THEN (* this key has been released *)
+						base.msg.flags := {};
+						INCL(base.msg.flags, Inputs.Release);
+						base.msg.ch := pressed[i].ch;
+						base.msg.keysym := pressed[i].keysym;
+						base.dkHack := base.deadKey;  (* value of deadKey should persist the key release event *)
+						base.HandleKey(c);
+						base.deadKey := base.dkHack;
+					END;
+					pressed[i].counter := tempPressed[i].counter;
+					pressed[i].ch := tempPressed[i].ch;
+					pressed[i].keysym := tempPressed[i].keysym;
+					pressed[i].repeat := tempPressed[i].repeat;
+					pressed[i].updated := FALSE;
+				END;
+
+				(* Generate events for modifiers *)
+				base.HandleModifiers(flags);
+
+				(* update status of the LEDs  of the keyboad if necessary *)
+				IF base.lastLeds # base.leds THEN (* LED status has changed *)
+					base.ledBuffer[0] := SYSTEM.VAL(CHAR, base.leds); base.lastLeds := base.leds;
+					res := SetReport(UsbHid.ReportOutput, 0, base.ledBuffer, 1); (* ignore res *)
+				END;
+				status := pipe.Transfer(pipe.maxPacketSize, 0, buffer);
+			ELSE
+				IF Debug THEN KernelLog.String("UsbKeyboard: Error. Disabling keyboard "); KernelLog.String(name); KernelLog.Ln; END;
+			END;
+		END EventHandler;
+
+		PROCEDURE Connect*(): BOOLEAN;
+		VAR status : Usbdi.Status; endpoint: LONGINT; i: ADDRESS; k : ARRAY 32 OF CHAR;
+		BEGIN
+			IF ~SetProtocol(0) THEN
+				IF Debug THEN KernelLog.String("UsbKeyboard: Error: Cannot set keyboard into boot protocol mode."); KernelLog.Ln; END;
+				RETURN FALSE
+			END;
+
+			IF ~SetIdle(0,10) THEN
+				IF Debug THEN KernelLog.String("UsbKeyboard: Error: Cannot set idle the keyboard."); KernelLog.Ln; END;
+				RETURN FALSE
+			END;
+
+			endpoint := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, interface.endpoints[0].bEndpointAddress) * {0,1,2,3,7});
+
+			pipe := device.GetPipe(endpoint);
+			IF pipe = NIL THEN
+				IF Debug THEN KernelLog.String("UsbKeyboard: Could not get pipe."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			(* Get  *)
+			Machine.GetConfig("Keyboard", k);
+			i := -1;
+			IF k # "" THEN i := base.TableFromFile(k); END;
+			IF i = -1 THEN (* Fallback to default *) i := TableUS(); END;
+			SYSTEM.PUT(ADDRESSOF(base.keytable), i);
+
+			(* Apply Numlock boot up state *)
+			Machine.GetConfig("NumLock", k);
+			IF k[0] = "1" THEN INCL(base.leds, NumLock) END;
+
+			base.keyDeadTime := KeyDeadTime DIV 10;
+			base.keyDeadTimeRepeat := KeyDeadTimeRepeat DIV 10;
+
+			NEW(base.ledBuffer, 1);
+
+			NEW(buffer, pipe.maxPacketSize);
+			pipe.SetTimeout(0);
+			pipe.SetCompletionHandler(EventHandler);
+			status := pipe.Transfer(pipe.maxPacketSize, 0, buffer); (* ignore status *)
+
+			RETURN TRUE;
+		END Connect;
+
+		PROCEDURE Disconnect*;
+		BEGIN
+			KernelLog.String("UsbKeyboard: USB Keyboard disconnected."); KernelLog.Ln;
+		END Disconnect;
+
+	END KeyboardDriver;
+
+VAR
+
+(* Translation table format:
+ *
+ *	table = { scancode unshifted-code shifted-code flags }  0FFX .
+ *	scancode = <scancode byte from keyboard, bit 7 set for "grey" extended keys>
+ *	unshifted-code = <CHAR produced by this scancode, without shift>
+ *	shifted-code = <CHAR produced by this scancode, with shift>
+ *	flags = <bit-mapped flag byte indicating special behaviour>
+ *
+ *	flag bit	function
+ *		0	01	DeadKey: Set dead key flag according to translated key code (1-7)
+ *		1	02	NumLock: if set, the state of NumLock will reverse the action of shift (for num keypad)
+ *		2	04	CapsLock: if set, the state of CapsLock will reverse the action of shift (for alpha keys)
+ *		3	08	LAlt:  \ the state of these two flags in the table and the current state of the two...
+ *		4	10	RAlt: / ...Alt keys must match exactly, otherwise the search is continued.
+ *		5	20	\
+ *		6	40	 >  dead key number (0-7), must match current dead key flag
+ *		7	80	/
+ *
+ *	The table is scanned sequentially (speed not critical).  Ctrl-Break, Ctrl-F10 and Ctrl-Alt-Del
+ *	are always defined and are not in the table.   The control keys are also always defined.
+ *)
+
+(* TableUS - US keyboard translation table (dead keys: ^=1, '=2, `=3, ~=4, "=5) *)
+PROCEDURE TableUS*(): ADDRESS;
+CODE
+	; Return address of first table element
+	ADD	R0, PC, #Base-8-$
+	MOV	SP, FP
+	LDR	FP, [SP], #4
+	LDR	LR, [SP], #4
+	BX		LR
+	; Table
+Base:
+		; alphabet
+	DB 1EH, 61H, 41H, 4H,		30H, 62H, 42H, 4H,		2EH, 63H, 43H, 4H,		20H, 64H, 44H, 4H
+	DB 12H, 65H, 45H, 4H,		21H, 66H, 46H, 4H,		22H, 67H, 47H, 4H,		23H, 68H, 48H, 4H
+	DB 17H, 69H, 49H, 4H,		24H, 6AH, 4AH, 4H,	25H, 6BH, 4BH, 4H,	26H, 6CH, 4CH, 4H
+	DB 32H, 6DH, 4DH, 4H,	31H, 6EH, 4EH, 4H,	18H, 6FH, 4FH, 4H,		19H, 70H, 50H, 4H
+	DB 10H, 71H, 51H, 4H,		13H, 72H, 52H, 4H,		1FH, 73H, 53H, 4H,		14H, 74H, 54H, 4H
+	DB 16H, 75H, 55H, 4H,		2FH, 76H, 56H, 4H,		11H, 77H, 57H, 4H,		2DH, 78H, 58H, 4H
+	DB 15H, 79H, 59H, 4H,		2CH, 7AH, 5AH, 4H
+
+		; dead keys (LAlt & RAlt)
+	DB 07H, 0FFH, 1H, 9H,		28H, 2H, 5H, 9H,	29H, 3H, 4H, 9H
+	DB 07H, 0FFH, 1H, 11H,	28H, 2H, 5H, 11H,	29H, 3H, 4H, 11H
+
+		; numbers at top
+	DB 0BH, 30H, 29H, 0H,		02H, 31H, 21H, 0H,		03H, 32H, 40H, 0H,		04H, 33H, 23H, 0H
+	DB 05H, 34H, 24H, 0H,		06H, 35H, 25H, 0H,		07H, 36H, 5EH, 0H,		08H, 37H, 26H, 0H
+	DB 09H, 38H, 2AH, 0H,		0AH, 39H, 28H, 0H
+		; symbol keys
+	DB 28H, 27H, 22H, 0H,		33H, 2CH, 3CH, 0H,	0CH, 2DH, 5FH, 0H,	34H, 2EH, 3EH, 0H
+	DB 35H, 2FH, 3FH, 0H,		27H, 3BH, 3AH, 0H,	0DH, 3DH, 2BH, 0H,	1AH, 5BH, 7BH, 0H
+	DB 2BH, 5CH, 7CH, 0H,	1BH, 5DH, 7DH, 0H,	29H, 60H, 7EH, 0H
+		; control keys
+	DB 0EH, 7FH, 7FH, 0H ; backspace
+	DB 0FH, 09H, 09H, 0H ; tab
+	DB 1CH, 0DH, 0DH, 0H ; enter
+	DB 39H, 20H, 20H, 0H ; space
+	DB 01H, 0FEH, 1BH, 0H ; esc
+		; keypad
+	DB 4FH, 0A9H, 31H, 2H ; end/1
+	DB 50H, 0C2H, 32H, 2H ; down/2
+	DB 51H, 0A3H, 33H, 2H ; pgdn/3
+	DB 4BH, 0C4H, 34H, 2H ; left/4
+	DB 4CH, 0FFH, 35H, 2H ; center/5
+	DB 4DH, 0C3H, 36H, 2H ; right/6
+	DB 47H, 0A8H, 37H, 2H ; home/7
+	DB 48H, 0C1H, 38H, 2H ; up/8
+	DB 49H, 0A2H, 39H, 2H ; pgup/9
+	DB 52H, 0A0H, 30H, 2H ; insert/0
+	DB 53H, 0A1H, 2EH, 2H ; del/.
+		; gray keys
+	DB 4AH, 2DH, 2DH, 0H ; gray -
+	DB 4EH, 2BH, 2BH, 0H ; gray +
+	DB 0B5H, 2FH, 2FH, 0H ; gray /
+	DB 37H, 2AH, 2AH, 0H ; gray *
+	DB 0D0H, 0C2H, 0C2H, 0H ; gray down
+	DB 0CBH, 0C4H, 0C4H, 0H ; gray left
+	DB 0CDH, 0C3H, 0C3H, 0H ; gray right
+	DB 0C8H, 0C1H, 0C1H, 0H ; gray up
+	DB 09CH, 0DH, 0DH, 0H ; gray enter
+	DB 0D2H, 0A0H, 0A0H, 0H ; gray ins
+	DB 0D3H, 0A1H, 0A1H, 0H ; gray del
+	DB 0C9H, 0A2H, 0A2H, 0H ; gray pgup
+	DB 0D1H, 0A3H, 0A3H, 0H ; gray pgdn
+	DB 0C7H, 0A8H, 0A8H, 0H ; gray home
+	DB 0CFH, 0A9H, 0A9H, 0H ; gray end
+		; function keys
+	DB 3BH, 0A4H, 0FFH, 0H ; F1
+	DB 3CH, 0A5H, 0FFH, 0H ; F2
+	DB 3DH, 1BH, 0FFH, 0H ; F3
+	DB 3EH, 0A7H, 0FFH, 0H ; F4
+	DB 3FH, 0F5H, 0FFH, 0H ; F5
+	DB 40H, 0F6H, 0FFH, 0H ; F6
+	DB 41H, 0F7H, 0FFH, 0H ; F7
+	DB 42H, 0F8H, 0FFH, 0H ; F8
+	DB 43H, 0F9H, 0FFH, 0H ; F9
+	DB 44H, 0FAH, 0FFH, 0H ; F10
+	DB 57H, 0FBH, 0FFH, 0H ; F11
+	DB 58H, 0FCH, 0FFH, 0H ; F12
+	DB 0FFH
+END TableUS;
+
+(* maps USB usage ID's to Oberon character code *)
+PROCEDURE UsbScanTab*() : ADDRESS;
+CODE
+	; Return address of first table element
+	ADD	R0, PC, #Base-8-$
+	MOV	SP, FP
+	LDR	FP, [SP], #4
+	LDR	LR, [SP], #4
+	BX		LR
+	; Table
+Base:
+	DB 000, 000, 000, 000, 030, 048, 046, 032, 018, 033, 034, 035, 023, 036, 037, 038
+	DB 050, 049, 024, 025, 016, 019, 031, 020, 022, 047, 017, 045, 021 ,044, 002, 003
+	DB 004, 005, 006, 007, 008, 009, 010, 011, 028, 001, 014, 015 ,057, 012, 013, 026
+	DB 027, 043, 043, 039, 040, 041, 051, 052, 053, 058, 059, 060, 061, 062, 063, 064
+	DB 065, 066, 067, 068, 087, 088, 099, 070, 119, 210, 199, 201, 211, 207, 209, 205
+	DB 203, 208, 200, 069, 181, 055, 074, 078, 156, 079, 080, 081, 075, 076, 077, 071
+	DB 072, 073, 082, 083, 086, 127, 116, 117, 085, 089, 090, 091, 092, 093, 094, 095
+	DB 120, 121, 122, 123, 134, 138, 130, 132, 128, 129, 131, 137, 133, 135, 136, 113
+	DB 115, 114, 000, 000, 000, 000, 000, 124, 000, 000, 000, 000, 000, 000, 000, 000
+	DB 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000
+	DB 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000
+	DB 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000
+	DB 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000
+	DB 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000
+	DB 029, 042, 056, 125, 097, 054, 100, 126, 164, 166, 165, 163, 161, 115, 114, 113
+	DB 150, 158, 159, 128, 136, 177, 178, 176, 142, 152, 173, 140, 000, 000, 000, 000
+END UsbScanTab;
+
+(* Maps USB key code to X11 keysym (/usr/include/X11/keysymdef.h). *)
+PROCEDURE KeySym*(VAR ch : CHAR; VAR leds : SET): LONGINT;
+VAR res: LONGINT;
+BEGIN
+	CASE ch OF
+		028X: res := Inputs.KsReturn (* Return *)
+	    	|029X: res := Inputs.KsEscape (* Escape *)
+	    	|02AX: res := Inputs.KsBackSpace (* Delete (Backspace) *)
+	   	|02BX: res := Inputs.KsTab (* Tab *)
+		|03AX: res := Inputs.KsF1 (* f1 *)
+		|03BX: res := Inputs.KsF2 (* f2 *)
+		|03CX: res := Inputs.KsF3 (* f3 *)
+		|03DX: res := Inputs.KsF4 (* f4 *)
+		|03EX: res := Inputs.KsF5 (* f5 *)
+		|03FX: res := Inputs.KsF6 (* f6 *)
+		|040X: res := Inputs.KsF7 (* f7 *)
+		|041X: res := Inputs.KsF8 (* f8 *)
+		|042X: res := Inputs.KsF9 (* f9 *)
+		|043X: res := Inputs.KsF10 (* f10 *)
+		|044X: res := Inputs.KsF11 (* f11 *)
+		|045X: res := Inputs.KsF12 (* f12 *)
+		|046X: res := Inputs.KsPrint (* Printscreen *)
+		|047X: res := Inputs.KsScrollLock (* ScrollLock *)
+		|048X: res := Inputs.KsPause (* Pause *)
+		|049X: res := Inputs.KsInsert (* insert *)
+		|04AX: res := Inputs.KsHome (* home *)
+		|04BX: res := Inputs.KsPageUp (* pgup *)
+		|04CX: res := Inputs.KsDelete (* delete *)
+		|04DX: res := Inputs.KsEnd (* end *)
+		|04EX: res := Inputs.KsPageDown (* pgdn *)
+		|04FX: res := Inputs.KsRight (* right *)
+		|050X: res := Inputs.KsLeft (* left *)
+		|051X: res := Inputs.KsDown (* down *)
+		|052X: res := Inputs.KsUp (* up *)
+		|053X: res := Inputs.KsNumLock; (* Keypad NumLock *)
+		|054X: res := Inputs.KsKPDivide (* Keypad / *)
+		|055X: res := Inputs.KsKPMultiply (* Keypad * *)
+		|056X: res := Inputs.KsKPSubtract (* Keypad - *)
+		|057X: res := Inputs.KsKPAdd (* Keypad + *)
+		|058X: res := Inputs.KsReturn (* Keypad Enter: Should be KsKPEnter *)
+		|059X: IF ~(NumLock IN leds) THEN res := Inputs.KsEnd; ELSE res := Inputs.KsNil END;  (* Keypad 1 and End *)
+		|05AX: IF ~(NumLock IN leds) THEN res := Inputs.KsDown; ELSE res := Inputs.KsNil END;  (* Keypad 2 and Down Arrow *)
+		|05BX: IF ~(NumLock IN leds) THEN res := Inputs.KsPageDown; ELSE res := Inputs.KsNil END;  (* Keypad 3 and PageDown *)
+		|05CX: IF ~(NumLock IN leds) THEN res := Inputs.KsLeft; ELSE res := Inputs.KsNil END;  (* Keypad 4 and Left Arrow *)
+		|05DX: IF ~(NumLock IN leds) THEN ch := 0X; res := Inputs.KsNil; ELSE res := Inputs.KsNil END; (* don't report key event !! *)
+		|05EX: IF ~(NumLock IN leds) THEN res := Inputs.KsRight; ELSE res := Inputs.KsNil END;  (* Keypad 6 and Right Arrow *)
+		|05FX: IF ~(NumLock IN leds) THEN res := Inputs.KsHome; ELSE res := Inputs.KsNil END;  (* Keypad 7 and Home *)
+		|060X: IF ~(NumLock IN leds) THEN res := Inputs.KsUp; ELSE res := Inputs.KsNil END;  (* Keypad 8 and Up Arrow *)
+		|061X: IF ~(NumLock IN leds) THEN res := Inputs.KsPageUp; ELSE res := Inputs.KsNil END;  (* Keypad 9 and Page Up *)
+		|062X: IF ~(NumLock IN leds) THEN res := Inputs.KsInsert; ELSE res := Inputs.KsNil END;  (* Keypad 0 and Insert *)
+		|063X: IF ~(NumLock IN leds) THEN res := Inputs.KsDelete; ELSE res := Inputs.KsNil END;  (* Keypad . and Delete *)
+		|067X:  IF ~(NumLock IN leds) THEN ch := 028X; res := Inputs.KsKPEnter; ELSE res := Inputs.KsNil END;  (* Keypad =; remap to KpEnter *)
+		|0B0X: ch := 0X; res := Inputs.KsNil;  (* Keypad 00; don't map *)
+		|0B1X: ch := 0X; res := Inputs.KsNil;  (* Keypad 000; don't map *)
+		|09AX: res := Inputs.KsSysReq (* SysReq / Attention *)
+		|0E0X: res := Inputs.KsControlL (* Left Control *)
+		|0E1X: res := Inputs.KsShiftL (* Left Shift *)
+		|0E2X: res := Inputs.KsAltL (* Left Alt *)
+		|0E3X: res := Inputs.KsMetaL (* Left GUI *)
+		|0E4X: res := Inputs.KsControlR (* Right Control *)
+		|0E5X: res := Inputs.KsShiftR (* Right Shift *)
+		|0E6X: res := Inputs.KsAltR (* Right Alt *)
+		|0E7X: res := Inputs.KsMetaR (* Right GUI *)
+		|076X: res := Inputs.KsMenu (* Windows Menu *)
+		|0FFX: res := Inputs.KsBreak (* Break *)
+	ELSE
+		(* if res=Inputs.KsNil, the KeySym will be assigned later (see HandleKey) *)
+		res := Inputs.KsNil (* no key *)
+	END;
+	RETURN res
+END KeySym;
+
+PROCEDURE GetModifierKeysym(modifier : LONGINT) : LONGINT;
+VAR res : LONGINT;
+BEGIN
+	CASE modifier OF
+		|Inputs.LeftCtrl: res := Inputs.KsControlL;
+		|Inputs.LeftShift: res := Inputs.KsShiftL;
+		|Inputs.LeftAlt: res := Inputs.KsAltL;
+		|Inputs.LeftMeta: res := Inputs.KsMetaL;
+		|Inputs.RightCtrl: res := Inputs.KsControlR;
+		|Inputs.RightShift: res := Inputs.KsShiftR;
+		|Inputs.RightAlt: res := Inputs.KsAltR;
+		|Inputs.RightMeta: res := Inputs.KsMetaR;
+	ELSE
+		res := Inputs.KsNil;
+	END;
+	RETURN res;
+END GetModifierKeysym;
+
+(* Translate - Translate scan code "c" to key. *)
+PROCEDURE Translate(flags, leds: SET;  c: CHAR; keyboardTable : ADDRESS; VAR keyboardDeadKey, keyboardKeyVal : LONGINT): LONGINT;
+CONST
+	(* The flags stored in the keytable are not the same as the ones defined in Inputs.
+		The parameter flags and leds use the Inputs constants.
+		The constants below are for the use of the flags stored in the keytable (variable s) *)
+	OScrollLock = 0;
+	ONumLock = 1;
+	OCapsLock = 2;
+	LAlt = 3;
+	RAlt = 4;
+	LCtrl = 5;
+	RCtrl = 6;
+	LShift = 7;
+	RShift = 8;
+	GreyEsc = 9;
+	LMeta = 13;
+	RMeta = 14;
+	Alt = {LAlt, RAlt};
+	Ctrl = {LCtrl, RCtrl};
+	Shift = {LShift, RShift};
+	DeadKey = 0;
+VAR
+	a: ADDRESS;
+	s1: CHAR;
+	s : SET;
+	k: INTEGER;
+	dkn: SHORTINT;
+BEGIN
+	IF (c = 46X) & (flags * Inputs.Ctrl # {}) THEN RETURN -2 END;  (* Ctrl-Break - break *)
+	IF (c = 44X) & (flags * Inputs.Ctrl # {}) THEN RETURN 0FFH END;	(* Ctrl-F10 - exit *)
+	IF (c = 53X) & (flags * Inputs.Ctrl # {}) & (flags * Inputs.Alt # {}) THEN RETURN 0FFH END; (* Ctrl-Alt-Del - exit *)
+
+	a := keyboardTable;
+
+	(* this loop linearly searches the keytable for an entry for the character c *)
+	LOOP
+		SYSTEM.GET(a, s1);
+
+		IF s1 = 0FFX THEN (* end of table -> unmapped key *)
+
+			(* reset key and dead key state *)
+			k := -1;  keyboardDeadKey := 0;  EXIT;
+
+		ELSIF s1 = c THEN (* found scan code in table *)
+
+			k := 0;
+
+			SYSTEM.GET(a+3, SYSTEM.VAL(CHAR, s)); (* flags from table *)
+			dkn := SHORT(SHORT(SYSTEM.VAL(LONGINT, LSH(s * {5..7}, -5))));
+
+			s := s * {DeadKey, ONumLock, OCapsLock, LAlt, RAlt, LCtrl, RCtrl};
+
+			IF ((s * Alt = LSH(flags * Inputs.Alt,-2)) OR (ONumLock IN s) OR (s1>03BX))  & (dkn = keyboardDeadKey) THEN	(* Alt & dead keys match exactly *)
+
+				(* check if shift pressed *)
+				IF flags * Inputs.Shift # {} THEN INCL(s, LShift) END;
+
+				(* handle CapsLock *)
+				IF (OCapsLock IN s) & (CapsLock IN leds) THEN s := s / {LShift} END;
+
+				(* handle NumLock *)
+				IF ONumLock IN s THEN
+					IF flags * Inputs.Alt # {} THEN INCL(s, LShift)
+					ELSIF NumLock IN leds THEN s := s / {LShift}
+					END
+				END;
+
+				(* get key code *)
+				IF LShift IN s THEN SYSTEM.GET(a+2, SYSTEM.VAL(CHAR, k))	(* shifted value *)
+				ELSE SYSTEM.GET(a+1, SYSTEM.VAL(CHAR, k))	(* unshifted value *)
+				END;
+
+				IF (DeadKey IN s) & (k <= 7) THEN (* dead key *)
+					keyboardDeadKey := SHORT(k);  k := -1	(* set new dead key state *)
+				ELSIF k = 0FFH THEN	(* unmapped key *)
+					k := -1;  keyboardDeadKey := 0	(* reset dead key state *)
+				ELSE	(* mapped key *)
+					IF flags * Inputs.Ctrl # {} THEN
+						IF ((k >= 64) & (k <= 95)) OR ((k >= 97) & (k <= 122)) THEN
+							k := SHORT(SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, ADDRESS(k)) * {0..4}))	(* control *)
+						ELSIF k = 13 THEN	(* Ctrl-Enter *)
+							k := 10
+						END
+					END;
+					IF flags * Inputs.Alt # {} THEN	(* Alt-keypad *)
+						IF (k >= ORD("0")) & (k <= ORD("9")) & (NumLock IN s) THEN	(* keypad num *)
+							IF keyboardKeyVal = -1 THEN keyboardKeyVal := k-ORD("0")
+							ELSE keyboardKeyVal := (10*keyboardKeyVal + (k-ORD("0"))) MOD 1000;
+							END;
+							k := -1
+						END
+					END;
+					keyboardDeadKey := 0	(* reset dead key state *)
+				END;
+				EXIT
+			END
+		END;
+		INC(a, 4)
+	END; (* LOOP *)
+	RETURN k
+END Translate;
+
+(* Displays textual representation of the set flags to KernelLog *)
+PROCEDURE ShowFlags(flags, leds : SET);
+BEGIN
+	KernelLog.String("Flags: ");
+	IF Inputs.LeftAlt IN flags THEN KernelLog.String("[Left Alt]"); END;
+	IF Inputs.RightAlt IN flags THEN KernelLog.String("[Right Alt]"); END;
+	IF Inputs.LeftCtrl IN flags THEN KernelLog.String("[Left Ctrl]"); END;
+	IF Inputs.RightCtrl IN flags THEN KernelLog.String("[Rigth Ctrl]"); END;
+	IF Inputs.LeftShift IN flags THEN KernelLog.String("[Left Shift]"); END;
+	IF Inputs.RightShift IN flags THEN KernelLog.String("[Right Shift]"); END;
+	IF Inputs.LeftMeta IN flags THEN KernelLog.String("[Left Meta]"); END;
+	IF Inputs.RightMeta IN flags THEN KernelLog.String("[Rigth Meta]"); END;
+	IF Inputs.Release IN flags THEN KernelLog.String("[Released]"); END;
+	IF ScrollLock IN leds THEN KernelLog.String("[ScrollLock]"); END;
+	IF NumLock IN leds THEN KernelLog.String("[NumLock]"); END;
+	IF CapsLock IN leds THEN KernelLog.String("[CapsLock]"); END;
+	IF Compose IN leds THEN KernelLog.String("[Compose]"); END;
+	IF Kana IN leds THEN KernelLog.String("[Kana]"); END;
+END ShowFlags;
+
+PROCEDURE Probe(dev : Usbdi.UsbDevice; if : Usbdi.InterfaceDescriptor) : Usbdi.Driver;
+VAR driver : KeyboardDriver;
+BEGIN
+	IF if.bInterfaceClass # 3 THEN RETURN NIL END; (* HID class *)
+	IF if.bInterfaceSubClass # 1 THEN RETURN NIL END; (* Boot protocol subclass *)
+	IF if.bInterfaceProtocol # 1 THEN RETURN NIL END; (* Keyboard *)
+	IF if.bNumEndpoints # 1 THEN RETURN NIL END;
+	KernelLog.String("UsbKeyboard: USB Keyboard found."); KernelLog.Ln;
+	NEW(driver);
+	RETURN driver;
+END Probe;
+
+PROCEDURE SetLayout*(context : Commands.Context); (** dev file ~ *)
+VAR
+	string : ARRAY 64 OF CHAR;
+	plugin : Plugins.Plugin; kd : KeyboardDriver;
+BEGIN
+	IF context.arg.GetString(string) THEN
+		plugin := Usb.usbDrivers.Get(string);
+		IF plugin # NIL THEN
+			IF plugin IS KeyboardDriver THEN
+				kd := plugin (KeyboardDriver);
+			ELSE context.error.String("UsbKeyboard: Device "); context.error.String(string); context.error.String(" is not a keyboard."); context.error.Ln;
+			END;
+		ELSE context.error.String("UsbKeyboard: Device "); context.error.String(string); context.error.String(" not found."); context.error.Ln;
+		END;
+	ELSE context.error.String("UsbKeyboard: Expected <dev> parameter."); context.error.Ln;
+	END;
+	IF kd # NIL THEN
+		IF context.arg.GetString(string) THEN
+			kd.base.SetLayout(string);
+			context.out.String("Layout set to "); context.out.String(string); context.out.Ln;
+		END;
+	END;
+END SetLayout;
+
+PROCEDURE Install*;
+END Install;
+
+PROCEDURE Cleanup;
+BEGIN
+	Usbdi.drivers.Remove(Name);
+END Cleanup;
+
+BEGIN
+	Modules.InstallTermHandler(Cleanup);
+	Usbdi.drivers.Add(Probe, Name, Description, Priority);
+END UsbKeyboard.
+
+UsbKeyboard.Install ~ SystemTools.Free UsbKeyboard ~
+
+UsbKeyboard.SetLayout UsbKeyboard00 KeyBE.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyCA.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyCH.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyD.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyDV.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyFR.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyIT.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyN.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyPL.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeySF.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyTR.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyUK.Bin ~
+UsbKeyboard.SetLayout UsbKeyboard00 KeyUS.Bin ~
+
+WMKeyCode.Open ~	SystemTools.Free WMKeyCode ~

source/ARM.UsbStorageBase.Mod

@@ -0,0 +1,1202 @@
+MODULE UsbStorageBase; (** AUTHOR "staubesv"; PURPOSE " USB Mass Storage Driver Base Class"; *)
+(**
+ *	Bluebottle USB Mass Storage Driver Base Class
+ *
+ *	This module is the base for all USB mass storage device drivers. There are three different transport layers:
+ *
+ *		UsbStorageCbi.Mod		CB/I transport layer
+ *		UsbStorageBot.Mod		Bulk-only transport layer
+ *		UsbStorageScm.Mod		SCM transport layer
+ *
+ * This driver is based on the work of Christian Plattner.
+ *
+ * Usage:
+ *
+ *	UsbStorageBase.Show ~ displays a list of all USB storage devices
+ *
+ * References:
+ *
+ *	[1] Universal Serial Bus Mass Storage Class Specification Overview, Revision 1.2, June 23, 2003
+ *	[2] Universal Serial Bus Mass Storage Class UFI Command Specification, Revision 1.0, December 1998
+ *	[3] Universal Serial Bus Mass Storage Specification for Bootability, Revision 1.0, October 25, 2004
+ *
+ *	All references available at www.usb.org
+ *
+ * History:
+ *
+ *	30.09.2000 	cp first release
+ *	20.10.2000 	cp many improvements (BulkOnly, UTS)
+ *	20.11.2005 	Added support for logical devices, use Disks.Mod-compatible result codes on transport layer (staubesv)
+ *	24.11.2005 	Added UFI commands TestUnitReady, Allow/Prevent medium removal (staubesv)
+ *	29.11.2005 	Added UFI commands SendDiagnostic and Start/Stop Unit (used and therefore named to EjectMedia,
+ *				moved ReadCapacity to USB layer (staubesv)
+ *	13.12.2005	Adapted to USBDI changes (staubesv)
+ *	14.12.2005	UsbStorageDevice.Transfer, GetSize & Handle made exclusive (staubesv)
+ *	15.12.2005	Fixed Bulk-Only reset recovery (staubesv)
+ *	19.12.2005	Trim vendor/product strings (staubesv)
+ *	03.01.2006	Use sequence number for CSW tag (staubesv)
+ *	05.01.2006	Added TraceCBWs & TraceCSWs trace options (staubesv)
+ *	12.01.2006	Removed data copying/memory allocation (staubesv)
+ *	09.02.2006	Refactored driver: Introduced UsbStorageBase, UsbStorageCbi, UsbStorageBot,
+ *				UsbStorageScm, UsbStorageBoot (staubesv)
+ *	28.06.2006	Use UsbUtilties (staubesv)
+ *	09.08.2006	Inquiry, TestUnitReady, ReadCapacity, Read & Write commands length set to 12 bytes since lower values broke the CBI Layer (staubesv)
+ *	26.03.2007	Added NnofReads, NnofWrites, NnofOthers and NnofErrors statistics (staubesv)
+ *)
+
+IMPORT SYSTEM, KernelLog, Kernel, Commands, Disks, Plugins, Usbdi, Debug := UsbDebug, Lib := UsbUtilities;
+
+CONST
+
+	NoData* = {};
+	DataIn* = {1};
+	DataOut* = {2};
+
+	(* Result codes *)
+	(* From Disks.Mod *)
+	ResOk* = Disks.Ok;
+	ResWriteProtected* = Disks.WriteProtected;
+	ResDeviceInUse* = Disks.DeviceInUse;
+	ResMediaChanged* = Disks.MediaChanged;
+	ResMediaMissing* = Disks.MediaMissing;
+	ResUnsupported* = Disks.Unsupported;
+	(* USB Storage Device Driver specific *)
+	ResTimeout* = 30;
+	ResShortTransfer* = 31;
+	ResDeviceNotReady* = 32;
+	ResDeviceNotReadyInit* = 33; (* Device need to be initialized *)
+	ResSenseError* = 34; (* Do sensing *)
+	ResError* = 35;
+	ResFatalError* = 36; (* Device needs to be resetted *)
+	ResDisconnected* = 37;
+
+	MethodCBI* = 1;
+	MethodCB* = 2;
+	MethodBulkOnly* = 3;
+	MethodSCMShuttle* = 4;
+
+	ProtocolUFI* = 100;  	(* UFI *)
+	ProtocolUTS* = 101; 	(* USB TRANSPARENT SCSI => UTS (plattner definition) *)
+	ProtocolRBC* = 102; 	(* RBC = reduced block commands, often used for flash devices *)
+	Protocol8020* = 103; 	(* ATAPI for CDROM *)
+	Protocol8070* = 104; 	(* ATAPI for floppy drives and similar devices *)
+	ProtocolQIC157* = 105;	(* QIC, meaning the tape company. Typically used by tape devices *)
+
+	(* UFI Commands according UFI Command Specification *)
+	UfiFormatUnit = 04H; 			(* Format unformatted media; not implemented *)
+	UfiInquiry = 12H;				(* Get device information *)
+	UfiStartStop = 1BH;				(* Request a removable-media device to load or unload its media; partially  implemented *)
+	UfiModeSelect = 55H;			(* Allow the host to set parameters in a peripheral (mode sense should be issued prior to mode select); not implemented *)
+	UfiModeSense = 5AH;			(* Report parameters to the host. *)
+	UfiAllowRemoval = 1EH;			(* Prevent or allow the removal of media from a removable media device *)
+	UfiRead10* = 28H;				(* Transfer binary data from the media to the host *)
+	UfiRead12 = 0A8H;				(* Transfer binary data from the media to the host not implemented *)
+	UfiReadCapacity = 25H;			(* Report current media capacity *)
+	UfiReadFormatCapacity = 23H; 	(* Read current media capacity and formattable capacities supported by media; not implemented *)
+	UfiRequestSense = 03H;			(* Tansfer status sense data to the host *)
+	UfiRezeroUnit = 01H;			(* Position a head of the drive to zero track not implemented *)
+	UfiSeek10 = 2BH;				(* Seek the device to a specified address not implemented *)
+	UfiSendDiag = 1DH;				(* Perform a hard reset and execute diagnostics  *)
+	UfiTestUnitReady = 00H;			(* Request the device to report if it's ready *)
+	UfiVerify = 2FH;					(* Verify data on the media not implemented *)
+	UfiWrite10 = 2AH;				(* Transfer binary data from the host to the media *)
+	UfiWrite12 = 0AAH;				(* Transfer binary data from the host to the media not implemented *)
+	UfiWriteNVerify = 2EH;			(* Transfer binary data from the host to the media and verify data not implemented *)
+
+	(* Device types as reported by the UFI Inquiry command *)
+	DtSbcDirectAccess = 00H;
+	DtCDROM = 05H;
+	DtOpticalMemory = 07H;
+	DtRbcDirectAccess = 0EH;
+
+	RemovableBit = {7};
+
+	(* UfiModeSense constants: Page Control Field *)
+	PcCurrentValues = 0;
+	PcChangeableValues = 1;
+	PcDefaultValues = 2;
+	PcSavedValues = 3;
+
+	(* UfiModeSense constants: Page Code field *)
+	PageRwErrorRecovery = 01H;
+	PageFlexibleDisk = 05H;
+	PageBlockAccessCapacities = 1BH;
+	PageTimerAndProtect = 1CH;
+	PageAll = 3FH; (* only for mode sense command *)
+
+	(* Timeout values in milliseconds *)
+	TransferTimeout = 20000;
+	CommandTimeout = 5000;
+
+TYPE
+
+	(* Information delivered by UFI Inquiry command *)
+	InquiryResult = POINTER TO RECORD
+		deviceType : LONGINT; 			(* Peripheral device type; 00h: direct access device (floppy), 1FH > none *)
+		removable : BOOLEAN; 			(* Removable media bit *)
+		ansiVersion : LONGINT; 			(* Should be 0 for compatible devices *)
+		additionalLength : LONGINT;
+		validStrings : BOOLEAN; 		(* Are the fields below valid? *)
+		vendor : ARRAY 9 OF CHAR;
+		product : ARRAY 17 OF CHAR;
+		revision : ARRAY 5 OF CHAR;
+	END;
+
+	(* Information delivered by UFI Mode Sense Command when Flexible Disk Page is requested *)
+	FlexibleDiskPage = POINTER TO RECORD
+		TransferRate : LONGINT; (* kbits/s *)
+		NumberOfHeads : LONGINT;
+		SectorsPerTrack : LONGINT; (* 1 - 63 *)
+		BytesPerSector : LONGINT;
+		NumberOfCylinders : LONGINT;
+		MotorOnDelay, MotorOffDelay : LONGINT;
+		MediumRotationRate : LONGINT; (* r.p.m. *)
+	END;
+
+TYPE
+
+	UsbStorageDevice = OBJECT (Disks.Device)
+	VAR
+		usbDriver : StorageDriver;
+		lun : LONGINT; (* Logical Unit Number of this storage device*)
+
+		transportProtocol : LONGINT;
+
+		(* Fields used by the DiskManager *)
+		number : LONGINT; (* Suffix appended to name to get unique device name *)
+		next : UsbStorageDevice;
+
+		PROCEDURE Transfer* (op, block, num: LONGINT; VAR data: ARRAY OF CHAR; ofs: LONGINT; VAR diskres: LONGINT);
+		VAR direction : SET; cmd : ARRAY 12 OF CHAR; i, tlen, trans, offset, num0 : LONGINT;
+		BEGIN {EXCLUSIVE}
+			IF (op = Disks.Read) OR (op = Disks.Write) THEN
+				FOR i := 0 TO 11 DO cmd[i] := 0X; END;
+				IF op = Disks.Read THEN
+					cmd[0] := CHR(UfiRead10); direction := DataIn;
+				ELSE
+					cmd[0] := CHR(UfiWrite10); direction := DataOut;
+				END;
+				cmd[1] := CHR(LSH(lun, 5));  (* Logical device number *)
+
+				offset := ofs; num0 := num;
+				WHILE num > 0 DO
+					IF num > 65000 THEN trans := 65000; ELSE trans := num END;
+					cmd[2] := CHR(LSH(block, -24));
+					cmd[3] := CHR(LSH(block, -16));
+					cmd[4] := CHR(LSH(block, -8));
+					cmd[5] := CHR(block);
+					cmd[7] := CHR(LSH(trans, -8));
+					cmd[8] := CHR(trans);
+					cmd[9] := 0X;
+					diskres := usbDriver.InvokeTransport(cmd, 12, direction, data, offset, trans * blockSize, tlen, TransferTimeout);
+					IF diskres # Disks.Ok THEN RETURN; END;
+					block := block + trans;
+					num := num - trans;
+					offset := offset + (trans * blockSize);
+				END;
+
+				IF Disks.Stats THEN
+					IF (op = Disks.Read) THEN
+						INC (NnofReads);
+						IF (diskres = ResOk) THEN INC (NbytesRead, num0 * blockSize);
+						ELSE INC (NnofErrors);
+						END;
+					ELSE
+						INC (NnofWrites);
+						IF (diskres = ResOk) THEN INC (NbytesWritten, num0 * blockSize);
+						ELSE INC (NnofErrors);
+						END;
+					END;
+				END;
+			ELSE
+				diskres := Disks.Unsupported;
+				IF Disks.Stats THEN INC (NnofOthers); END;
+			END;
+		END Transfer;
+
+		(** Get number of blocks and size of blocks using the UFI Read Capacity command *)
+		PROCEDURE GetSize* (VAR size, res: LONGINT);
+		BEGIN {EXCLUSIVE}
+			IF Debug.Trace & Debug.traceInfo THEN KernelLog.String("UsbStorage: GetSize: "); KernelLog.Ln; END;
+			(* Some devices I've seen didn't like to be asked for their capacity when no medium was inserted... *)
+			res := usbDriver.WaitForReady(lun, 5000);
+			IF (res # ResOk) & (res # ResMediaChanged) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: GetCapacity: command failed: "); ShowRes(res); KernelLog.Ln; END;
+				RETURN;
+			END;
+
+			blockSize := 0; size := 0;
+			res := usbDriver.ReadCapacity(lun, size, blockSize);
+
+			IF ((res # ResOk) & (res # ResShortTransfer))  THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ReadCapacity: command failed: "); ShowRes(res); KernelLog.Ln; END;
+				RETURN;
+			END;
+			res := ResOk; (* Allow short transfer *)
+		END GetSize;
+
+		PROCEDURE Handle*(VAR msg: Disks.Message; VAR diskres: LONGINT);
+		VAR cmd : ARRAY 12 OF CHAR; i : LONGINT; fdp : FlexibleDiskPage;
+		BEGIN {EXCLUSIVE}
+			FOR i := 0 TO 11 DO cmd[i] := CHR(0); END;
+			IF msg IS Disks.GetGeometryMsg THEN
+			 	 IF transportProtocol = ProtocolUFI THEN
+					WITH msg : Disks.GetGeometryMsg DO
+						(* TODO: But what if the user changes the medium... *)
+						fdp := usbDriver.ModeSense(lun, 0, PageFlexibleDisk, 32);
+						IF fdp # NIL THEN
+							msg.spt := fdp.SectorsPerTrack;
+							msg.hds := fdp.NumberOfHeads;
+							msg.cyls := fdp.NumberOfCylinders;
+						ELSE
+							(* assume 1440KB floppy disk *)
+							msg.spt := 18; msg.hds := 2; msg.cyls := 80;
+						END;
+						IF Debug.Trace & Debug.traceScRequests THEN
+							KernelLog.String("UsbStorageBase: Disk geometry CHS:");
+							KernelLog.Int(msg.cyls, 0); KernelLog.String("x"); KernelLog.Int(msg.hds, 0); KernelLog.String("x"); KernelLog.Int(msg.spt, 0); KernelLog.Ln;
+						END;
+					END;
+					diskres := Disks.Ok;
+				ELSE
+					diskres := Disks.Unsupported;
+				END;
+			ELSIF msg IS Disks.LockMsg THEN
+				diskres := usbDriver.PreventMediumRemoval(lun, TRUE);
+			ELSIF msg IS Disks.UnlockMsg THEN
+				diskres := usbDriver.PreventMediumRemoval(lun, FALSE);
+			ELSIF msg IS Disks.EjectMsg THEN
+				diskres := usbDriver.EjectMedia(lun);
+			ELSE
+				diskres := Disks.Unsupported;
+			END;
+		END Handle;
+
+	END UsbStorageDevice;
+
+TYPE
+
+	StorageDriver* = OBJECT (Usbdi.Driver);
+	VAR
+		sdevs- : UsbStorageDevice; (* list of storage devices associated to this driver *)
+		description* : Plugins.Description;
+
+		transportProtocol*, transportMethod* : LONGINT;
+
+		bulkIn*, bulkOut*, interrupt* : LONGINT; (* adresses of the used endpoints *)
+		bulkInPipe-, bulkOutPipe-, interruptPipe-, defaultPipe- : Usbdi.Pipe;
+
+		initialize* : BOOLEAN; (* if TRUE, the Initialization() procedure is called *)
+
+		timer : Kernel.Timer;
+
+		(** Transport layer specific reset procedure *)
+		PROCEDURE Reset*(timeout : LONGINT) : LONGINT;
+		BEGIN
+			HALT(301); RETURN 0; (* abstract *)
+		END Reset;
+
+		(** Transport layer specific transfer procedure *)
+		PROCEDURE Transport*(cmd : ARRAY OF CHAR; cmdlen : LONGINT; dir :  SET;
+			VAR buffer : ARRAY OF CHAR; ofs, bufferlen : LONGINT; VAR tlen : LONGINT; timeout : LONGINT) : LONGINT;
+		BEGIN
+			HALT(301); RETURN 0;  (* abstract *)
+		END Transport;
+
+		(* Bulk-only transport layer only *)
+		PROCEDURE GetMaxLun*(VAR lun : LONGINT) : LONGINT;
+		BEGIN
+			HALT(301); RETURN 0; (* abstract *)
+		END GetMaxLun;
+
+		(* UFI command: Test whether the specified logical unit is ready *)
+		PROCEDURE TestUnitReady(lun : LONGINT) : LONGINT;
+		VAR cmd : ARRAY 12 OF CHAR; i, ignore : LONGINT;
+		BEGIN
+			cmd[0] := CHR(UfiTestUnitReady);
+			cmd[1] := CHR(LSH(lun, 5));
+			FOR i := 2 TO 11 DO cmd[i] := 0X; END;
+			RETURN InvokeTransport(cmd, 12, NoData, cmd, 0, 0, ignore, 30000);
+		END TestUnitReady;
+
+		(* Issues TestUnitReady commands until device is ready or an error occurs *)
+		PROCEDURE WaitForReady(lun, timeout : LONGINT) : LONGINT;
+		VAR retry, res : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorageBase: WaitForReady..."); KernelLog.Ln; END;
+			retry := 0;
+			LOOP
+				res := TestUnitReady(lun);
+				IF (res = ResDisconnected) THEN
+					EXIT;
+				ELSIF (res = ResDeviceNotReady) OR (res = ResMediaChanged) THEN
+					(* continue *)
+				ELSE
+					INC(retry);
+				END;
+				IF (retry > 3) OR (timeout < 0) THEN EXIT; END;
+				Wait(100);
+				timeout := timeout - 100;
+			END;
+			IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorageBase: WaitForReady done, res: "); ShowRes(res); KernelLog.Ln; END;
+			RETURN res;
+		END WaitForReady;
+
+		(* UFI command: Start/Stop Unit. 														*)
+		(* Note: Since UFI devices control the motor on/off themselves, it's not implemented here.	*)
+		(* We just use the command to eject the media, thus the name							*)
+		PROCEDURE EjectMedia(lun : LONGINT) : LONGINT;
+		VAR cmd, data : ARRAY 12 OF CHAR; i, tlen, res : LONGINT;
+		BEGIN
+			cmd[0] := CHR(UfiStartStop);
+			cmd[1] := CHR(LSH(lun, 5));
+			FOR i := 2 TO 11 DO cmd[i] := 0X; END;
+			cmd[4] := CHR(2); (* Eject *)
+			res := InvokeTransport(cmd, 12, DataOut, data, 0, 0, tlen, CommandTimeout);
+			RETURN res;
+		END EjectMedia;
+
+		(* UFI command: Prevent/Allow Medium Removal *)
+		PROCEDURE PreventMediumRemoval(lun : LONGINT; prevent : BOOLEAN) : LONGINT;
+		VAR cmd, data : ARRAY 12 OF CHAR; i, tlen, res : LONGINT;
+		BEGIN
+			cmd[0] := CHR(UfiAllowRemoval);
+			cmd[1] := CHR(LSH(lun, 5));
+			FOR i := 2 TO 11 DO cmd[i] := 0X; END;
+			IF prevent THEN cmd[4] := CHR(1); END;
+			res := InvokeTransport(cmd, 12, DataOut, data, 0, 0, tlen, CommandTimeout);
+			RETURN res;
+		END PreventMediumRemoval;
+
+		(* UFI command: Request UFI device to do a reset or perform a self-test *)
+		PROCEDURE SendDiagnostic(lun : LONGINT) : LONGINT;
+		VAR cmd, data : ARRAY 12 OF CHAR; i, tlen, res : LONGINT;
+		BEGIN
+			cmd[0] := CHR(UfiSendDiag);
+			cmd[1] := CHR(SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, LSH(lun, 5)) + {2}));
+			FOR i := 2 TO 11 DO cmd[i] := 0X; END;
+			res := InvokeTransport(cmd, 12, DataOut, data, 0, 0, tlen, CommandTimeout);
+			RETURN res;
+		END SendDiagnostic;
+
+		(* UFI command: Get the number of blocks and the blocksize of the specified logical unit *)
+		PROCEDURE ReadCapacity(lun : LONGINT; VAR blocks, blocksize : LONGINT) : LONGINT;
+		VAR cmd, data : ARRAY 12 OF CHAR; res, i, tlen : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceInfo THEN KernelLog.String("UsbStorage: ReadCapacity: "); KernelLog.Ln; END;
+			cmd[0] := CHR(UfiReadCapacity);
+			cmd[1] := CHR(LSH(lun, 5));
+			FOR i := 2 TO 11 DO cmd[i] := CHR(0); END;
+
+			blocks := 0; blocksize := 0;
+
+			res := InvokeTransport(cmd, 12, DataIn, data, 0, 8, tlen, TransferTimeout);
+			IF (res # ResOk) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ReadCapacity: command failed: "); ShowRes(res); KernelLog.Ln; END;
+				RETURN res;
+			END;
+
+			FOR i := 0 TO 3 DO
+				blocks := blocks*100H + ORD(data[i]);
+				blocksize := blocksize*100H + ORD(data[4+i])
+			END;
+			INC(blocks); (* The device reports the highest valid block address -> also count block zero *)
+			res := ResOk; (* allow short transfers *)
+			IF Debug.Trace & Debug.traceInfo THEN
+				KernelLog.String("UsbStorage: Disk info: Blocks: "); KernelLog.Int(blocks, 0);
+				KernelLog.String(" blocksize: "); KernelLog.Int(blocksize, 0); KernelLog.String(" size: "); KernelLog.Int(blocks*blocksize, 0);
+				KernelLog.Ln;
+			END;
+			RETURN ResOk;
+		END ReadCapacity;
+
+		(* UFI command: Inquiry the specified locigal unit; Returns NIL in error case *)
+		PROCEDURE Inquiry(lun : LONGINT) : InquiryResult;
+		VAR cmd : ARRAY 12 OF CHAR; data : ARRAY 36 OF CHAR; result : InquiryResult; i, j, tlen, res : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceInfo THEN
+				KernelLog.String("UsbStorage: Inquiry logical device "); KernelLog.Int(lun, 0); KernelLog.String("... "); KernelLog.Ln;
+			END;
+			FOR i := 0 TO 11 DO cmd[i] := CHR(0); END;
+			cmd[0] := CHR(UfiInquiry);
+			cmd[1] := CHR(LSH(lun, 5));
+			cmd[4] := CHR(36); (* maximum allocation length *)
+
+			res := InvokeTransport(cmd, 12, DataIn, data, 0, 36, tlen, 50000);
+			IF ((res # Disks.Ok) & (res # ResShortTransfer)) OR (tlen < 5) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Fatal, inquiry device error: "); ShowRes(res); KernelLog.Ln; END;
+				RETURN NIL;
+			END;
+
+			NEW(result);
+			result.deviceType := ORD(data[0]) MOD 32;
+			IF SYSTEM.VAL(SET, ORD(data[1])) * RemovableBit # {} THEN
+				result.removable := TRUE;
+			END;
+			result.ansiVersion := ORD(data[2]) MOD 8;
+			result.additionalLength := ORD(data[4]);
+
+			IF transportProtocol = ProtocolRBC THEN
+				IF result.deviceType # DtRbcDirectAccess THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: RBC device type is not 0EH"); KernelLog.Ln; END;
+					RETURN NIL;
+				END;
+			ELSIF (result.deviceType # DtSbcDirectAccess) & (result.deviceType # DtCDROM) & (result.deviceType # DtOpticalMemory) THEN
+				IF Debug.Trace & Debug.traceInfo THEN KernelLog.String("UsbStorage: Device is not a storage device"); KernelLog.Ln; END;
+				RETURN NIL;
+			END;
+
+			IF tlen >= 36 THEN
+				result.validStrings := TRUE;
+				j := 0; FOR i := 8 TO 15 DO result.vendor[j] := data[i]; INC(j); END; result.vendor[8] := 0X;
+				j := 0; FOR i := 16 TO 31 DO result.product[j] := data[i]; INC(j); END; result.product[16] := 0X;
+				j := 0; FOR i := 32 TO 35 DO result.revision[j] := data[i]; INC(j); END; result.revision[4] := 0X;
+			END;
+			IF Debug.Trace & Debug.traceInfo THEN ShowInquiryResult(result); END;
+			RETURN result;
+		END Inquiry;
+
+		(* The mode sense command allows the UFI device to report medium or device parameters to the host *)
+		PROCEDURE ModeSense(lun, pagecontrol, page, length : LONGINT) : FlexibleDiskPage;
+		VAR cmd : ARRAY 12 OF CHAR; data : Usbdi.BufferPtr; actLen, res : LONGINT; fdp : FlexibleDiskPage;
+
+			(* page[index] is first byte of flexible disk page *)
+			PROCEDURE ParseFlexibleDiskPage(page : Usbdi.BufferPtr; index : LONGINT) : FlexibleDiskPage;
+			VAR temp : LONGINT; fdp : FlexibleDiskPage;
+			BEGIN
+				ASSERT((page # NIL) & (LEN(page) - index >= 32));
+				ASSERT(SYSTEM.VAL(LONGINT, (SYSTEM.VAL(SET, page[index]) * {0..5})) = PageFlexibleDisk);
+				NEW(fdp);
+				temp := LSH(ORD(page[index+2]), 8) + ORD(page[index+3]);
+				IF temp = 00FAH THEN fdp.TransferRate := 250;
+				ELSIF temp = 012CH THEN fdp.TransferRate := 300;
+				ELSIF temp = 01F4H THEN fdp.TransferRate := 500;
+				ELSIF temp = 03E8H THEN fdp.TransferRate := 1000;
+				ELSIF temp = 07D0H THEN fdp.TransferRate := 2000;
+				ELSIF temp = 1388H THEN fdp.TransferRate := 5000;
+				ELSIF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: ParseFlexibleDiskPage: Warning: Parse error"); KernelLog.Ln;
+				END;
+				fdp.NumberOfHeads := ORD(page[index+4]);
+				fdp.SectorsPerTrack := ORD(page[index+5]);
+				fdp.BytesPerSector := LSH(ORD(page[index+6]), 8) + ORD(page[index+7]);
+				fdp.NumberOfCylinders := LSH(ORD(page[index+8]), 8) + ORD(page[index+9]);
+				fdp.MotorOnDelay := ORD(page[index+19]);
+				fdp.MotorOffDelay := ORD(page[index+20]);
+				IF fdp.MotorOffDelay = 0FFH THEN fdp.MotorOffDelay := 0; (* don't turn it off !! *) END;
+				fdp.MediumRotationRate := LSH(ORD(page[index+28]), 8) + ORD(page[index+29]);
+				IF Debug.Trace & Debug.traceInfo THEN ShowFlexibleDiskPage(fdp); END;
+				RETURN fdp;
+			END ParseFlexibleDiskPage;
+
+		BEGIN
+			length := length + 8; (* 8 additional bytes for mode parameter header *)
+			cmd[0] := CHR(UfiModeSense);
+			cmd[1] := CHR(LSH(lun, 5)); (* logical unit number *)
+			cmd[2] := CHR(LSH(pagecontrol, 6) + page);
+			cmd[3] := CHR(0); (* reserved *)
+			cmd[5] := CHR(0); (* reserved *)
+			cmd[6] := CHR(0); (* reserved *)
+			cmd[7] := CHR(LSH(length, -8)); (* Parameter List Length (MSB)*)
+			cmd[8] := CHR(length); (* Parameter List Length (LSB) *)
+			cmd[9] := CHR(0); (* reserved *)
+			cmd[10] := CHR(0); (* reserved *)
+			cmd[11] := CHR(0); (* reserved *)
+
+			NEW(data, length);
+			res := InvokeTransport (cmd, 12, DataIn, data.ToArray()^, data.ToArrayOfs(), length, actLen, CommandTimeout);
+			IF ((res # Disks.Ok) OR ((res # ResShortTransfer) & (actLen < length))) THEN
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: UFI Mode Sense command failed."); KernelLog.Ln; END;
+				RETURN NIL;
+			END;
+
+			IF (ORD(data[1]) + 100H*SYSTEM.VAL(LONGINT, ORD(data[0]))) # length + 8 THEN
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: ModeSense: Error: Wrong Mode Data Length returned."); KernelLog.Ln; END;
+				RETURN NIL;
+			END;
+
+			(* parse the result *)
+			CASE page OF
+				PageFlexibleDisk : fdp := ParseFlexibleDiskPage(data, 8);
+			ELSE
+				IF Debug.Level >= Debug.Warnings THEN
+					KernelLog.String("UsbStorage: ModeSense: Parsing of page type "); KernelLog.Hex(page,-2);
+					KernelLog.String(" not (yet) supported."); KernelLog.Ln;
+				END;
+			END;
+			RETURN fdp;
+		END ModeSense;
+
+		(* UFI command: The Request Sense command instructs the UFI device to transfer sense data to the host for
+		 * the specified logical unit *)
+		PROCEDURE RequestSense(lun, cmdlen : LONGINT) : LONGINT;
+		VAR
+			cmd : ARRAY 12 OF CHAR; data : ARRAY 36 OF CHAR;
+			key, asc, ascq : CHAR;
+			information : LONGINT;
+			i, tlen, res : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceSensing THEN KernelLog.String("UsbStorage: Doing auto sense..."); KernelLog.Ln; END;
+			IF (transportProtocol = ProtocolUTS) OR (transportProtocol = ProtocolRBC) THEN
+				cmdlen := 6;
+			END;
+			FOR i := 0 TO 11 DO cmd[i] := CHR(0); END;
+			cmd[0] := CHR(UfiRequestSense);
+			cmd[1] := CHR(LSH(lun, 5));
+			cmd[4] := CHR(18); (* allocation length (max. 18 Bytes) *)
+
+			res := Transport(cmd, cmdlen, DataIn, data, 0, 18, tlen, 2000);
+
+			IF (res = ResDisconnected) THEN
+				RETURN res;
+			ELSIF (res = ResShortTransfer) THEN
+				IF (tlen < 14) THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Crappy device gives not enough sense data"); KernelLog.Ln; END;
+					RETURN ResSenseError;
+				ELSE
+					(* sense >= 14 is ok *)
+				END;
+			ELSIF (transportProtocol = ProtocolUFI) & (res = ResSenseError) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: UFI autosense TransportSenseError"); KernelLog.Ln; END;
+				(* thats ok *)
+			ELSIF res # ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Sense error on protocol sense"); KernelLog.Ln; END;
+				res := Reset(5000);
+				RETURN ResSenseError;
+			END;
+
+			key := CHR(ORD(data[2]) MOD 16); asc := data[12]; ascq := data[13];
+
+			IF Debug.Trace & Debug.traceSensing THEN
+				IF SYSTEM.VAL(SET, ORD(data[0])) * {7} # {} THEN (* information field is valid *)
+					information := ORD(data[3]) + 10H*ORD(data[4]) + 100H*ORD(data[5]) + 1000H*ORD(data[6]);
+				END;
+				ShowSenseData(key, asc, ascq, information, SYSTEM.VAL(SET, ORD(data[0])) * {7} # {});
+			END;
+			IF key = 0X THEN (* No sense -> okay *)
+				IF (res = ResOk) OR (res = ResShortTransfer) THEN RETURN ResOk; END;
+				RETURN ResError;
+			ELSIF (key = 1X) THEN RETURN ResOk; (* Recovered error -> okay *)
+			ELSIF (key = 2X) & (asc = 4X) & (ascq = 01X) THEN RETURN ResDeviceNotReady; (* Logical drive not ready - becoming ready *)
+			ELSIF (key = 2X) & (asc = 4X) & (ascq = 02X) THEN RETURN ResDeviceNotReadyInit; (* Logical drive not ready - initialization required *)
+			ELSIF (key = 2X) & (asc = 4X) & (ascq = 04X) THEN RETURN ResDeviceInUse; (* Logical drive not ready - format in progress *)
+			ELSIF (key = 2X) & (asc = 4X) & (ascq = 0FFX) THEN RETURN ResDeviceInUse; (* Logical drive not ready - device is busy *)
+			ELSIF (key = 2X) & (asc = 6X) & (ascq = 00X) THEN RETURN ResError; (* No reference position found *)
+			ELSIF (key = 2X) & (asc = 8X) & (ascq = 00X) THEN RETURN ResError; (* Logical unit communication failure *)
+			ELSIF (key = 2X) & (asc = 8X) & (ascq = 01X) THEN RETURN ResTimeout; (* Logical unit communication timeout *)
+			ELSIF (key = 2X) & (asc = 8X) & (ascq = 80X) THEN RETURN ResError; (* Logical unit communication overrun *)
+			ELSIF (key = 2X) & (asc = 3AX) & (ascq = 0X) THEN RETURN ResMediaMissing; (* Medium not present *)
+			ELSIF (key = 2X) THEN RETURN ResError;
+			ELSIF (key = 3X) THEN RETURN ResError; (* Medium Error *)
+			ELSIF (key = 4X) THEN RETURN ResError; (* Hardware Error *)
+			ELSIF (key = 5X) THEN RETURN ResUnsupported; (* Illegal Request *)
+			ELSIF (key = 6X) & (asc = 28X) & (ascq = 00X) THEN RETURN ResMediaChanged;
+			ELSIF (key = 6X) & (asc = 29X) & (ascq = 0X) THEN	RETURN ResDeviceNotReady; (* PowerOnReset *)
+			ELSIF (key = 6X) THEN RETURN ResDeviceNotReady;
+			ELSIF (key = 7X) THEN RETURN ResWriteProtected;
+			ELSE
+				RETURN ResError;
+			END;
+		END RequestSense;
+
+	(*	PROCEDURE ReadWrite10(lun, blockSize, op, block, num : LONGINT; VAR data : ARRAY OF CHAR; ofs : LONGINT; VAR diskres : LONGINT);
+		VAR cmd : ARRAY 12 OF CHAR;  trans, tlen, i, res : LONGINT; direction : SET;
+		BEGIN
+			FOR i:= 0 TO 11 DO cmd[i] := 0X; END;
+			IF op = Disks.Read THEN
+				cmd[0] := CHR(UfiRead10); direction := DataIn;
+			ELSE
+				cmd[0] := CHR(UfiWrite10); direction := DataOut;
+			END;
+			cmd[1] := CHR(LSH(lun, 5));  (* Logical device number *)
+
+			WHILE num > 0 DO
+				IF num > 65000 THEN trans := 65000; ELSE trans := num END;
+				trans := trans * blockSize;
+				cmd[2] := CHR(LSH(block, -24));
+				cmd[3] := CHR(LSH(block, -16));
+				cmd[4] := CHR(LSH(block, -8));
+				cmd[5] := CHR(block);
+				cmd[7] := CHR(LSH(num, -8));
+				cmd[8] := CHR(num);
+				diskres := InvokeTransport(cmd, 12, direction, data, ofs, trans, tlen, TransferTimeout);
+				IF diskres # Disks.Ok THEN RETURN; END;
+				block := block + trans;
+				num := num - trans;
+			END;
+		END ReadWrite10;
+
+		PROCEDURE ReadWrite12(lun, op, block, num : LONGINT; VAR data : ARRAY OF CHAR; ofs : LONGINT; VAR diskres : LONGINT);
+		VAR cmd : ARRAY 12 OF CHAR;  trans, tlen, i, res : LONGINT; direction : SET;
+		BEGIN
+			ASSERT(num < MAX(LONGINT));
+			FOR i:= 0 TO 11 DO cmd[i] := 0X; END;
+			IF op = Disks.Read THEN
+				cmd[0] := CHR(UfiRead12); direction := DataIn;
+			ELSE
+				cmd[0] := CHR(UfiWrite12); direction := DataOut;
+			END;
+			cmd[1] := CHR(LSH(lun, 5));  (* Logical device number *)
+			cmd[2] := CHR(LSH(block, -24));
+			cmd[3] := CHR(LSH(block, -16));
+			cmd[4] := CHR(LSH(block, -8));
+			cmd[5] := CHR(block);
+			cmd[6] := CHR(LSH(num, -24));
+			cmd[7] := CHR(LSH(num, -16));
+			cmd[8] := CHR(LSH(num, -8));
+			cmd[9] := CHR(num);
+			diskres := InvokeTransport(cmd, 12, direction, data, ofs, trans, tlen, TransferTimeout);
+		END ReadWrite12; *)
+
+		(* Generic Transport Handler  *)
+		PROCEDURE InvokeTransport (CONST cmd : ARRAY OF CHAR; cmdlen : INTEGER; dir : SET;
+			VAR data : ARRAY OF CHAR; ofs, datalen : LONGINT; VAR tlen : LONGINT; timeout : LONGINT) : LONGINT;
+		VAR
+			sensecmdlen, res, retry, i : LONGINT; forceRetry : BOOLEAN;
+		BEGIN {EXCLUSIVE}
+			(* here one could add additional stuff for the different protocols *)
+			IF transportProtocol = ProtocolUFI THEN
+				cmdlen := 12; sensecmdlen := 12;
+			ELSIF (transportProtocol = ProtocolUTS) OR (transportProtocol = ProtocolRBC) THEN
+				(* all ok *) sensecmdlen := 6;
+			ELSIF (transportProtocol = Protocol8020) OR (transportProtocol = Protocol8070) THEN
+				cmdlen := 12; sensecmdlen := 12;
+			ELSIF transportProtocol = ProtocolQIC157 THEN
+				cmdlen := 12; sensecmdlen := 12;
+			END;
+
+			retry := 0; (* retries for "power up/reset" and "lun becoming ready" *)
+
+			LOOP
+				IF Debug.Trace & Debug.traceScTransfers THEN
+					KernelLog.String("UsbStorageBase: [cmd: ");
+					IF transportProtocol = ProtocolUFI THEN ShowUFICmd(cmd[0]); END;
+					FOR i := 0 TO cmdlen-1 DO KernelLog.String(" "); KernelLog.Hex(ORD(cmd[i]), -2); END;
+					KernelLog.String(" BufferLen: "); KernelLog.Int(datalen, 0); KernelLog.String(" Bytes]");
+					KernelLog.Ln;
+				END;
+				res := Transport(cmd, cmdlen, dir, data, ofs, datalen, tlen, timeout);
+				IF Debug.Trace & Debug.traceScTransfers THEN
+					KernelLog.String("UsbStorageBase: Sent "); KernelLog.Int(cmdlen, 0); KernelLog.String(" bytes commands, res: ");
+					ShowRes(res); KernelLog.Ln;
+				END;
+
+				IF (res = ResDisconnected) THEN
+					RETURN res;
+				END;
+
+				forceRetry := FALSE;
+				IF (res = ResFatalError) OR (res = ResTimeout) THEN
+					res := Reset(5000); (* ignore res *)
+					IF res # ResOk THEN
+					(*	TODO:
+						lun := SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, ORD(cmd[1])) * {5..7}, -5));
+						res := SendDiagnostic(lun); (* hardware reset; ignore res *) *)
+						RETURN ResFatalError;
+					ELSE
+						forceRetry := TRUE;
+						res := ResError; (* retry *)
+					END;
+				END;
+
+				IF (res = ResShortTransfer) & (transportMethod # MethodCB) THEN
+					IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorageBase: Had a short read"); KernelLog.Ln; END;
+					RETURN ResShortTransfer;
+				END;
+
+				(* Do an auto-sense if something was not ok or if we are using the CB (not CBI) transport method *)
+				IF (res = ResOk) & (transportMethod # MethodCB) THEN
+					RETURN ResOk;
+				END;
+
+				(* It makes no sense to auto-sense on Inquiry/Sense on UFI/CB (not CBI)*)
+				IF (transportMethod = MethodCB) & (transportProtocol = ProtocolUFI) & ((res = ResOk) OR (res = ResShortTransfer)) THEN
+					IF (ORD(cmd[0]) = UfiInquiry) OR (ORD(cmd[0]) = UfiRequestSense) THEN RETURN res; END;
+				END;
+
+				res := RequestSense(SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, ORD(cmd[1])) * {5..7}, -5)), sensecmdlen);
+				IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorageBase: Sent sense command, res: "); ShowRes(res); KernelLog.Ln; END;
+
+				IF (res = ResWriteProtected) OR (res = ResDeviceInUse) OR (res = ResMediaMissing) OR (res = ResUnsupported) OR (res = ResDisconnected)THEN
+					RETURN res; (* don't retry *)
+				END;
+
+				IF forceRetry OR (res = ResDeviceNotReady) THEN
+					INC(retry);
+					IF retry = 4 THEN
+						IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorageBase: Too many protocol retries, giving up"); KernelLog.Ln; END;
+						EXIT;
+					ELSE
+						IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorageBase: Retry #"); KernelLog.Int(retry, 0); KernelLog.Ln; END;
+					END;
+				ELSE
+					EXIT;
+				END;
+
+				Wait(50); (* try again *)
+			END;
+			RETURN res;
+		END InvokeTransport;
+
+		PROCEDURE Initialization*() : BOOLEAN;
+		BEGIN
+			(* dummy *)
+			RETURN TRUE;
+		END Initialization;
+
+		PROCEDURE RegisterDevice(lun : LONGINT) : BOOLEAN;
+		VAR stordev : UsbStorageDevice; info : InquiryResult; i, j : LONGINT;
+		BEGIN
+			info := Inquiry(lun);
+			IF info # NIL THEN
+				NEW(stordev);
+				stordev.SetName("USB");
+				IF info.validStrings THEN (* override description *)
+					Lib.TrimWS(info.vendor);
+					stordev.desc := ""; i := 0; j := 0;
+					LOOP (* append vendor string *)
+						IF (i >= LEN(info.vendor)) OR (info.vendor[i] = 0X) THEN EXIT; END;
+						stordev.desc[j] := info.vendor[i];
+						INC(i); INC(j);
+					END;
+					IF j # 0 THEN stordev.desc[j] := " "; INC(j); END;
+					Lib.TrimWS(info.product);
+					i := 0;
+					LOOP (* append product string *)
+						IF (i >= LEN(info.product)) OR (info.product[i] = 0X) THEN EXIT; END;
+						stordev.desc[j] := info.product[i];
+						INC(i); INC(j);
+					END;
+					stordev.desc[j] := 0X;
+				ELSE (* use USB vendor/product strings *)
+					stordev.desc := description;
+				END;
+				stordev.lun := lun;
+				stordev.transportProtocol := transportProtocol;
+				stordev.blockSize := 0;
+				stordev.flags := {}; stordev.table := NIL; stordev.openCount := 0;
+				stordev.usbDriver := SELF;
+				IF (info.deviceType = DtCDROM) OR (info.deviceType = DtOpticalMemory) THEN
+					stordev.flags := stordev.flags + {Disks.ReadOnly};
+				END;
+				IF info.removable THEN stordev.flags := stordev.flags + {Disks.Removable}; END;
+				diskManager.Add(stordev);
+				RETURN TRUE;
+			ELSE
+				IF Debug.Level >= Debug.Default THEN
+					KernelLog.String("UsbStorage: Inquiry for device LUN "); KernelLog.Int(lun, 0); KernelLog.String(" failed."); KernelLog.Ln;
+				END;
+				RETURN FALSE;
+			END;
+		END RegisterDevice;
+
+		PROCEDURE RegisterDevices() : BOOLEAN;
+		VAR lun, maxlun, res : LONGINT; succeeded : LONGINT; (* How many storage device were added to the disk manager? *)
+		BEGIN
+			maxlun := 0; succeeded := 0;
+			IF transportMethod = MethodBulkOnly THEN (* logical devices support *)
+				res := GetMaxLun(maxlun);
+				IF res = ResOk THEN
+					IF Debug.Trace & Debug.traceInfo THEN KernelLog.String("UsbStorageBase: MaxLUN is "); KernelLog.Int(maxlun, 0); KernelLog.Ln; END;
+				ELSIF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageBase: GetMaxLun failed: "); ShowRes(res); KernelLog.Ln;
+				END;
+			END;
+
+			FOR lun := 0 TO maxlun DO
+				IF RegisterDevice(lun) THEN INC(succeeded); END;
+			END;
+
+			RETURN succeeded > 0;
+		END RegisterDevices;
+
+		PROCEDURE Connect*(): BOOLEAN;
+		BEGIN
+			(* note that this procedure is common to the Bulkonly, CB and CB/I transport layer *)
+			(* get the default control pipe *)
+			defaultPipe := device.GetPipe(0);
+
+			(* get the bulk pipes *)
+			bulkInPipe := device.GetPipe(bulkIn);
+			bulkOutPipe := device.GetPipe(bulkOut);
+
+			IF (bulkInPipe=NIL) OR (bulkOutPipe=NIL)  THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Could not allocate pipes"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			(* if the interface support a interrupt endpoint, get the corresponding pipe *)
+			IF (interrupt # 0) & (transportMethod # MethodBulkOnly) THEN
+				interruptPipe := device.GetPipe(interrupt);
+				IF interruptPipe = NIL THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Could not allocate interrupt pipe."); KernelLog.Ln; END;
+					RETURN FALSE;
+				END;
+			END;
+
+			IF initialize & ~Initialization() THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Could not initialize device."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			IF ~RegisterDevices() THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Error while registering new devices."); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			RETURN TRUE;
+		END Connect;
+
+		PROCEDURE Disconnect*;
+		BEGIN
+			diskManager.RemovedDriver(SELF);
+		END Disconnect;
+
+		PROCEDURE Wait(ms : LONGINT);
+		BEGIN
+			timer.Sleep(ms);
+		END Wait;
+
+		PROCEDURE &Init*;
+		BEGIN
+			NEW(timer);
+		END Init;
+
+	END StorageDriver;
+
+TYPE
+
+	(* Manages a list of all installed USB storage devices and registers/unregisters them at the Disks.registry *)
+	DiskManager = OBJECT
+	VAR
+		storageDeviceList : UsbStorageDevice;
+		suffixUsed : ARRAY 100 OF BOOLEAN;
+		regName : ARRAY 32 OF CHAR;
+		res : LONGINT;
+
+		PROCEDURE Add*(dev : UsbStorageDevice);
+		VAR i : LONGINT;
+		BEGIN {EXCLUSIVE}
+			ASSERT(dev#NIL);
+			(* add new device to list *)
+			dev.next := storageDeviceList.next; storageDeviceList.next := dev;
+
+			(* get unused suffix *)
+			i := 0; WHILE suffixUsed[i] & (i < 100) DO INC(i) END;
+			IF (i = 99) & suffixUsed[99] THEN
+				KernelLog.String("UsbStorage: Can't register storage device. Maximal 100 devices supported."); KernelLog.Ln;
+				RETURN;
+			END;
+
+			(* generate unique device name *)
+			suffixUsed[i] := TRUE; dev.number := i;
+
+			i := 1; WHILE (dev.name[i] # 0X) & (i < 32) DO INC(i); END;
+			IF (dev.name[i] # 0X) THEN
+				KernelLog.String("UsbStorage: Error: Couldn't register the device "); KernelLog.String(dev.name);
+				KernelLog.String(" (device names shall be 2-30 characters long (incl. 0X)"); KernelLog.Ln;
+				suffixUsed[dev.number] := FALSE;
+				RETURN;
+			END;
+
+			COPY(dev.name, regName);
+
+			IF dev.number < 10 THEN
+				regName[i] := CHR(ORD("0") + dev.number); regName[i+1] := 0X;
+			ELSIF dev.number < 100 THEN
+				regName[i] := CHR(ORD("0") + dev.number DIV 10);
+				regName[i+1] := CHR(ORD("0") + dev.number MOD 10); regName[i+2] := 0X;
+			END;
+
+			dev.SetName(regName);
+
+			Disks.registry.Add(dev, res);
+			IF res # Plugins.Ok THEN
+				KernelLog.String("UsbStorage: Error: Couldn't add device to Disks.registry (Error code: ");
+				KernelLog.Int(res, 0); KernelLog.String(")"); KernelLog.Ln;
+				suffixUsed[dev.number] := FALSE;
+				RETURN;
+			END;
+			IF Debug.Verbose THEN
+				KernelLog.String("UsbStorage: Storage device "); KernelLog.String(dev.name);
+				KernelLog.String(" ("); KernelLog.String(dev.desc); KernelLog.String(") is now accessible."); KernelLog.Ln;
+			END;
+		END Add;
+
+		(** Remove storage device from Disks.registry. *)
+		PROCEDURE Remove*(dev : UsbStorageDevice);
+		VAR temp : UsbStorageDevice;
+		BEGIN {EXCLUSIVE}
+			temp:=storageDeviceList;
+			WHILE (temp.next#NIL) & (temp.next.name#dev.name) DO temp:=temp.next; END;
+			IF (temp.next=NIL) OR (temp.next.name#dev.name) THEN
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: Warning: Couldn't remove device from registry (device not found)"); KernelLog.Ln; END;
+			ELSE
+				temp.next := temp.next.next;
+				Disks.registry.Remove(dev);
+				suffixUsed[dev.number]:=FALSE;
+			END;
+			IF Debug.Verbose THEN
+				KernelLog.String("UsbStorage: Removed storage device "); KernelLog.String(dev.name);
+				KernelLog.String(" ("); KernelLog.String(dev.desc); KernelLog.String(")"); KernelLog.Ln;
+			END;
+		END Remove;
+
+		(* Remove all storage devices associated to the specifed storage device driver *)
+		PROCEDURE RemovedDriver*(driver : StorageDriver);
+		VAR temp : UsbStorageDevice;
+		BEGIN {EXCLUSIVE}
+			temp := storageDeviceList;
+			WHILE(temp # NIL) & (temp.next # NIL) DO
+				IF temp.next.usbDriver = driver THEN
+					Disks.registry.Remove(temp.next);
+					suffixUsed[temp.next.number] := FALSE;
+					IF Debug.Verbose THEN
+						KernelLog.String("UsbStorage: Remove storage device "); KernelLog.String(temp.next.name);
+						KernelLog.String(" ("); KernelLog.String(temp.next.desc); KernelLog.String(")"); KernelLog.Ln;
+					END;
+					temp.next := temp.next.next;
+				ELSE
+					temp := temp.next;
+				END;
+			END;
+		END RemovedDriver;
+
+		(* Removes all USB storage devices from the Disks.registry. *)
+		PROCEDURE RemoveAll*;
+		VAR temp : UsbStorageDevice;
+		BEGIN {EXCLUSIVE}
+			temp := storageDeviceList.next;
+			WHILE temp # NIL DO
+				Disks.registry.Remove(temp);
+				suffixUsed[temp.number] := FALSE;
+				temp := temp.next;
+			END;
+			storageDeviceList.next := NIL;
+			IF Debug.Verbose THEN KernelLog.Enter; KernelLog.String("UsbStorage: All storage devices removed."); KernelLog.Exit; END;
+		END RemoveAll;
+
+		(* Displays a list of all USB storage devices which are registered at the DiskManager *)
+		PROCEDURE Show;
+		VAR temp : UsbStorageDevice;
+		BEGIN
+			KernelLog.String("UsbStorage: Accessible USB storage devices:"); KernelLog.Ln;
+			IF storageDeviceList.next=NIL THEN
+				KernelLog.String("No devices registred.");
+			ELSE
+				temp := storageDeviceList.next;
+				WHILE temp # NIL DO
+					KernelLog.String(temp.name); KernelLog.String(" ("); KernelLog.String(temp.desc); KernelLog.String(")"); KernelLog.Ln;
+					temp := temp.next;
+				END;
+				KernelLog.Ln;
+			END;
+		END Show;
+
+		PROCEDURE &Init*;
+		BEGIN
+			NEW(storageDeviceList);
+		END Init;
+
+	END DiskManager;
+
+VAR
+	diskManager- : DiskManager;
+	performance- : LONGINT;
+
+(* displays the UFI command to KernelLog *)
+PROCEDURE ShowUFICmd(cmd : CHAR) ;
+BEGIN
+	IF Debug.Trace THEN
+	CASE ORD(cmd) OF
+		04H : KernelLog.String("Format Unit");
+		| 12H :  KernelLog.String("Inquiry");
+		| 55H : KernelLog.String("Mode Select");
+		| 5AH : KernelLog.String("Mode Sense");
+		| 1EH : KernelLog.String("Prevent-Allow Media Removal");
+		| 28H : KernelLog.String("Read(10)");
+		| 0A8H : KernelLog.String("Read(12)");
+		| 25H : KernelLog.String("Read Capacity");
+		| 23H : KernelLog.String("Read Format Capabilities");
+		| 03H : KernelLog.String("Request Sense");
+		| 01H : KernelLog.String("Rezero");
+		| 2BH : KernelLog.String("Seek(10)");
+		| 1DH : KernelLog.String("Send Diagnostic");
+		| 1BH :KernelLog.String("Start-Stop Unit");
+		| 00H : KernelLog.String("Test Unit Ready");
+		| 2FH : KernelLog.String("Verify");
+		| 2AH : KernelLog.String("Write(10)");
+		| 0AAH : KernelLog.String("Write(12)");
+		| 2EH : KernelLog.String("Write and Verify");
+	ELSE
+		KernelLog.String("Unknown Command("); KernelLog.Int(ORD(cmd), 0); KernelLog.String(")");
+	END;
+	END;
+END ShowUFICmd;
+
+PROCEDURE ShowInquiryResult(i : InquiryResult);
+BEGIN
+	IF Debug.Trace  THEN
+	KernelLog.String("Inquiry data:"); KernelLog.Ln;
+	IF i # NIL THEN
+		KernelLog.String("   Peripheral device type: "); KernelLog.Int(i.deviceType, 0);
+		IF i.removable THEN KernelLog.String(" [removable]"); ELSE KernelLog.String(" [not removable]"); END; KernelLog.Ln;
+		KernelLog.String("   ANSI version: "); KernelLog.Int(i.ansiVersion, 0); KernelLog.Ln;
+		KernelLog.String("   Additional Length: "); KernelLog.Int(i.additionalLength, 0); KernelLog.String("B"); KernelLog.Ln;
+		KernelLog.String("   Vendor: "); KernelLog.String(i.vendor); KernelLog.Ln;
+		KernelLog.String("   Product: "); KernelLog.String(i.product); KernelLog.Ln;
+		KernelLog.String("   Revision: "); KernelLog.String(i.revision); KernelLog.Ln;
+	ELSE
+		KernelLog.String("Information not available");
+	END;
+	END;
+END ShowInquiryResult;
+
+PROCEDURE ShowFlexibleDiskPage(fdp : FlexibleDiskPage);
+BEGIN
+	IF Debug.Trace  THEN
+	KernelLog.String("UsbStorage: Flexible Disk Page Contents: "); KernelLog.Ln;
+	IF fdp # NIL THEN
+		KernelLog.String("   Transfer rate: "); KernelLog.Int(fdp.TransferRate, 0); KernelLog.String(" kbits/s"); KernelLog.Ln;
+		KernelLog.String("   Cylinders: "); KernelLog.Int(fdp.NumberOfCylinders, 0);
+		KernelLog.String("   Heads: "); KernelLog.Int(fdp.NumberOfHeads, 0);
+		KernelLog.String(" SectorsPerTrack: "); KernelLog.Int(fdp.SectorsPerTrack, 0);
+		KernelLog.String(" BytesPerSector: "); KernelLog.Int(fdp.BytesPerSector, 0); KernelLog.Ln;
+		KernelLog.String("   Motordelay On: "); KernelLog.Int(fdp.MotorOnDelay, 0);
+		KernelLog.String(" Off: "); 	IF fdp.MotorOffDelay = 0 THEN KernelLog.String("Don't turn off!!"); ELSE KernelLog.Int(fdp.MotorOffDelay, 0); END;
+		KernelLog.Ln;
+		KernelLog.String("   Medium Rotation rate: "); KernelLog.Int(fdp.MediumRotationRate, 0); KernelLog.String(" rpm"); KernelLog.Ln;
+	ELSE
+		KernelLog.String("Information not available"); KernelLog.Ln;
+	END;
+	END;
+END ShowFlexibleDiskPage;
+
+PROCEDURE ShowSenseData(key, asc, ascq : CHAR;  information : LONGINT; valid : BOOLEAN);
+BEGIN
+	IF Debug.Trace THEN
+	KernelLog.String("UsbStorage: Sense Key: "); KernelLog.Int(ORD(key), 0);
+	KernelLog.String(" asc: "); KernelLog.Hex(ORD(asc), 0); KernelLog.String(" ascq: "); KernelLog.Hex(ORD(ascq), 0); KernelLog.String(" -> ");
+	IF key = 0X THEN KernelLog.String("Device reports error, but auto-sense gives 0X");
+	ELSIF (key = 1X) & (asc = 17X) & (ascq = 01X) THEN KernelLog.String("Recovered data with retries");
+	ELSIF (key = 1X) & (asc = 18X) & (ascq = 00X) THEN KernelLog.String("Recovered data with ECC");
+	ELSIF (key = 1X) THEN KernelLog.String("Recovered Error");
+	ELSIF (key = 2X) & (asc = 04X) & (ascq = 01X) THEN KernelLog.String("Logical drive not ready, becoming ready");
+	ELSIF (key = 2X) & (asc = 04X) & (ascq = 02X) THEN KernelLog.String("Logical drive not ready - initialization required");
+	ELSIF (key = 2X) & (asc = 04X) & (ascq = 04X) THEN KernelLog.String("Logical unit not ready - format in progress");
+	ELSIF (key = 2X) & (asc = 04X) & (ascq = 0FFX) THEN KernelLog.String("Logical drive not ready - device is busy");
+	ELSIF (key = 2X) & (asc = 06X) & (ascq = 00X) THEN KernelLog.String("No reference position found");
+	ELSIF (key = 2X) & (asc = 08X) & (ascq = 00X) THEN KernelLog.String("Logical unit communication failure");
+	ELSIF (key = 2X) & (asc = 08X) & (ascq = 01X) THEN KernelLog.String("Logical unit communication timeout");
+	ELSIF (key = 2X) & (asc = 08X) & (ascq = 80X) THEN KernelLog.String("Logical unit communication overrun");
+	ELSIF (key = 2X) & (asc = 3AX) & (ascq = 00X) THEN KernelLog.String("Medium not present");
+	ELSIF (key = 2X) & (asc = 54X) & (ascq = 00X) THEN KernelLog.String("USB to host system interface failure");
+	ELSIF (key = 2X) & (asc = 80X) & (ascq = 00X) THEN KernelLog.String("Insufficient resources");
+	ELSIF (key = 2X) & (asc = 0FFX) & (ascq = 0FFX) THEN KernelLog.String("Unknown error");
+	ELSIF (key = 2X) THEN KernelLog.String("Not Ready");
+	ELSIF (key = 3X) & (asc = 02X) & (ascq = 00X) THEN KernelLog.String("No seek complete");
+	ELSIF (key = 3X) & (asc = 03X) & (ascq = 00X) THEN KernelLog.String("Write fault");
+	ELSIF (key = 3X) & (asc = 10X) & (ascq = 00X) THEN KernelLog.String("ID CRC Error");
+	ELSIF (key = 3X) & (asc = 11X) & (ascq = 00X) THEN KernelLog.String("Unrecovered read error");
+	ELSIF (key = 3X) & (asc = 12X) & (ascq = 00X) THEN KernelLog.String("Address mark not found for ID field");
+	ELSIF (key = 3X) & (asc = 13X) & (ascq = 00X) THEN KernelLog.String("Address mark not found for data field");
+	ELSIF (key = 3X) & (asc = 14X) & (ascq = 00X) THEN KernelLog.String("Recorded entity not found");
+	ELSIF (key = 3X) & (asc = 30X) & (ascq = 01X) THEN KernelLog.String("Cannot read medium - unknown format");
+	ELSIF (key = 3X) & (asc = 31X) & (ascq = 01X) THEN KernelLog.String("Format command failed");
+	ELSIF (key = 3X) THEN KernelLog.String("Medium Error");
+	ELSIF (key = 4X) & (asc = 40X) THEN KernelLog.String("Diagnostic failure on component "); KernelLog.Int(ORD(ascq), 0);
+	ELSIF (key = 4X) THEN KernelLog.String("Hardware Error");
+	ELSIF (key = 5X) & (asc = 1AX) & (ascq = 00X) THEN KernelLog.String("Parameter list length error");
+	ELSIF (key = 5X) & (asc = 20X) & (ascq = 00X) THEN KernelLog.String("Invalid command operation code");
+	ELSIF (key = 5X) & (asc = 21X) & (ascq = 00X) THEN KernelLog.String("Logical block address out of range");
+	ELSIF (key = 5X) & (asc = 24X) & (ascq = 00X) THEN KernelLog.String("Invalid field in command packet");
+	ELSIF (key = 5X) & (asc = 25X) & (ascq = 00X) THEN KernelLog.String("Logical unit not supported");
+	ELSIF (key = 5X) & (asc = 26X) & (ascq = 00X) THEN KernelLog.String("Invalid field in parameter list");
+	ELSIF (key = 5X) & (asc = 26X) & (ascq = 01X) THEN KernelLog.String("Parameter not supported");
+	ELSIF (key = 5X) & (asc = 26X) & (ascq = 02X) THEN KernelLog.String("Parameter value invalid");
+	ELSIF (key = 5X) & (asc = 39X) & (ascq = 00X) THEN KernelLog.String("Saving parameters not supported");
+	ELSIF (key = 5X) THEN KernelLog.String("Illegal Request");
+	ELSIF (key = 6X) & (asc = 28X) & (ascq = 00X) THEN KernelLog.String("Not ready to ready transition - media changed");
+	ELSIF (key = 6X) & (asc = 29X) & (ascq = 00X) THEN	KernelLog.String("PowerOnReset, retrying");
+	ELSIF (key = 6X) & (asc = 2FX) & (ascq = 00X) THEN	KernelLog.String("Commands cleared by another indicator");
+	ELSIF (key = 6X) THEN KernelLog.String("Unit Attention");
+	ELSIF (key = 7X) & (asc = 27X) & (ascq = 00X) THEN KernelLog.String("Write protected media");
+	ELSIF (key = 7X) THEN KernelLog.String("Data Protected");
+	ELSIF (key = 8X) THEN KernelLog.String("Blank Check");
+	ELSIF (key = 9X) THEN KernelLog.String("Vendor Specific");
+	ELSIF (key = 0AX) THEN KernelLog.String("Reserved");
+	ELSIF (key = 0BX) & (asc = 4EX) & (ascq = 00X) THEN KernelLog.String("Overlapped command attemted");
+	ELSIF (key = 0BX) THEN KernelLog.String("Aborted command");
+	ELSIF (key = 0CX) THEN KernelLog.String("Reserved");
+	ELSIF (key = 0DX) THEN KernelLog.String("Volume Overflow");
+	ELSIF (key = 0EX) THEN KernelLog.String("Miscompare");
+	ELSIF (key = 0FX) THEN KernelLog.String("Reserved");
+	ELSE KernelLog.String("Unkown");
+	END;
+	IF valid THEN KernelLog.String(" [information: "); KernelLog.Int(information, 0); KernelLog.String("]"); END;
+	KernelLog.Ln;
+	END;
+END ShowSenseData;
+
+PROCEDURE ShowRes(res : LONGINT);
+BEGIN
+	IF Debug.Level >= Debug.Errors THEN
+	CASE res OF
+		ResOk: KernelLog.String("OK");
+		|ResWriteProtected: KernelLog.String("Write-Protected");
+		|ResDeviceInUse: KernelLog.String("DeviceInUse");
+		|ResMediaChanged: KernelLog.String("MediaChanged");
+		|ResMediaMissing: KernelLog.String("MediaMissing");
+		|ResUnsupported: KernelLog.String("Unsupported");
+		|ResTimeout: KernelLog.String("Timeout");
+		|ResShortTransfer: KernelLog.String("ShortTransfer");
+		|ResDeviceNotReady: KernelLog.String("DeviceNotReady");
+		|ResDeviceNotReadyInit: KernelLog.String("DeviceNotReady-need init");
+		|ResSenseError: KernelLog.String("SenseError");
+		|ResError: KernelLog.String("Error");
+		|ResFatalError: KernelLog.String("FatalError");
+		|ResDisconnected: KernelLog.String("Disconnected");
+	ELSE
+		KernelLog.String("Unknown(res="); KernelLog.Int(res, 0); KernelLog.String(")");
+	END;
+	END;
+END ShowRes;
+
+(** Shows all devices which are registered at the UsbStorage Disk Manager and the Disks.registry *)
+PROCEDURE Show*(context : Commands.Context);
+VAR table : Plugins.Table; i : LONGINT;
+BEGIN
+	(* show all devices which are registered at the UsbStorage disk manager *)
+	diskManager.Show;
+	(* show all devices registered at Disks.registry *)
+	Disks.registry.GetAll(table);
+	context.out.String("Storage devices registered at Disks.registry:"); context.out.Ln;
+	IF table = NIL THEN
+		context.out.String("No devices registered."); context.out.Ln;
+	ELSE
+		FOR i := 0 TO LEN(table)-1 DO
+			context.out.String(table[i].name); context.out.String(" ("); context.out.String(table[i].desc); context.out.String(")"); context.out.Ln;
+		END;
+	END;
+END Show;
+
+(* ToBeRemoved *)
+PROCEDURE SetMax*(context : Commands.Context);
+BEGIN
+	context.out.String("UsbStorage: Max performance mode."); context.out.Ln;
+	performance := Usbdi.MaxPerformance;
+END SetMax;
+
+(* ToBeRemoved *)
+PROCEDURE SetNormal*(context : Commands.Context);
+BEGIN
+	context.out.String("UsbStorage: Normal performance mode."); context.out.Ln;
+	performance := Usbdi.Normal;
+END SetNormal;
+
+BEGIN
+	NEW(diskManager);
+	performance := Usbdi.MaxPerformance;
+END UsbStorageBase.
+
+UsbStorage.Install ~ SystemTools.Free UsbStorage ~
+
+UsbStorage.Show ~

source/ARM.UsbStorageBot.Mod

@@ -0,0 +1,239 @@
+MODULE UsbStorageBot; (** AUTHOR "staubesv"; PURPOSE "Bulk-Only transport layer of USB mass storage driver"; *)
+(**
+ * References:
+ *
+ * 	-	Universal Serial Bus Mass Storage Class Bulk-Only Transport, Revision 1.0, September 31, 1999
+ *		www.usb.org
+ *
+ * History:
+ *
+ *	09.02.2006	First release (staubesv)
+ *	05.07.2006	Adapted to Usbi (staubesv)
+ *)
+
+IMPORT
+	SYSTEM, KernelLog,
+	Usbdi, Base := UsbStorageBase, Debug := UsbDebug;
+
+TYPE
+
+	(* USB mass storage class bulk only transport layer *)
+	BulkOnlyTransport* = OBJECT(Base.StorageDriver)
+	VAR
+		CBWbuffer : Usbdi.BufferPtr;
+		CSWbuffer : Usbdi.BufferPtr;
+		seqNbr : LONGINT;
+
+		(* Perform reset recovery, i.e. send Bulk-Only Mass Storage Reset command via default control pipe and 	*)
+		(* then clear the EndpointHalt feature of the bulk in and bulk out endpoints of the USB device. The request 	*)
+		(* shall ready the device for the next CBW from the host. See [2], pages 7 & 16								*)
+		PROCEDURE Reset*(timeout : LONGINT) : LONGINT;
+		VAR critical : BOOLEAN; status : Usbdi.Status;
+		BEGIN
+			IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorage: Doing reset recovery ... "); END;
+			(* Mass storage devices request: Bulk-Only Mass Storage Reset *)
+			status := device.Request(Usbdi.ToDevice + Usbdi.Class + Usbdi.Interface, 255, 0, interface.bInterfaceNumber, 0, Usbdi.NoData);
+			IF (status = Usbdi.Disconnected) THEN
+				RETURN Base.ResDisconnected;
+			ELSIF status # Usbdi.Ok THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Mass storage reset failed."); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+			IF bulkInPipe.IsHalted() THEN critical := ~bulkInPipe.ClearHalt(); END;
+			IF bulkOutPipe.IsHalted() THEN critical := critical OR ~bulkOutPipe.ClearHalt(); END;
+			IF critical THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Failure on BulkOnly reset ClearHalt"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+			IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorage: Reset recovery succeeded."); KernelLog.Ln; END;
+			RETURN Base.ResOk;
+		END Reset;
+
+		(* The Get Max LUN device request is used to determine the number of logical units supported by the device *)
+		PROCEDURE GetMaxLun*(VAR maxlun : LONGINT): LONGINT;
+		VAR buffer : Usbdi.BufferPtr;  status : Usbdi.Status;
+		BEGIN
+			IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorage: GetMaxLUN.... "); END;
+			NEW(buffer, 1);
+			status := device.Request(Usbdi.ToHost + Usbdi.Class + Usbdi.Interface, 254, 0, interface.bInterfaceNumber, 1, buffer);
+			IF status = Usbdi.Ok THEN
+				maxlun := ORD(buffer[0]);
+				IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("MaxLUN is: "); KernelLog.Int(maxlun, 0); KernelLog.Ln; END;
+				RETURN Base.ResOk;
+			ELSIF status = Usbdi.Stalled THEN (* Devices that do not suppoert multiple LUNs may stall this command *)
+				maxlun := 0;
+				IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("MaxLUN request not supported (STALL)"); KernelLog.Ln; END;
+				RETURN Base.ResOk;
+			ELSIF status = Usbdi.Disconnected THEN
+				RETURN Base.ResDisconnected;
+			ELSE
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: GetMaxLUN request failed."); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+		END GetMaxLun;
+
+		(* Process a bulk-only transfer. A three stage protocol is used:												*)
+		(* 	1. Send command block wrapper (CBW) to device														*)
+		(*	2. Dataphase (optional)																				*)
+		(*	3. Receive command status word (CSW) from device 													*)
+		PROCEDURE Transport*(cmd : ARRAY OF CHAR; cmdlen : LONGINT; dir :  SET;
+			VAR buffer : ARRAY OF CHAR; ofs, bufferlen : LONGINT; VAR tlen : LONGINT; timeout : LONGINT) : LONGINT;
+		VAR status : Usbdi.Status; i, residual : LONGINT;
+			b: Usbdi.Buffer;
+		BEGIN (* No concurrency allowed *)
+			ASSERT((cmdlen > 0) & (cmdlen <= 16)); (* [2], page 14 *)
+			(* set up dCBWSignature *)
+			CBWbuffer[0] := 55X; CBWbuffer[1] := 53X; CBWbuffer[2] := 42X; CBWbuffer[3] := 43X;
+			(* set up dCBWTag - will be echoed by the device *)
+			INC(seqNbr);
+			CBWbuffer[4] := CHR(seqNbr);
+			CBWbuffer[5] := CHR(LSH(seqNbr, -8));
+			CBWbuffer[6] := CHR(LSH(seqNbr, -16));
+			CBWbuffer[7] := CHR(LSH(seqNbr, -24));
+			(* set up dCBWDataTransferLength *)
+			CBWbuffer[8] := CHR(bufferlen);
+			CBWbuffer[9] := CHR(LSH(bufferlen, -8));
+			CBWbuffer[10] := CHR(LSH(bufferlen, -16));
+			CBWbuffer[11] := CHR(LSH(bufferlen, -24));
+			(* set up bmCBWFlags *)
+			IF dir = Base.DataIn THEN CBWbuffer[12] := 80X; ELSE CBWbuffer[12] := 0X; END;
+			(* set bCBWLUN *)
+			CBWbuffer[13] := CHR(SYSTEM.VAL(LONGINT, LSH(SYSTEM.VAL(SET, ORD(cmd[1])) * {5..7}, -5)));
+			(*set bCBWCBLength *)
+			CBWbuffer[14] := CHR(cmdlen);
+			FOR i := 15 TO 30 DO CBWbuffer[i] := 0X; END;
+			(* copy CBWCB *)
+			FOR i := 0 TO cmdlen-1 DO CBWbuffer[15+i] := cmd[i]; END;
+			IF Debug.Trace & Debug.traceCBWs THEN
+				KernelLog.String("Sending CBW: "); FOR i := 0 TO LEN(CBWbuffer)-1 DO KernelLog.Hex(ORD(CBWbuffer[i]), -2); KernelLog.Char(" "); END; KernelLog.Ln;
+			END;
+
+			(* send the CBW *)
+			IF Base.performance = Usbdi.MaxPerformance THEN bulkOutPipe.mode := Usbdi.MaxPerformance; ELSE bulkOutPipe.mode := Usbdi.MinCpu; END;
+			status := bulkOutPipe.Transfer(31, 0, CBWbuffer);
+			IF (status = Usbdi.Disconnected) THEN
+				RETURN Base.ResDisconnected;
+			ELSIF status # Usbdi.Ok THEN (* sending the CBW failed -> Perform reset recovery [2], page 15 *)
+				RETURN Base.ResFatalError;
+			END;
+
+			(* If there is data to send, enter the data stage *)
+			IF bufferlen # 0 THEN
+				NEW(b, bufferlen);
+				FOR i := 0 TO bufferlen - 1 DO b[i] := buffer[ofs + i] END;
+				IF Debug.Trace & Debug.traceScTransfers THEN
+					KernelLog.String("UsbStorage: Data Phase: Transfering "); KernelLog.Int(bufferlen, 0); KernelLog.String(" Bytes to ");
+					IF dir = Base.DataIn THEN KernelLog.String("Host Controller"); ELSIF dir = Base.DataOut THEN  KernelLog.String("Device");	ELSE HALT(301); END;
+					KernelLog.Ln;
+				END;
+				IF dir = Base.DataIn THEN
+					IF Base.performance = Usbdi.MaxPerformance THEN bulkInPipe.mode := Usbdi.Normal; ELSE bulkInPipe.mode := Usbdi.MinCpu; END;
+					bulkInPipe.SetTimeout(timeout);
+					status := bulkInPipe.Transfer(bufferlen, 0(*ofs*), b);
+					tlen := bulkInPipe.GetActLen();
+				ELSIF dir = Base.DataOut THEN
+					bulkOutPipe.SetTimeout(timeout);
+					IF Base.performance = Usbdi.MaxPerformance THEN bulkOutPipe.mode := Usbdi.Normal; ELSE bulkOutPipe.mode := Usbdi.MinCpu; END;
+					status := bulkOutPipe.Transfer(bufferlen, 0(*ofs*), b);
+					tlen := bulkOutPipe.GetActLen();
+				ELSE HALT(303);
+				END;
+				FOR i := 0 TO tlen - 1 DO buffer[ofs + i] := b[i] END;
+
+				(* clear halt if STALL occured, but do not abort!!! *)
+				IF status = Usbdi.Stalled THEN
+					IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: Stall on BulkOnly data phase"); KernelLog.Ln; END;
+					(* only abort if clear halt fails *)
+					 IF ((dir = Base.DataIn) & (~bulkInPipe.ClearHalt())) OR ((dir = Base.DataOut) & (~bulkOutPipe.ClearHalt())) THEN
+						IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Failure on BulkOnly clear halt"); KernelLog.Ln; END;
+						RETURN Base.ResFatalError;
+					END;
+				ELSIF status = Usbdi.InProgress THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Timeout on BulkOnly data phase"); KernelLog.Ln; END;
+					RETURN Base.ResTimeout;
+				ELSIF status = Usbdi.Disconnected THEN
+					RETURN Base.ResDisconnected;
+				ELSIF status = Usbdi.Error THEN
+					(* allow short packets and stalls !!! *)
+					IF Debug.Level >= Debug.Errors  THEN KernelLog.String("UsbStorage: Failure on BulkOnly data phase"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				END;
+			ELSE
+				tlen := 0;
+			END;
+
+			(* enter the status phase - Get the CSW *)
+			IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorage: Getting BulkOnly CSW"); KernelLog.Ln; END;
+			IF Base.performance = Usbdi.MaxPerformance THEN bulkInPipe.mode := Usbdi.MaxPerformance;  ELSE bulkInPipe.mode := Usbdi.MinCpu; END;
+			bulkInPipe.SetTimeout(timeout);
+			status := bulkInPipe.Transfer(13, 0, CSWbuffer);
+
+			IF status = Usbdi.InProgress THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Timeout in CSW phase"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError
+			ELSIF status = Usbdi.Disconnected THEN
+				RETURN Base.ResDisconnected;
+			ELSIF status # Usbdi.Ok THEN
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: Could not get CSW, must retry CSW phase"); KernelLog.Ln; END;
+				IF (status = Usbdi.Stalled) & ~bulkInPipe.ClearHalt() THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Failure on BulkOnly clear halt"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				END;
+				(* Host shall retry to get CSW ([2], page 19) *)
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: Retrying BulkOnly CSW"); KernelLog.Ln; END;
+				status := bulkInPipe.Transfer(13, 0, CSWbuffer);
+				IF (status = Usbdi.Disconnected) THEN
+					RETURN Base.ResDisconnected;
+				ELSIF status # Usbdi.Ok THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: 2nd try to get CSW failed"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				END;
+			END;
+
+			IF Debug.Trace & Debug.traceCSWs THEN
+				KernelLog.String("Received CSW: "); FOR i := 0 TO 12 DO KernelLog.Hex(ORD(CSWbuffer[i]), -2); KernelLog.Char(" "); END; KernelLog.Ln;
+			END;
+
+			(* Check whether the CSW is valid. If it's not, perform a reset recovery ([2], page 18) *)
+			(* Validity: Check CSW signature *)
+			IF (CSWbuffer[0] # 55X) OR (CSWbuffer[1] # 53X) OR (CSWbuffer[2] # 42X) OR (CSWbuffer[3] # 53X) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Device did not send a valid CSW! (wrong signature)"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+			(* Validity: check the dCSWTag *)
+			IF (CSWbuffer[4] # CBWbuffer[4]) OR (CSWbuffer[5] # CBWbuffer[5]) OR (CSWbuffer[6] # CBWbuffer[6]) OR (CSWbuffer[7] # CBWbuffer[7]) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Device sent wrong tag in CSW"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+
+			(* Check whether the CSW is meaningful *)
+			residual := ORD(CSWbuffer[8]) + 10H*ORD(CSWbuffer[9]) + 100H*ORD(CSWbuffer[10]) + 1000H*ORD(CSWbuffer[11]);
+
+			(* Meaning: bCSWStatus *)
+			IF (CSWbuffer[12] = 0X) & (residual <= bufferlen) THEN (* CSW meaningful; Command Passed *)
+				IF residual # 0 THEN
+					tlen := bufferlen - residual;
+					RETURN Base.ResShortTransfer;
+				ELSE
+					RETURN Base.ResOk;
+				END;
+			ELSIF (CSWbuffer[12] = 1X) & (residual <= bufferlen)  THEN (* CSW meaningful; Command Error *)
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: CSW reports Error"); KernelLog.Ln; END;
+				RETURN Base.ResError;
+			ELSIF CSWbuffer[12] = 2X THEN (* Phase Error: Perform reset recovery [2], page 16*)
+				IF Debug.Trace & Debug.Trace  & Debug.traceCSWs THEN KernelLog.String("UsbStorage: CSW reports Phase Error"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			ELSE (* CSW not meaningful -> Perform reset recovery  *)
+				IF Debug.Trace THEN KernelLog.String("UsbStorage: CSW not meaningful"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+		END Transport;
+
+		PROCEDURE &Init*;
+		BEGIN
+			Init^; NEW(CBWbuffer, 31); NEW(CSWbuffer, 13); seqNbr := 0;
+		END Init;
+
+	END BulkOnlyTransport;
+
+END UsbStorageBot.

source/ARM.UsbStorageCbi.Mod

@@ -0,0 +1,217 @@
+MODULE UsbStorageCbi;  (** AUTHOR "cplattner/staubesv"; PURPOSE " CB/I transport layer of USB mass storage driver"; *)
+(**
+ * References:
+ *
+ *	- 	[1] Universal Serial Bus Mass Storage Class Control/Bulk/Interrupt (CBI) Transport, Revision 1.1, June 23, 2003
+ *		www.usb.org
+ *
+ * History:
+ *
+ *	09.02.2006	First release (staubesv)
+ *	05.07.2006	Adapted to Usbdi (staubesv)
+ *	07.08.2006	Cleanups, improved error case in transport, fixed transfer offset ignored  (staubesv)
+ *)
+
+IMPORT
+	KernelLog,
+	Base := UsbStorageBase, Usbdi, Debug := UsbDebug;
+
+CONST
+
+	(* Interrupt Data Block coding, 3.4.3.1.1 in [1] *)
+	Pass = 0;
+	Fail = 1;
+	PhaseError = 2;
+	PersistentFailure = 3;
+
+TYPE
+
+	(** USB Mass Storage Class Control/Bulk/Interrupt (CBI) and Control/Bulk (CB) transport layer *)
+	CBITransport* = OBJECT(Base.StorageDriver);
+
+		(**
+		 * The Accept Device-Specific Command class-specific request is uses by the CBI Command Transport
+		 * Protocol to send a command block from a host to a device.
+		 * @param cmdLen Length of the command
+		 * @param cmd Command
+		 * @param timeout in milliseconds
+		 * @return transport status of command block
+		 *)
+		PROCEDURE  AcceptCommand(cmd : Usbdi.Buffer; cmdlen, timeout : LONGINT) : Usbdi.Status;
+		BEGIN
+			ASSERT(LEN(cmd) >= cmdlen);
+			defaultPipe.SetTimeout(timeout);
+			RETURN device.Request(Usbdi.ToDevice + Usbdi.Class + Usbdi.Interface, 0, 0, interface.bInterfaceNumber, cmdlen, cmd);
+		END AcceptCommand;
+
+		PROCEDURE Reset*(timeout : LONGINT) : LONGINT;
+		VAR buffer, interruptData : Usbdi.BufferPtr; status : Usbdi.Status; i : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorageCbi: Sending CB/I reset ControlTransfer"); KernelLog.Ln; END;
+			NEW(buffer, 12);
+			buffer[0] := CHR(1DH);
+			buffer[1] := CHR(4);
+			FOR i := 2 TO 11 DO buffer[i] := CHR(255) END;
+			status := AcceptCommand(buffer, 12, timeout);
+			IF (status = Usbdi.Disconnected) THEN
+				RETURN Base.ResDisconnected;
+			ELSIF (status # Usbdi.Ok) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Failure on TransportCB/I-Reset Control"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+			IF transportMethod = Base.MethodCBI THEN
+				IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorageCbi: Sending CB/I reset InterruptTransfer"); KernelLog.Ln; END;
+				NEW(interruptData, 8);
+				interruptPipe.SetTimeout(timeout);
+				status := interruptPipe.Transfer(2, 0, interruptData);
+				IF status = Usbdi.Stalled THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Stall on TransportCB/I-Reset Interrupt"); KernelLog.Ln; END;
+					IF ~interruptPipe.ClearHalt() THEN RETURN Base.ResError END;
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Failure on TransportCB/I-Reset clear halt on Interruptpipe"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				ELSIF status = Usbdi.InProgress THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Timeout on TransportCB/I-Reset Interrupt"); KernelLog.Ln; END;
+					RETURN Base.ResTimeout;
+				ELSIF status = Usbdi.Disconnected THEN
+					RETURN Base.ResDisconnected;
+				ELSIF status # Usbdi.Ok THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Failure on TransportCB/I-Reset Interrupt"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				END;
+			END;
+			(* After a Command Block Reset, the Stall condition and data toggle of the device's endpoints are undefined (2.2 in [1]) *)
+			IF ~bulkInPipe.ClearHalt() OR ~bulkOutPipe.ClearHalt() THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Failure on CB/I reset ClearHalt"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+			IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorageCbi: CB/I reset OK"); KernelLog.Ln; END;
+			RETURN Base.ResOk;
+		END Reset;
+
+		PROCEDURE Transport*(cmd : ARRAY OF CHAR; cmdlen : LONGINT; dir : SET;
+			VAR buffer : ARRAY OF CHAR; ofs, bufferlen : LONGINT; VAR tlen : LONGINT; timeout : LONGINT) : LONGINT;
+		VAR status : Usbdi.Status;	 interruptData : Usbdi.BufferPtr; blockStatus : LONGINT;
+			b, c: Usbdi.BufferPtr; i: LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorageCbi: Sending TransportCB/I Control"); KernelLog.Ln; END;
+			NEW(c, cmdlen);
+			FOR i := 0 TO cmdlen - 1 DO c[i] := cmd[i] END;
+			NEW(b, bufferlen);
+			FOR i := 0 TO bufferlen - 1 DO b[i] := buffer[ofs + i] END;
+			status := AcceptCommand(c, cmdlen, timeout);
+			IF status = Usbdi.Stalled THEN
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorageCbi: Stall on TransportCB/I Control"); KernelLog.Ln; END;
+				RETURN Base.ResError; (* sense device *)
+			ELSIF status = Usbdi.InProgress THEN
+				IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorageCbi: Timeout on TransportCB/I Control"); KernelLog.Ln; END;
+				RETURN Base.ResTimeout;
+			ELSIF status = Usbdi.Disconnected THEN
+				RETURN Base.ResDisconnected;
+			ELSIF status # Usbdi.Ok THEN
+				IF Debug.Level >= Debug.Warnings THEN
+					KernelLog.String("UsbStorageCbi: Failure on TransportCB/I Control, status :"); KernelLog.Int(status, 0); KernelLog.Ln;
+				END;
+				RETURN Base.ResError; (* sense device *)
+			END;
+
+			IF (bufferlen # 0) THEN
+
+				IF dir = Base.DataIn THEN
+					IF Debug.Trace & Debug.traceScTransfers THEN
+						KernelLog.String("UsbStorageCbi: Get "); KernelLog.Int(bufferlen, 0); KernelLog.String(" bytes from device"); KernelLog.Ln;
+					END;
+					bulkInPipe.SetTimeout(timeout);
+					status := bulkInPipe.Transfer(bufferlen, ofs, b);
+					tlen := bulkInPipe.GetActLen();
+				ELSIF dir = Base.DataOut THEN
+					IF Debug.Trace & Debug.traceScTransfers THEN
+						KernelLog.String("UsbStorageCbi: Send "); KernelLog.Int(bufferlen, 0); KernelLog.String(" bytes to device"); KernelLog.Ln;
+					END;
+					bulkOutPipe.SetTimeout(timeout);
+					status := bulkOutPipe.Transfer(bufferlen, ofs, b);
+					tlen := bulkOutPipe.GetActLen();
+				ELSE HALT(303);
+				END;
+
+				IF status = Usbdi.Stalled THEN
+					IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorageCbi: Stall on TransportCB/I Bulk"); KernelLog.Ln; END;
+					IF ((dir = Base.DataIn) & ~bulkInPipe.ClearHalt()) OR ((dir = Base.DataOut) & ~bulkOutPipe.ClearHalt()) THEN
+						IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorage: Failure on TransportCB/I clear halt on Bulkpipe"); KernelLog.Ln; END;
+						RETURN Base.ResFatalError
+					END;
+					RETURN Base.ResError; (* sense device *)
+				ELSIF status = Usbdi.InProgress THEN
+					IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorageCbi: Timeout on TransportCB/I Bulk"); KernelLog.Ln; END;
+					RETURN Base.ResTimeout;
+				ELSIF status = Usbdi.Disconnected THEN
+					RETURN Base.ResDisconnected;
+				ELSIF status # Usbdi.Ok THEN
+					IF Debug.Level >= Debug.Warnings THEN KernelLog.String("UsbStorageCbi: Failure on TransportCB/I Bulk"); KernelLog.Ln; END;
+					RETURN Base.ResError; (* sense device *)
+				END;
+			ELSE
+				tlen := 0;
+			END;
+
+			IF transportMethod = Base.MethodCBI THEN
+				IF Debug.Trace & Debug.traceScRequests THEN KernelLog.String("UsbStorageCbi: Sending TransportCB/I Interrupt"); KernelLog.Ln; END;
+
+				NEW(interruptData, 2);
+				interruptPipe.SetTimeout(timeout);
+				status := interruptPipe.Transfer(2, 0, interruptData);
+
+				IF status = Usbdi.Stalled THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Stall on TransportCB/I Interrupt"); KernelLog.Ln; END;
+					IF interruptPipe.ClearHalt() THEN RETURN Base.ResError END;
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Failure on TransportCB/I clear halt on Interruptpipe"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError
+				ELSIF status = Usbdi.InProgress THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Timeout on TransportCB/I Interrupt"); KernelLog.Ln; END;
+					RETURN Base.ResTimeout;
+				ELSIF status = Usbdi.Disconnected THEN
+					RETURN Base.ResDisconnected;
+				ELSIF status # Usbdi.Ok THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: Failure on TransportCB/I Interrupt"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				END;
+
+				IF (transportProtocol = Base.ProtocolUFI) THEN
+					IF (cmd[0] = 12X) OR (cmd[0] = 03X) THEN
+						(* UFI Inquiry + Sense do not change the sense data, so we cannot be sure that those commands succeded!!! *)
+						(* just go on and hope the best! *)
+					ELSIF (interruptData[0] # 0X) THEN
+						IF Debug.Level >= Debug.Errors THEN
+							KernelLog.String("UsbStorageCbi: Error on CBI/UFI, asc = "); KernelLog.Hex(ORD(interruptData[0]), 0);
+							KernelLog.String(" ascq = "); KernelLog.Hex(ORD(interruptData[1]), 0); KernelLog.Ln;
+						END;
+						RETURN Base.ResSenseError; (* just retry *)
+					END;
+					(* go on *)
+				ELSIF interruptData[0] # 0X THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorageCbi: CBI returned invalid interupt data block"); KernelLog.Ln; END;
+					RETURN Base.ResSenseError; (* try to recover by manual sensing *)
+				ELSE
+					(* Command completion interrupt. Error handling according 3.4.3.1.1. in [1] *)
+					blockStatus := ORD(interruptData[1]) MOD 4;
+					CASE blockStatus OF
+						|Pass: (* command status ok *)
+						|Fail: RETURN Base.ResError;
+						|PhaseError: RETURN Base.ResFatalError; (* reset device *)
+						|PersistentFailure: RETURN Base.ResSenseError; (* request sense *)
+					ELSE
+						HALT(99);
+					END;
+				END;
+			END;
+			IF tlen # bufferlen THEN RETURN Base.ResShortTransfer; END;
+			RETURN Base.ResOk;
+		END Transport;
+
+		PROCEDURE &Init*;
+		BEGIN
+			Init^;
+		END Init;
+
+	END CBITransport;
+
+END UsbStorageCbi.

source/ARM.UsbStorageScm.Mod

@@ -0,0 +1,674 @@
+MODULE UsbStorageScm; (** AUTHOR "cplattner/staubesv"; PURPOSE "SCM transport layer of USB mass storage driver"; *)
+(**
+ * SCSI commands borrowed from SCSI.Mod.
+ *
+ * History:
+ *
+ *	09.02.2006	First release (staubesv)
+ *)
+
+IMPORT
+	SYSTEM, KernelLog, Kernel,
+	Usbdi, Base := UsbStorageBase, Debug := UsbDebug, UsbStorageCbi;
+
+CONST
+
+	(* Constans for the SCM USB-ATAPI Shuttle device *)
+	ScmATA = 40X; ScmISA = 50X;
+
+	(* Data registers *)
+	ScmUioEpad = 80X; ScmUioCdt = 40X; ScmUio1 = 20X; ScmUio0 = 10X;
+
+	(* User i/o enable registers *)
+	ScmUioDrvrst = 80X; ScmUioAckd = 40X; ScmUioOE1 = 20X; ScmUioOE0 = 10X;
+
+TYPE
+
+	(* SCM Shuttle Transport Layer  *)
+	SCMTransport* = OBJECT(UsbStorageCbi.CBITransport) (* same Reset procedure as CBITransport -> inherit it *)
+	VAR
+		(* these buffers will be re-used; they are created in &Init *)
+		command : Usbdi.BufferPtr;
+		buffer : Usbdi.BufferPtr;
+		timer : Kernel.Timer;
+
+		PROCEDURE ScmShortPack(p1, p2 : CHAR) : INTEGER;
+		BEGIN
+			RETURN SYSTEM.VAL(INTEGER, SYSTEM.VAL(SET, ORD(p2)*256) + SYSTEM.VAL(SET, ORD(p1)));
+		END ScmShortPack;
+
+		PROCEDURE ScmSendControl(dir : SET; req, reqtyp, value, index : LONGINT; VAR buffer : Usbdi.Buffer;
+			ofs, bufferlen, timeout : LONGINT) : LONGINT;
+		VAR status : Usbdi.Status; 	ignore : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceScTransfers THEN
+				KernelLog.String("UsbStorage: Sending SCM Control:"); KernelLog.String(" Direction: ");
+				IF dir = Base.DataIn THEN KernelLog.String("In");
+				ELSIF dir = Base.DataOut THEN KernelLog.String("Out");
+				ELSE KernelLog.String("Unknown");
+				END;
+				KernelLog.String(" Bufferlen: "); KernelLog.Int(bufferlen, 0); KernelLog.String(" Offset: "); KernelLog.Int(ofs, 0); KernelLog.Ln;
+			END;
+			IF (bufferlen > 0) & (bufferlen+ofs > LEN(buffer)) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmSendControl: Buffer underrun"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+
+			IF device.Request(SYSTEM.VAL(SET, reqtyp), req, value, index, bufferlen, buffer) # Usbdi.Ok THEN
+				status := defaultPipe.GetStatus(ignore);
+				IF status = Usbdi.Stalled THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Stall on Transport SCM Control"); KernelLog.Ln; END;
+					IF defaultPipe.ClearHalt() THEN
+						RETURN Base.ResError;
+					ELSE
+						IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Failure on Transport SCM clear halt on Controlpipe"); KernelLog.Ln; END;
+						RETURN Base.ResFatalError;
+					END;
+				END;
+				IF status = Usbdi.InProgress THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Timeout on Transport SCM Control"); KernelLog.Ln; END;
+					RETURN Base.ResTimeout;
+				ELSIF status = Usbdi.Disconnected THEN
+					RETURN Base.ResDisconnected;
+				ELSIF status # Usbdi.Ok THEN
+					IF Debug.Level >= Debug.Errors  THEN KernelLog.String("UsbStorage: Failure on Transport SCM Control"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				END;
+			END;
+			RETURN Base.ResOk;
+		END ScmSendControl;
+
+		PROCEDURE ScmBulkTransport(dir : SET; VAR buffer : Usbdi.Buffer; ofs, bufferlen : LONGINT;
+			VAR tlen : LONGINT; timeout : LONGINT) : LONGINT;
+		VAR status : Usbdi.Status;
+		BEGIN
+			IF Debug.Trace & Debug.traceScTransfers THEN
+				KernelLog.String("UsbStorage: Transfering SCM Data: Direction: ");
+				IF dir = Base.DataIn THEN KernelLog.String("IN");
+				ELSIF dir =  Base.DataOut THEN KernelLog.String("OUT");
+				ELSE KernelLog.String("Unknown");
+				END;
+				KernelLog.String(" Bufferlen: "); KernelLog.Int(bufferlen, 0); KernelLog.String(" Offset: "); KernelLog.Int(ofs, 0);
+				KernelLog.Ln;
+			END;
+			tlen := 0;
+			IF bufferlen = 0 THEN RETURN Base.ResOk END;
+
+			IF bufferlen + ofs > LEN(buffer) THEN
+				IF Debug.Level >= Debug.Errors THEN
+					KernelLog.String("UsbStorage: ScmBulkTransport: Buffer underrun");
+					KernelLog.String(" (buffer length: "); KernelLog.Int(LEN(buffer), 0); KernelLog.String(")"); KernelLog.Ln;
+				END;
+				RETURN Base.ResFatalError;
+			END;
+
+			IF dir = Base.DataIn THEN
+				bulkInPipe.SetTimeout(timeout);
+				status := bulkInPipe.Transfer(bufferlen, ofs, buffer);
+				tlen := bulkInPipe.GetActLen();
+			ELSIF dir = Base.DataOut THEN
+				bulkOutPipe.SetTimeout(timeout);
+				status := bulkOutPipe.Transfer(bufferlen, ofs, buffer);
+				tlen := bulkOutPipe.GetActLen();
+			ELSE
+				HALT(301);
+			END;
+
+			(* clear halt if STALL occured, but do not abort!!! *)
+			IF status = Usbdi.Stalled THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Stall on SCM data phase"); KernelLog.Ln; END;
+				(* only abort if clear halt fails *)
+				IF ((dir=Base.DataIn) & ~bulkInPipe.ClearHalt()) OR ((dir=Base.DataOut) & ~bulkOutPipe.ClearHalt())  THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Failure on SCM bulk clear halt"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				END;
+			END;
+
+			IF status = Usbdi.InProgress THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Timeout on SCM data phase"); KernelLog.Ln; END;
+				RETURN Base.ResTimeout;
+			ELSIF status = Usbdi.Disconnected THEN
+				RETURN Base.ResDisconnected;
+			ELSIF status = Usbdi.Error THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Failure on SCM bulk"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+
+			IF tlen # bufferlen THEN
+				IF Debug.Level >= Debug.Errors THEN
+					KernelLog.String("UsbStorage: ScmBulkTransport short read: ");
+					KernelLog.Int(bufferlen, 0); KernelLog.Char("/"); KernelLog.Int(tlen, 0); KernelLog.Ln;
+				END;
+				RETURN Base.ResShortTransfer;
+			END;
+			RETURN Base.ResOk;
+			END ScmBulkTransport;
+
+		PROCEDURE ScmWaitNotBusy(timeout : LONGINT) : LONGINT;
+		VAR status : CHAR; res : LONGINT;
+		BEGIN
+			LOOP
+				res := ScmRead(ScmATA, 17X, status, 1000);
+				IF res # Base.ResOk THEN
+					IF (res = Base.ResDisconnected) OR (res = Base.ResFatalError) THEN RETURN res ELSE RETURN Base.ResError END;
+				ELSIF (SYSTEM.VAL(SET, status) * {0}) # {} THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmWaitNotBusy: check condition"); KernelLog.Ln; END;
+					RETURN Base.ResError;
+				ELSIF (SYSTEM.VAL(SET, status) * {5}) # {} THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmWaitNotBusy: device fault"); KernelLog.Ln; END;
+					RETURN Base.ResFatalError;
+				ELSIF (SYSTEM.VAL(SET, status) * {7}) = {} THEN
+					IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorage: ScmWaitNotBusy: good"); KernelLog.Ln; END;
+					RETURN Base.ResOk;
+				END;
+				IF timeout # -1 THEN
+					timeout  := timeout - 10; IF timeout < 0 THEN EXIT END;
+				END;
+				Wait(10);
+			END;
+			IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmWaitNotBusy: Timeout"); KernelLog.Ln; END;
+			RETURN Base.ResTimeout;
+		END ScmWaitNotBusy;
+
+		PROCEDURE ScmReadUserIO(VAR dataflags: CHAR; timeout : LONGINT) : LONGINT;
+		VAR res : LONGINT;
+		BEGIN
+			res := ScmSendControl(Base.DataIn, 82H, 0C0H, 0, 0, buffer, 0, 1, timeout);
+			dataflags := buffer[0];
+			RETURN res;
+		END ScmReadUserIO;
+
+		PROCEDURE ScmWriteUserIO(enableflags, dataflags: CHAR; timeout : LONGINT) : LONGINT;
+		BEGIN
+			RETURN ScmSendControl(Base.DataOut, 82H, 40H, ScmShortPack(enableflags, dataflags), 0, Usbdi.NoData, 0, 0, timeout);
+		END ScmWriteUserIO;
+
+		PROCEDURE ScmRead(access, reg : CHAR; VAR content: CHAR; timeout : LONGINT) : LONGINT;
+		VAR res : LONGINT;
+		BEGIN
+			res := ScmSendControl(Base.DataIn, ORD(access), 0C0H, ORD(reg), 0, buffer, 0, 1, timeout);
+			content := buffer[0];
+			RETURN res;
+		END ScmRead;
+
+		PROCEDURE ScmWrite(access, reg, content : CHAR; timeout : LONGINT) : LONGINT;
+		BEGIN
+			access := SYSTEM.VAL(CHAR, SYSTEM.VAL(SET, access) + {0});
+			RETURN ScmSendControl(Base.DataOut, ORD(access), 040H, ScmShortPack(reg, content), 0, Usbdi.NoData, 0, 0, timeout);
+		END ScmWrite;
+
+		PROCEDURE ScmMultipleWrite(access : CHAR; VAR registers, dataout: ARRAY OF CHAR; numregs,  timeout : LONGINT) : LONGINT;
+		VAR res, i, tlen : LONGINT;
+		BEGIN
+			IF numregs > 7 THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmMultipleWrite: Numregs > 7."); KernelLog.Ln;END;
+				RETURN Base.ResFatalError;
+			END;
+			command[0] := 40X;
+			command[1] := SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, access)+ {0,1,2}));
+			command[2] := 0X;
+			command[3] := 0X;
+			command[4] := 0X;
+			command[5] := 0X;
+			command[6] := CHR(numregs*2);
+			command[7] := CHR(LSH(numregs*2, -8));
+
+			FOR i:= 0 TO numregs - 1 DO
+				buffer[i*2] := registers[i];
+				buffer[(i*2)+1] := dataout[i];
+			END;
+
+			res := ScmSendControl(Base.DataOut, 80H, 040H, 0, 0, command, 0, 8, timeout);
+			IF res # Base.ResOk THEN RETURN res END;
+
+			res := ScmBulkTransport(Base.DataOut, buffer, 0, numregs*2, tlen, timeout);
+			IF res # Base.ResOk THEN RETURN res END;
+
+			RETURN ScmWaitNotBusy(timeout);
+		END ScmMultipleWrite;
+
+		PROCEDURE ScmReadBlock(access, reg : CHAR; VAR content : Usbdi.Buffer; ofs, len: LONGINT; VAR tlen : LONGINT; timeout : LONGINT): LONGINT;
+		VAR res : LONGINT;
+		BEGIN
+			command[0] := 0C0X;
+			command[1] := SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, access)+ {1}));
+			command[2] := reg;
+			command[3] := 0X;
+			command[4] := 0X;
+			command[5] := 0X;
+			command[6] := CHR(len);
+			command[7] := CHR(LSH(len, -8));
+
+			tlen := 0;
+			res := ScmSendControl(Base.DataOut, 80H, 40H, 0, 0, command, 0, 8, timeout);
+			IF res # Base.ResOk THEN RETURN res END;
+			res := ScmBulkTransport(Base.DataIn, content, ofs, len, tlen, timeout);
+			RETURN res;
+		END ScmReadBlock;
+
+		PROCEDURE ScmWriteBlock(access, reg : CHAR; VAR content : Usbdi.Buffer; ofs, len : LONGINT; VAR tlen : LONGINT; timeout : LONGINT): LONGINT;
+		VAR res : LONGINT;
+		BEGIN
+			command[0] := 40X;
+			command[1] := SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, access)+ {0,1}));
+			command[2] := reg;
+			command[3] := 0X;
+			command[4] := 0X;
+			command[5] := 0X;
+			command[6] := CHR(len);
+			command[7] := CHR(LSH(len, -8));
+
+			tlen := 0;
+			res := ScmSendControl(Base.DataOut, 80H, 40H, 0, 0, command, 0, 8, timeout);
+			IF res # Base.ResOk THEN RETURN res END;
+
+			res := ScmBulkTransport(Base.DataOut, content, ofs, len, tlen, timeout);
+			IF res # Base.ResOk THEN RETURN res END;
+			RETURN ScmWaitNotBusy(timeout);
+		END ScmWriteBlock;
+
+		PROCEDURE ScmRWBlockTest(access : CHAR; VAR registers, dataout : ARRAY OF CHAR;
+			numregs :INTEGER; datareg, statusreg, atapitimeout, qualifier : CHAR; dir : SET; VAR content : Usbdi.Buffer;
+			ofs, contentlen: LONGINT; VAR tlen : LONGINT; timeout : LONGINT): LONGINT;
+		VAR
+			tmpreg : CHAR;
+			status : CHAR;
+			i, msgindex, msglen : INTEGER;
+			tmplen : LONGINT;
+			res : LONGINT;
+		BEGIN
+			IF numregs > 19 THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmRWBlockTest too many registers"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+			ASSERT(LEN(command)>=16);
+			command[0] := 40X;
+			command[1] := SYSTEM.VAL(CHAR, SYSTEM.VAL(SET,access) + {0,1,2});
+			command[2] := 7X;
+			command[3] := 17X;
+			command[4] := 0FCX;
+			command[5] := 0E7X;
+			command[6] := CHR(numregs*2);
+			command[7] := CHR(LSH(numregs*2, -8));
+			IF dir = Base.DataOut THEN
+				command[8] := 40X;
+				command[9] := SYSTEM.VAL(CHAR, SYSTEM.VAL(SET, access) + {0,2});
+			ELSIF dir = Base.DataIn THEN
+				command[8] := 0C0X;
+				command[9] := SYSTEM.VAL(CHAR, SYSTEM.VAL(SET, access) + {2});
+			ELSE
+				HALT(303)
+			END;
+			command[10] := datareg;
+			command[11] := statusreg;
+			command[12] := atapitimeout;
+			command[13] := qualifier;
+			command[14] := CHR(contentlen);
+			command[15] := CHR(LSH(contentlen, -8));
+
+			FOR i:=0 TO numregs -1 DO
+				buffer[i*2] := registers[i]; buffer[(i*2)+1] := dataout[i];
+			END;
+
+			tlen := 0;
+
+			FOR i := 0 TO 19 DO
+
+				IF i = 0 THEN msgindex := 0; msglen := 16 ELSE msgindex := 8; msglen := 8 END;
+
+				res := ScmSendControl(Base.DataOut, 80H, 40H, 0, 0, command, msgindex, msglen, 1000);
+				IF res # Base.ResOk THEN
+					IF (res = Base.ResFatalError) OR (res = Base.ResTimeout) OR (res = Base.ResDisconnected) THEN RETURN res ELSE RETURN Base.ResError END;
+				END;
+
+				IF i = 0 THEN
+					res := ScmBulkTransport(Base.DataOut, buffer, 0, numregs*2, tmplen, 1000);
+					IF res # Base.ResOk THEN
+						IF (res = Base.ResFatalError) OR (res = Base.ResTimeout) OR (res = Base.ResDisconnected)  THEN RETURN res ELSE RETURN Base.ResError END;
+					END;
+				END;
+
+				res := ScmBulkTransport(dir, content, 0, contentlen, tlen, timeout);
+				IF res = Base.ResShortTransfer THEN
+					IF (dir = Base.DataIn) & (i=0) THEN (* hm. makes somehow no sense, but that's life *)
+						IF ~bulkOutPipe.ClearHalt() THEN
+							IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmRWBlockTest clear halt failed"); KernelLog.Ln; END;
+						END
+					END;
+
+				IF dir = Base.DataOut THEN tmpreg := 17X; ELSE tmpreg := 0EX; END;
+
+				res := ScmRead(ScmATA, tmpreg, status, 1000);
+				IF res # Base.ResOk THEN
+					IF (res = Base.ResFatalError) OR (res = Base.ResDisconnected) OR (res = Base.ResTimeout)  THEN RETURN res ELSE RETURN Base.ResError END;
+					ELSIF (SYSTEM.VAL(SET, status) * {0}) # {} THEN
+						IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmRWBlockTest: check condition"); KernelLog.Ln; END;
+						RETURN Base.ResError;
+					ELSIF (SYSTEM.VAL(SET, status) * {5}) # {} THEN
+						IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmRWBlockTest: device fault"); KernelLog.Ln; END;
+						RETURN Base.ResFatalError;
+					END;
+				ELSE
+					RETURN ScmWaitNotBusy(timeout);
+				END;
+			END;
+			IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmRWBlockTest failed 20 times!"); KernelLog.Ln; END;
+			RETURN Base.ResError;
+		END ScmRWBlockTest;
+
+		PROCEDURE ScmSelectAndTestRegisters() : BOOLEAN;
+		VAR selector : INTEGER; status : CHAR;
+		BEGIN
+			FOR selector := 0A0H TO 0B0H BY 10H DO (* actually, test 0A0H and 0B0H *)
+				IF ScmWrite(ScmATA, 16X, CHR(selector), 1000) # Base.ResOk THEN RETURN FALSE END;
+				IF ScmRead(ScmATA, 17X, status, 1000) # Base.ResOk THEN RETURN FALSE END;
+				IF ScmRead(ScmATA, 16X, status, 1000) # Base.ResOk THEN RETURN FALSE END;
+				IF ScmRead(ScmATA, 14X, status, 1000) # Base.ResOk THEN RETURN FALSE END;
+				IF ScmRead(ScmATA, 15X, status, 1000) # Base.ResOk THEN RETURN FALSE END;
+				IF ScmWrite(ScmATA, 14X, 55X, 1000) # Base.ResOk THEN RETURN FALSE END;
+				IF ScmWrite(ScmATA, 15X, 0AAX, 1000) # Base.ResOk THEN RETURN FALSE END;
+				IF ScmRead(ScmATA, 14X, status, 1000) # Base.ResOk THEN RETURN FALSE END;
+				IF ScmRead(ScmATA, 15X, status, 1000) # Base.ResOk THEN RETURN FALSE END;
+			END;
+			RETURN TRUE;
+		END ScmSelectAndTestRegisters;
+
+		PROCEDURE ScmSetShuttleFeatures(externaltrigger, eppcontrol, maskbyte, testpattern, subcountH, subcountL : CHAR) : BOOLEAN;
+		BEGIN
+			command[0] := 40X;
+			command[1] := 81X;
+			command[2] := eppcontrol;
+			command[3] := externaltrigger;
+			command[4] := testpattern;
+			command[5] := maskbyte;
+			command[6] := subcountL;
+			command[7] := subcountH;
+			IF ScmSendControl(Base.DataOut, 80H, 40H, 0, 0, command, 0, 8, 1000) # Base.ResOk THEN
+				RETURN FALSE;
+			ELSE
+				RETURN TRUE;
+			END;
+		END ScmSetShuttleFeatures;
+
+		PROCEDURE Initialization*() : BOOLEAN;
+		VAR status : CHAR; res : LONGINT;
+		BEGIN
+			IF Debug.Trace & Debug.traceScInit THEN KernelLog.String("UsbStorage: Initializing SCM USB-ATAPI Shuttle... "); KernelLog.Ln; END;
+			res := ScmWriteUserIO(SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, ScmUioOE0) + SYSTEM.VAL(SET, ScmUioOE1))),
+				SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, ScmUioEpad) + SYSTEM.VAL(SET, ScmUio1))), 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 1"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			Wait(2000);
+
+			res := ScmReadUserIO(status, 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 2"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			res := ScmReadUserIO(status, 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 3"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			res := ScmWriteUserIO(SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, ScmUioDrvrst) + SYSTEM.VAL(SET, ScmUioOE0)
+				+ SYSTEM.VAL(SET, ScmUioOE1))), SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, ScmUioEpad)
+				+ SYSTEM.VAL(SET, ScmUio1))), 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 4"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			res := ScmWriteUserIO(SYSTEM.VAL(CHAR, SYSTEM.VAL(SET, ScmUioOE0) + SYSTEM.VAL(SET, ScmUioOE1)),
+				 SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, ScmUioEpad) + SYSTEM.VAL(SET, ScmUio1))), 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 5"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			Wait(250);
+
+			res := ScmWrite(ScmISA, 03FX, 080X, 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 6"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			res := ScmRead(ScmISA, 027X, status, 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 7"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			res := ScmReadUserIO(status, 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 8"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			IF ~ScmSelectAndTestRegisters() THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 9"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			res := ScmReadUserIO(status, 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 10"); KernelLog.Ln; END;
+				 RETURN FALSE;
+			END;
+
+			res := ScmWriteUserIO(SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, ScmUioAckd) + SYSTEM.VAL(SET, ScmUioOE0)
+				+ SYSTEM.VAL(SET, ScmUioOE1))), SYSTEM.VAL(CHAR, (SYSTEM.VAL(SET, ScmUioEpad)
+				+ SYSTEM.VAL(SET, ScmUio1))), 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 11"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			res := ScmReadUserIO(status, 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 12"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+
+			Wait(1400);
+
+			res := ScmReadUserIO(status, 1000);
+			IF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 13"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			IF ~ScmSelectAndTestRegisters() THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 14"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			IF ~ScmSetShuttleFeatures(83X, 0X, 88X, 08X, 15X, 14X) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM Init error, step 15"); KernelLog.Ln; END;
+				RETURN FALSE;
+			END;
+			IF  Debug.Trace & Debug.traceScInit THEN KernelLog.String("UsbStorage: Initialization done."); KernelLog.Ln; END;
+			RETURN TRUE;
+		END Initialization;
+
+		PROCEDURE Transport*(cmd : ARRAY OF CHAR; cmdlen : LONGINT; dir : SET;
+			VAR buffer : ARRAY OF CHAR; ofs, bufferlen : LONGINT; VAR tlen : LONGINT; timeout : LONGINT) : LONGINT;
+		VAR
+			registers, data : ARRAY 32 OF CHAR;
+			i, res : LONGINT;
+			status : CHAR;
+			atapilen, tmplen, sector, transfered : LONGINT;
+			j : LONGINT;
+			c, b: Usbdi.Buffer;
+		BEGIN
+			IF Debug.Trace & Debug.traceScTransfers THEN
+				KernelLog.String("UsbStorage: Transport:");
+				KernelLog.String(" Direction: ");
+				IF dir = Base.DataIn THEN KernelLog.String("In");
+				ELSIF dir = Base.DataOut THEN KernelLog.String("Out");
+				ELSE KernelLog.String("Unknown");
+				END;
+				KernelLog.String(" Bufferlen: "); KernelLog.Int(bufferlen, 0); KernelLog.String(" Offset: "); KernelLog.Int(ofs, 0);
+				KernelLog.String(" Cmd: ");
+				IF cmdlen = 0 THEN KernelLog.String("None");
+				ELSE
+					FOR j := 0 TO cmdlen-1 DO KernelLog.Int(ORD(cmd[j]), 0); KernelLog.Char(" "); END;
+				END;
+				KernelLog.Ln;
+			END;
+			registers[0] := 11X;
+			registers[1] := 12X;
+			registers[2] := 13X;
+			registers[3] := 14X;
+			registers[4] := 15X;
+			registers[5] := 16X;
+			registers[6] := 17X;
+
+			data[0] := 0X;
+			data[1] := 0X;
+			data[2] := 0X;
+			data[3] := CHR(bufferlen);
+			data[4] := CHR(LSH(bufferlen, -8));
+			data[5] := 0B0X;
+			data[6] := 0A0X;
+
+			FOR i:= 7 TO 18 DO
+				registers[i] := 010X;
+				IF (i - 7) >= cmdlen THEN data[i] := 0X; ELSE data[i] := cmd[i-7]; END;
+			END;
+
+			tlen := 0;
+
+			NEW(b, bufferlen);
+			FOR i := 0 TO bufferlen - 1 DO b[i] := buffer[ofs + i] END;
+
+			NEW(c, cmdlen);
+			FOR i := 0 TO cmdlen - 1 DO c[i] := cmd[i] END;
+
+			IF dir = Base.DataOut THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM DataOut not supported!!!"); KernelLog.Ln; END;
+				RETURN Base.ResUnsupported;
+			END;
+
+			IF bufferlen > 65535 THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Too large request for SCM USB-ATAPI Shuttle"); KernelLog.Ln;	END;
+				RETURN Base.ResUnsupported;
+			END;
+
+			IF cmd[0] = CHR(Base.UfiRead10) THEN
+				IF bufferlen < 10000H THEN
+					IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorage: Doing SCM single read"); KernelLog.Ln; END;
+					res := ScmRWBlockTest(ScmATA, registers, data, 19, 10X, 17X, 0FDX, 30X, Base.DataIn, b, ofs, bufferlen, tlen, timeout);
+					RETURN res;
+				ELSE
+					IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorage: Doing SCM multi read"); KernelLog.Ln; END;
+					tmplen := (65535 DIV sdevs.blockSize) * sdevs.blockSize;
+					sector := LSH(ScmShortPack(data[10], data[9]), 16) + ScmShortPack(data[12], data[11]);
+					transfered := 0;
+					WHILE transfered # bufferlen DO
+						IF tmplen > (bufferlen - transfered) THEN tmplen := bufferlen - transfered END;
+						data[3] := CHR(tmplen);
+						data[4] := CHR(LSH(tmplen, -8));
+						data[9] := CHR(LSH(sector, -24));
+						data[10] := CHR(LSH(sector, -16));
+						data[11] := CHR(LSH(sector, -8));
+						data[12] := CHR(sector);
+						data[14] := CHR(LSH(tmplen DIV sdevs.blockSize, -8));
+						data[15] := CHR(tmplen DIV sdevs.blockSize);
+						res := ScmRWBlockTest(ScmATA, registers, data, 19, 10X, 17X, 0FDX, 30X, Base.DataIn, b, ofs+transfered, tmplen, atapilen, timeout);
+						transfered := transfered + atapilen; tlen := transfered;
+						sector := sector + (tmplen DIV sdevs.blockSize);
+						IF res # Base.ResOk THEN RETURN res END;
+					END;
+					RETURN Base.ResOk;
+				END;
+			END;
+
+			IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorage: Sending SCM registers"); KernelLog.Ln; END;
+			res := ScmMultipleWrite(ScmATA, registers, data, 7, 1000);
+			IF (res = Base.ResDisconnected) THEN
+				RETURN res;
+			ELSIF (res # Base.ResOk) THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM register setup failed"); KernelLog.Ln; END;
+				RETURN Base.ResError
+			END;
+
+			IF cmdlen # 12 THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM command len # 12"); KernelLog.Ln; END;
+				RETURN Base.ResFatalError;
+			END;
+
+			IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorage: Sending SCM command"); KernelLog.Ln; END;
+
+			res := ScmWriteBlock(ScmATA, 10X, c, 0, 12, tmplen, timeout);
+			IF (res = Base.ResDisconnected) THEN
+				RETURN res;
+			ELSIF res # Base.ResOk THEN
+				IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: SCM command transfer failed"); KernelLog.Ln; END;
+				RETURN Base.ResError
+			END;
+
+			IF (bufferlen # 0) & (dir = Base.DataIn) THEN
+				IF Debug.Trace & Debug.traceScTransfers THEN KernelLog.String("UsbStorage: SCM  data transfer"); KernelLog.Ln; END;
+				res := ScmRead(ScmATA, 014X, status, 1000);
+				IF (res = Base.ResDisconnected) THEN
+					RETURN res;
+				ELSIF res # Base.ResOk THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmRead failed"); KernelLog.Ln; END;
+					RETURN Base.ResError
+				END;
+				atapilen := ORD(status);
+				IF bufferlen > 255 THEN
+					res := ScmRead(ScmATA, 015X, status, 1000);
+					IF (res = Base.ResDisconnected) THEN
+						RETURN res;
+					ELSIF res # Base.ResOk THEN
+						IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmRead2 failed"); KernelLog.Ln; END;
+						RETURN res;
+					END;
+					atapilen := atapilen + (ORD(status) * 256);
+				END;
+				IF Debug.Trace & Debug.traceScTransfers THEN
+					KernelLog.String("UsbStorage: Scm Transfer: Want: "); KernelLog.Int(bufferlen, 0);
+					KernelLog.String(" / have: "); KernelLog.Int(atapilen, 0); KernelLog.Ln;
+				END;
+				tmplen := atapilen;
+				IF atapilen < bufferlen THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Scm has FEWER bytes in the atapi buffer"); KernelLog.Ln; END;
+				ELSIF atapilen > bufferlen THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: Scm has MORE bytes in the atapi buffer"); KernelLog.Ln; END;
+					tmplen := bufferlen;
+				END;
+
+				res := ScmReadBlock(ScmATA, 10X, b, ofs, tmplen, tlen, timeout);
+				IF Debug.Trace & Debug.traceScTransfers THEN
+					IF (res = Base.ResOk) OR (res = Base.ResShortTransfer) THEN
+						KernelLog.String("UsbStorage: wanted: "); KernelLog.Int(tmplen, 0);
+						KernelLog.String(" / got: "); KernelLog.Int(tlen, 0); KernelLog.Ln;
+					END;
+				END;
+				IF (res = Base.ResOk) & (atapilen < bufferlen) THEN res := Base.ResShortTransfer END;
+				IF (res # Base.ResOk) & (res # Base.ResShortTransfer) THEN
+					IF Debug.Level >= Debug.Errors THEN KernelLog.String("UsbStorage: ScmReadBlock failed"); KernelLog.Ln; END;
+					RETURN res;
+				END;
+			ELSE
+				tlen := 0;
+			END;
+
+			RETURN Base.ResOk;
+		END Transport;
+
+		PROCEDURE Wait(ms : LONGINT);
+		BEGIN
+			timer.Sleep(ms);
+		END Wait;
+
+		PROCEDURE &Init*;
+		BEGIN
+			Init^; NEW(command, 16); NEW(buffer, 64); NEW(timer);
+		END Init;
+
+	END SCMTransport;
+
+END UsbStorageScm.

source/ARM.UsbVarTdAlloc.Mod

@@ -0,0 +1,296 @@
+MODULE UsbVarTdAlloc; (** AUTHOR "Timothée Martiel"; PURPOSE "Variable-size data-structure allocator for EHCI."; *)
+
+IMPORT Machine;
+
+CONST
+	AllocSize = 1024 * 1024;
+	AllocAlign = 1024 * 1024;
+
+TYPE
+	(**
+		Buffer descriptor
+	 *)
+	TdBuffer = OBJECT
+	VAR
+		buffer: POINTER TO ARRAY OF CHAR;
+		used: POINTER TO ARRAY OF SET;
+		ofs: LONGINT;
+		next: TdBuffer;
+
+		PROCEDURE & Init (block: LONGINT);
+		VAR
+			bitmaskSize: LONGINT;
+		BEGIN
+			NEW(buffer, AllocSize + AllocAlign);
+			ofs := AllocAlign - ADDRESSOF(buffer[0]) MOD AllocAlign;
+			bitmaskSize := AllocSize DIV (block * SetSize);
+			NEW(used, bitmaskSize)
+		END Init;
+
+		PROCEDURE Used(block: LONGINT): BOOLEAN;
+		BEGIN
+			RETURN (block MOD SetSize) IN used[block DIV SetSize];
+		END Used;
+
+		PROCEDURE SetUsed(block: LONGINT);
+		BEGIN
+			INCL(used[block DIV SetSize], block MOD SetSize);
+		END SetUsed;
+		
+		(* faster version of SetUsed for blocks
+		PROCEDURE SetUsedR(from, to: LONGINT);
+		VAR startSet, stopSet, startBit, stopBit: LONGINT;
+		BEGIN
+			IF to < from THEN RETURN END;
+			startBit := from MOD SetSize;
+			stopBit := to MOD SetSize;
+			startSet := from DIV SetSize;
+			stopSet := to DIV SetSize;
+			IF startSet < stopSet THEN
+				used[startSet] := used[startSet] + {startBit .. MAX(SET)};
+				INC(startSet);
+				WHILE startSet < stopSet DO
+					used[startSet] := {MIN(SET)..MAX(SET)};
+					INC(startSet); 
+				END;
+				used[stopSet] := used[stopSet] + {MIN(SET) .. stopBit};
+			ELSE
+				used[stopSet] := used[stopSet] + {startBit .. stopBit};
+			END;
+		END SetUsedR;
+		*)
+				
+		PROCEDURE SetFree(block: LONGINT);
+		BEGIN
+			EXCL(used[block DIV SetSize], block MOD SetSize);
+		END SetFree;
+
+		(* faster version of SetFree for blocks
+		PROCEDURE SetFreeR(from, to: LONGINT);
+		VAR startSet, stopSet, startBit, stopBit: LONGINT;
+		BEGIN
+			IF to < from THEN RETURN END;
+			startBit := from MOD SetSize;
+			stopBit := to MOD SetSize;
+			startSet := from DIV SetSize;
+			stopSet := to DIV SetSize;
+			IF startSet < stopSet THEN
+				used[startSet] := used[startSet] - {startBit .. MAX(SET)};
+				INC(startSet);
+				WHILE startSet < stopSet DO
+					used[startSet] := {};
+					INC(startSet); 
+				END;
+				used[stopSet] := used[stopSet] - {MIN(SET) .. stopBit};
+			ELSE
+				used[stopSet] := used[stopSet] - {startBit .. stopBit};
+			END;
+		END SetFreeR;
+		*)
+		
+
+	END TdBuffer;
+
+	(**
+		Allocator.
+		
+		The allocator is created with a page size and a block size. It can then allocate memory blocks with a granularity of the block size.
+		Each allocated block is guaranteed not to cross a page boundary.
+		
+		Allocated blocks must be freed manually.
+	*)
+	CONST SetSize = SIZEOF(SET) * 8;
+
+	TYPE Allocator * = OBJECT
+	VAR
+		tdBuffers: TdBuffer;
+		pageSize, blockSize, bitmaskSize: LONGINT;
+
+		PROCEDURE & Setup * (pageSize, blockSize: LONGINT);
+		BEGIN
+			ASSERT(SetSize = 32);
+			ASSERT((AllocSize MOD blockSize) MOD SetSize = 0);
+			SELF.pageSize := pageSize;
+			SELF.blockSize := blockSize;
+			bitmaskSize := AllocSize DIV (blockSize * SetSize)
+		END Setup;
+
+		(** Allocate memory for a TD or a QH of the given size. The size must be a multiple of 32. *)
+		PROCEDURE Allocate * (size: SIZE): ADDRESS;
+		VAR
+			buf: TdBuffer;
+			start, pos, count: LONGINT;
+			adr: ADDRESS;
+			
+			(** Allocate a new TD buffer and mark as used the last 32-byte block before a 4kB page boundary. *)
+			PROCEDURE AllocateBuffer;
+			VAR
+				buf: TdBuffer;
+				count, mod: LONGINT;
+			BEGIN
+				(* No buffer found: allocate a new one *)
+				NEW(buf, blockSize);
+				(*NEW(buf.buffer, AllocSize + AllocAlign);
+				NEW(buf.used, bitmaskSize);*)
+
+				buf.next := tdBuffers;
+				tdBuffers := buf;
+				count := 0;
+				mod := ADDRESSOF(buf.buffer[0]) MOD AllocAlign;
+				IF mod # 0 THEN
+					buf.ofs := AllocAlign - mod
+				END;
+
+				Machine.DisableDCacheRange(ADDRESSOF(buf.buffer[buf.ofs]), AllocSize);
+
+				(* Remove last 32-byte block before a 4kB page boundary from free blocks *)
+				LOOP
+					IF count >= AllocSize DIV blockSize THEN EXIT END;
+					IF (ADDRESSOF(buf.buffer[buf.ofs + count * blockSize]) MOD pageSize) = pageSize - blockSize THEN
+						buf.SetUsed(count); 
+					END;
+					INC(count)
+				END;
+			END AllocateBuffer;
+
+			
+		BEGIN {EXCLUSIVE}
+			ASSERT(size MOD blockSize = 0);
+			size := size DIV blockSize;
+			
+			buf := tdBuffers;
+			LOOP
+				IF buf = NIL THEN
+					AllocateBuffer;
+					buf := tdBuffers;
+					ASSERT(buf # NIL)
+				END;
+				count := 0;
+				pos := 0;
+				start := pos;  
+				WHILE (count < size) & (pos < bitmaskSize) DO
+					IF buf.Used(pos) THEN
+						count := 0; 
+						start := pos + 1;
+					ELSE
+						INC(count);
+					END;
+					INC(pos); 
+				END; 
+
+				IF count = size THEN EXIT END;
+				buf := buf.next
+			END;
+			
+			ASSERT(buf # NIL);
+
+			adr := ADDRESSOF(buf.buffer[buf.ofs + start*blockSize]);
+			
+			(* faster version:
+			buf.SetUsedR(start, start+count-1);
+			*)
+			WHILE(count > 0) DO
+				ASSERT(~buf.Used(start)); 
+				buf.SetUsed(start); 
+				INC(start); DEC(count); 
+			END;
+			Machine.Fill32(adr, size * blockSize, 0);
+			RETURN adr; 
+		END Allocate;
+
+		(** Marks a TD as free, so that its memory can be used again *)
+		PROCEDURE Free * (td: ADDRESS; size: SIZE);
+		VAR
+			buf: TdBuffer;
+			adr: ADDRESS;
+			slot: LONGINT;
+		BEGIN {EXCLUSIVE}
+			ASSERT(size MOD blockSize = 0);
+			size := size DIV blockSize; 
+
+			buf := tdBuffers;
+			LOOP
+				IF buf = NIL THEN EXIT END;
+				adr := ADDRESSOF(buf.buffer[buf.ofs]);
+				IF (adr <= td) & (td < adr + AllocSize) THEN EXIT END;
+				buf := buf.next
+			END;
+			ASSERT(buf # NIL); (* Not a TD *)
+
+			slot := (td - adr) DIV blockSize;
+			(* faster version:
+			buf.SetFreeR(slot, slot+size-1);
+			*)
+			WHILE (size > 0) DO 
+				ASSERT(buf.Used(slot));
+				buf.SetFree(slot);
+				INC(slot); DEC(size); 
+			END;
+		END Free;
+	END Allocator;
+
+
+	StaticAllocator * = OBJECT
+	VAR
+		buffers: TdBuffer;
+		size: LONGINT;
+
+		PROCEDURE & Setup * (allocSize: LONGINT);
+		BEGIN
+			size := allocSize
+		END Setup;
+
+		PROCEDURE Allocate * (): ADDRESS;
+		BEGIN {EXCLUSIVE}
+		END Allocate;
+
+		PROCEDURE Free * (td: ADDRESS);
+		BEGIN {EXCLUSIVE}
+		END Free;
+	END StaticAllocator;
+(*VAR
+	padding: POINTER TO ARRAY OF CHAR;
+BEGIN
+	NEW(padding, 1024 * 1024)*)
+END UsbVarTdAlloc.
+
+(* test module, uncomment for running a randomized test
+
+MODULE TestUsbVarTdAlloc; (** AUTHOR ""; PURPOSE ""; *)
+
+IMPORT UsbVarTdAlloc, Random;
+
+PROCEDURE Test*;
+VAR allocator: UsbVarTdAlloc.Allocator; adr: POINTER TO ARRAY OF ADDRESS;
+gen: Random.Generator; i,j, k: LONGINT; size: POINTER TO ARRAY OF SIZE;
+BEGIN
+	NEW(allocator, 4096, 32);
+	NEW(gen); 
+	NEW(adr, 1024); NEW(size, 1024);
+	FOR j := 0 TO 100 DO
+		FOR i := 0 TO LEN(adr)-1  DO
+			size[i] := 32+gen.Dice(120)*32;
+			adr[i] := allocator.Allocate(size[i]); 
+			ASSERT(adr[i] MOD 32 = 0);
+			ASSERT(adr[i] DIV 4096 = (adr[i] + size[i]) DIV 4096);
+			FOR k := 0 TO i-1 DO
+				ASSERT(adr[k] # adr[i]);
+			END;
+		END;
+		FOR i := 0 TO LEN(adr)-1 DO
+			allocator.Free(adr[i], size[i]); 
+		END;
+		TRACE(j); 
+	END;
+	TRACE("done");
+END Test;
+
+	
+
+END TestUsbVarTdAlloc.
+
+SystemTools.Free TestUsbVarTdAlloc UsbVarTdAlloc ~
+
+TestUsbVarTdAlloc.Test ~
+
+*)

source/ARM.Usbdi.Mod

@@ -0,0 +1,320 @@
+MODULE Usbdi; (** AUTHOR "staubesv"; PURPOSE "USB Driver Interface"; *)
+(**
+ * A2 USB Driver Interface
+ *
+ * This is the interface between USB device drivers and the A2 USB system software. If you want to develop a USB device
+ * driver, be sure you don't miss the following ressources:
+ *
+ *	UsbSkeleton.Mod	USB device driver skeleton
+ *
+ * Overview:
+ *
+ *	Driver			Base class of USB device drivers, provides access to UsbDevice object
+ *	UsbDevice		Abstraction of USB device, provides access to device descriptors and USB pipes
+ *	Pipe			Communication channel used to send/receive data. Association of device endpoint and client buffer
+ *	DriverManager	Keeps track of loaded USB device drivers
+ *)
+
+IMPORT Plugins, UsbBuffers;
+
+CONST
+
+	(** Result codes for USB transfers *)
+
+	Ok* = 0;			(** Transfer completed without errors *)
+	ShortPacket* = 1;	(** Device sent less data than requested *)
+	Stalled* = 2;		(** Pipe stalled -> needs to be cleared using the Clearhalt request before communication can continue *)
+	InProgress* = 3;		(** Transfer is still ongoing. For blocking transfers this means a timeout occured *)
+	Error* = 4;			(** Unrecoverable Error *)
+	Disconnected* = 5;	(** Device not connected to the bus anymore *)
+
+	(** Coding of the EndpointDescriptor.type field *)
+	Control* = 0;
+	BulkIn* = 1;	BulkOut* = 2;
+	InterruptIn* = 3; InterruptOut* = 4;
+	IsochronousIn* = 5; IsochronousOut* = 6;
+
+	(** bmRequestType encoding for control transfers *)
+
+	(** Request direction *)
+	ToDevice* = {}; ToHost* = {7};
+
+	(** Request type *)
+	Standard* = {}; Class* = {5}; Vendor* = {6};
+
+	(** Request recipient *)
+	Device* = {}; Interface* = {0}; Endpoint* = {1}; Other* = {0,1};
+
+	(** Pipe modes *)
+
+	Normal* = 0;			(** Poll pipe status and Yield() if transfer still in progress, slow transfers & less CPU time needed *)
+	MaxPerformance* = 1;	(** Poll pipe status continuously, fast transfers & more CPU time needed *)
+	MinCpu* = 2;			(** Don't poll pipe status - use interrupt notification, slow transfers & minimal CPU workload *)
+
+TYPE
+
+	Name* = Plugins.Name;
+	Description* = Plugins.Description;
+
+	Buffer* = (*ARRAY OF CHAR;*) UsbBuffers.Buffer;
+	BufferPtr* = (*POINTER TO Buffer;*) UsbBuffers.BufferPtr;
+
+	Status* = LONGINT;
+
+TYPE
+
+	(** Consider all fields of all descriptors as read-only! *)
+
+	DeviceDescriptor* = POINTER TO RECORD
+		bcdUSB* : LONGINT;
+		bDeviceClass* : LONGINT;
+		bDeviceSubClass* : LONGINT;
+		bDeviceProtocol* : LONGINT;
+		idVendor* : LONGINT;
+		idProduct* : LONGINT;
+		bcdDevice* : LONGINT;
+		bNumConfigurations* : LONGINT;
+	END;
+
+	(** 	A configuration is a set of interfaces. *)
+	ConfigurationDescriptor* = POINTER TO RECORD
+		bNumInterfaces* : LONGINT;
+		bConfigurationValue* : LONGINT;
+		interfaces* : POINTER TO ARRAY OF InterfaceDescriptor;
+		iads* : Iads; (* Optional Interface Association Descriptors *)
+		unknown* : UnknownDescriptor; (* Optional Class-specific descriptors *)
+	END;
+
+	(**	An interface is a set of endpoints. Device driver are typically bound to interfaces *)
+	InterfaceDescriptor* = POINTER TO RECORD
+		bInterfaceNumber* : LONGINT;
+		bAlternateSetting* : LONGINT;
+		bNumEndpoints* : LONGINT;
+		bInterfaceClass* : LONGINT;
+		bInterfaceSubClass* : LONGINT;
+		bInterfaceProtocol* : LONGINT;
+		numAlternateInterfaces* : LONGINT;
+		alternateInterfaces*: POINTER TO ARRAY OF InterfaceDescriptor;
+		endpoints* : POINTER TO ARRAY OF EndpointDescriptor;
+		unknown* : UnknownDescriptor;
+	END;
+
+	(**	Descriptor of a logical communication endpoint. USB pipe can be allocated for all endpoints using the endpoint
+		address field *)
+	EndpointDescriptor* = POINTER TO RECORD
+		type* : LONGINT; (* Control, BulkIn, BulkOut, InterruptIn, InterruptOut, IsochronousIn or IsochronousOut *)
+		bEndpointAddress* : LONGINT;
+		wMaxPacketSize* : LONGINT;
+		bmAttributes* : SET;
+		unknown* : UnknownDescriptor;
+	END;
+
+	(**	An optional Interface Association Descriptor describes a set of associated interfaces which should be handled by
+		a single device driver. *)
+	InterfaceAssociationDescriptor* = POINTER TO RECORD;
+		bFirstInterface* : LONGINT;
+		bInterfaceCount* : LONGINT;
+		bFunctionClass* : LONGINT;
+		bFunctionSubClass* : LONGINT;
+		bFunctionProtocol* : LONGINT;
+	END;
+
+	(** Optional non-USB-Standard descriptors.
+		Linked list of optional descriptors contained in the configuration descriptor but not specified in the USB specification.
+		UnknownDescriptors can be located at configuration, interface or endpoint level *)
+	UnknownDescriptor* = POINTER TO RECORD;
+		bLength* : LONGINT;
+		bDescriptorType* : LONGINT;
+		descriptor* : BufferPtr; (* contains bLength and bDescriptorType *)
+		next* : UnknownDescriptor;
+	END;
+
+TYPE
+
+	(** USB device driver base class *)
+	Driver* = OBJECT(Plugins.Plugin)
+	VAR
+		(** The fields below will be initialized by the USB driver before the driver's Connect procedure is called *)
+
+		(** Provides access to USB pipes and all the devices' descriptors *)
+		device* : UsbDevice;
+
+		(** Most often, a device driver is bound to one interface of the device. This is its descriptor. *)
+		interface* : InterfaceDescriptor;
+
+		(** This procedure is called by the USB driver when this object has been returned by the probe() procedure *)
+
+		PROCEDURE Connect*() : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END Connect; (* abstract *)
+
+		(** This procedure is called by the USB driver when the device is detached. Note that the allocated pipes will be freed automatically *)
+
+		PROCEDURE Disconnect*;
+		BEGIN HALT(301); END Disconnect; (* abstract *)
+
+	END Driver;
+
+TYPE
+
+	Configurations* = POINTER TO ARRAY OF ConfigurationDescriptor;
+
+	Iads* = POINTER TO ARRAY OF InterfaceAssociationDescriptor;
+
+TYPE
+
+	UsbDevice* = OBJECT
+	VAR
+		(** Consider all fields of this object as read-only!! *)
+
+		(** Device descriptor *)
+		descriptor* : DeviceDescriptor;
+
+		(** Device configurations *)
+		configurations* : Configurations;
+
+		(** Currently selected device configuration *)
+		actConfiguration* : ConfigurationDescriptor;
+
+		(** Device state *)
+		state* : LONGINT;
+
+		(** Direct access to the Default Control Pipe (endpoint zero). Concurrent access is allowed here. *)
+
+		PROCEDURE Request*(bmRequestType : SET;  bRequest, wValue, wIndex, wLength : LONGINT; VAR buffer : Buffer) : Status;
+		BEGIN HALT(301); RETURN Error; END Request; (* abstract *)
+
+		(** Allocate a pipe for the specified USB device endpoint *)
+
+		PROCEDURE GetPipe*(endpoint : LONGINT) : Pipe;
+		BEGIN HALT(301); RETURN NIL; END GetPipe; (* abstract *)
+
+		(**	Deallocate the ressources associtated with the specified pipe.
+			Note: This is done automatically for device drivers after their Disconnect procedure has been called. *)
+
+		PROCEDURE FreePipe*(pipe : Pipe);
+		BEGIN HALT(301); END FreePipe; (* abstract *)
+
+	END UsbDevice;
+
+TYPE
+
+	(** Handler that can be installed for a pipe and will be called when the USB transfer has been processed. *)
+	CompletionHandler* = PROCEDURE {DELEGATE} (status : Status; actLen : LONGINT);
+
+	(**	USB Communication Pipe
+	 	USB communication happens between buffers provided by client software and USB device endpoints. The association between
+	 	a client software buffer and a specific USB device endpoint is called pipe. *)
+	Pipe* = OBJECT
+	VAR
+	 	(**	How many tries should the host controllers retry the USB transaction if it fails. Note that this field is actually depended
+	 		on the host controller used. Typically, the allowed range of this value is [0,3], zero meaning infinite number of retries *)
+		maxRetries* : LONGINT;
+
+		(** Maximum packet size supported by endpoint. This field is set by the USB driver and should be considered read-only. *)
+		maxPacketSize* : LONGINT;
+
+		mode* : LONGINT;
+		
+		(** Transfer 'bufferLen' bytes from/to the specified buffer, starting at 'offset' *)
+
+		PROCEDURE Transfer*(bufferLen, offset : LONGINT; VAR buffer : Buffer) : Status;
+		BEGIN HALT(301); RETURN Error; END Transfer; (* abstract *)
+
+		(** For control transfers (only for Control Pipes) *)
+
+		PROCEDURE Request*(bmRequestType : SET;  bRequest, wValue, wIndex, wLength : LONGINT; VAR buffer : Buffer) : Status;
+		BEGIN HALT(301); RETURN Error; END Request; (* abstract *)
+
+		(** Is the halt feature of the endpoint associated with this pipe set? *)
+
+		PROCEDURE IsHalted*() : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END IsHalted; (* abstract *)
+
+		(** Clear halt feature of endpoint associated with this pipe. *)
+
+		PROCEDURE ClearHalt*() : BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE; END ClearHalt; (* abstract *)
+
+		(** Set timeout for transfers for this pipe;  0 = NonBlocking, n = n milliseconds *)
+
+		PROCEDURE SetTimeout*(timeout : LONGINT);
+		BEGIN HALT(301); END SetTimeout; (* abstract *)
+
+		(** Specifiy the completion handler that is called when the USB transfer is processed. *)
+
+		PROCEDURE SetCompletionHandler*(handler: CompletionHandler);
+		BEGIN HALT(301); END SetCompletionHandler;
+
+		(** Update and return the status of the current USB transfer.
+		 	@param actLen: Number of bytes that have been sent/receive (only valid if status * ResInProgress = {})
+		 	@return: Status of the USB transfer *)
+
+		PROCEDURE GetStatus*(VAR actLen : LONGINT) : Status;
+		BEGIN HALT(301); RETURN Error; END GetStatus; (* abstract *)
+
+		(** Return the actual number of bytes transfered. *)
+
+		PROCEDURE GetActLen*() : LONGINT;
+		BEGIN HALT(301); RETURN 0; END GetActLen; (* abstract *)
+
+		(**
+		 * Cancel a transaction (transfer or request) that was already scheduled in the pipe.
+		 * Fails if the transfer is completed, in progress or under some HCD-specific constraints.
+		 * Available only if the pipe is non-blocking.
+		 *)
+		PROCEDURE CancelTransaction * (id: LONGINT): BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE
+		END CancelTransaction;
+
+		(**
+		 * Set the policy of a periodic (isochronous or interrupt) pipe.
+		 * The policy is given as an interval and a size.
+		 * The interval is the time between 2 samples in 0.1 microsecond units.
+		 * The size is the maximum sample size in bytes.
+		 * This procedure returns FALSE if there is not enough bandwidth to accomodate
+		 * the specified policy and TRUE otherwise.
+		 *)
+		PROCEDURE SetPolicy * (interval, size: LONGINT): BOOLEAN;
+		BEGIN HALT(301); RETURN FALSE
+		END SetPolicy;
+
+		(** Show debug information for this pipe. In detailed mode, the scheduling data structures related to
+			this pipe will be shown (QH and TD list) *)
+
+		PROCEDURE Show*(detailed : BOOLEAN);
+		BEGIN HALT(301); END Show; (* abstract *)
+
+	END Pipe;
+
+TYPE
+
+	(** The USB driver will call the Probe procedure for each interface of the device *)
+	ProbeProc* = PROCEDURE {DELEGATE} (device : UsbDevice; interface : InterfaceDescriptor) : Driver;
+
+	(** This object manages USB device drivers. All device drivers registered at the driver manager are automatically mapped to
+		appropriate USB functions. *)
+	DriverManager* = OBJECT
+
+		(** Add a USB device driver to the internal registry. Driver names have to be unique and no longer than 30 characters (incl. Null-String) *)
+
+		PROCEDURE Add*(probe : ProbeProc; CONST name: Plugins.Name; CONST desc: Plugins.Description; priority : LONGINT);
+		BEGIN HALT(301); END Add; (* abstract *)
+
+		(** Calls Disconnect of all instances of the driver. All instances are removed from the usbDrivers registry
+			and the device driver is removed from the internal registry  *)
+
+		PROCEDURE Remove*(CONST name : Plugins.Name);
+		BEGIN HALT(301); END Remove; (* abstract *)
+
+	END DriverManager;
+
+VAR
+	(** Can be used if you need to pass an VAR parameter argument which is not used *)
+	NoData* : (*ARRAY 1 OF CHAR;*)UsbBuffers.Buffer;
+
+	(** Instantiated by USB driver; consider read-only *)
+	drivers* : DriverManager;
+
+BEGIN
+	NEW(NoData, 1)
+END Usbdi.

source/ARM.WMRasterScale.Mod

@@ -0,0 +1,1876 @@
+MODULE WMRasterScale;	(** AUTHOR "TF"; PURPOSE "Support scaling of images"; *)
+(** AUTHOR "MZ"; PURPOSE "Speedup rasterops with SSE2"; *)
+
+IMPORT
+	SYSTEM, Raster, Rect := WMRectangles;
+
+CONST
+	(** Copy Modes *)
+	ModeCopy* = 0; ModeSrcOverDst* = 1;
+
+	(** Scale Modes *)
+	ScaleBox* = 0; ScaleBilinear* = 1;
+
+TYPE
+	Rectangle = Rect.Rectangle;
+	Image = Raster.Image;
+	ScalerProc = PROCEDURE (src, dst : Image; VAR dr : Rectangle; sx, sy, sdx, sdy : LONGINT);
+	XScalerProc = PROCEDURE (srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+
+(* copy sr in 16.16 fix rectangle  from src to dr integer rectangle in dst *)
+PROCEDURE Q0GenericCopy(src, dst : Image; VAR dr : Rectangle; sx, sy, sdx, sdy : LONGINT);
+VAR x, y : LONGINT; col : Raster.Pixel;
+	getMode, putMode : Raster.Mode;
+	fx, fy : LONGINT;
+BEGIN
+	Raster.InitMode(getMode, Raster.srcCopy);
+	Raster.InitMode(putMode, Raster.srcCopy);
+	fy := sy;
+	FOR y := dr.t TO dr.b - 1 DO
+		fx := sx;
+		FOR x := dr.l TO dr.r - 1 DO
+			Raster.Get(src, fx DIV 65536, fy DIV 65536, col, getMode);
+			INC(fx, sdx);
+			Raster.Put(dst, x, y, col, putMode)
+		END;
+		INC(fy, sdy)
+	END
+END Q0GenericCopy;
+
+PROCEDURE Q0GenericSrcOverDst(src, dst : Image; VAR dr : Rectangle; sx, sy, sdx, sdy : LONGINT);
+VAR x, y : LONGINT; col : Raster.Pixel;
+	getMode, putMode : Raster.Mode;
+	fx, fy : LONGINT;
+BEGIN
+	Raster.InitMode(getMode, Raster.srcCopy);
+	Raster.InitMode(putMode, Raster.srcOverDst);
+	fy := sy;
+	FOR y := dr.t TO dr.b - 1 DO
+		fx := sx;
+		FOR x := dr.l TO dr.r - 1 DO
+			Raster.Get(src, fx DIV 65536, fy DIV 65536, col, getMode);
+			INC(fx, sdx);
+			Raster.Put(dst, x, y, col, putMode)
+		END;
+		INC(fy, sdy)
+	END
+END Q0GenericSrcOverDst;
+
+(* copy sr in 16.16 fix rectangle  from src to dr integer rectangle in dst *)
+PROCEDURE Q1GenericCopy(src, dst : Image; VAR dr : Rectangle; sx, sy, sdx, sdy : LONGINT);
+VAR x, y, xfleft, xfright, yftop, yfbottom : LONGINT; col, col0, col1, col2, col3 : Raster.Pixel;
+	b0, g0, r0, a0, b1, g1, r1, a1, cb, cg, cr, ca : LONGINT;
+	getMode, putMode : Raster.Mode;
+	fx, fy : LONGINT; x0, x1, y0, y1 : LONGINT;
+BEGIN
+	Raster.InitMode(getMode, Raster.srcCopy);
+	Raster.InitMode(putMode, Raster.srcCopy);
+	fy := sy - 8000H; sx := sx - 8000H;
+	FOR y := dr.t TO dr.b - 1 DO
+		fx := sx; y0 := Bounds(fy DIV 65536, 0, src.height - 1); y1 := Bounds(fy DIV 65536 + 1, 0, src.height - 1);
+		FOR x := dr.l TO dr.r - 1 DO
+			x0 := Bounds(fx DIV 65536, 0, src.width - 1); x1 := Bounds(fx DIV 65536 + 1, 0, src.width - 1);
+			Raster.Get(src, x0, y0, col0, getMode);
+			Raster.Get(src, x1, y0, col1, getMode);
+			Raster.Get(src, x0, y1, col2, getMode);
+			Raster.Get(src, x1, y1, col3, getMode);
+
+			xfleft := (65536 - fx MOD 65536);
+			xfright := (fx MOD 65536);
+
+			b0 := (ORD(col0[Raster.b]) * xfleft + ORD(col1[Raster.b]) * xfright) DIV 65536;
+			g0 := (ORD(col0[Raster.g]) * xfleft + ORD(col1[Raster.g]) * xfright) DIV 65536;
+			r0 := (ORD(col0[Raster.r]) * xfleft + ORD(col1[Raster.r]) * xfright) DIV 65536;
+			a0 := (ORD(col0[Raster.a]) * xfleft + ORD(col1[Raster.a]) * xfright) DIV 65536;
+
+			b1 := (ORD(col2[Raster.b]) * xfleft + ORD(col3[Raster.b]) * xfright) DIV 65536;
+			g1 := (ORD(col2[Raster.g]) * xfleft + ORD(col3[Raster.g]) * xfright) DIV 65536;
+			r1 := (ORD(col2[Raster.r]) * xfleft + ORD(col3[Raster.r]) * xfright) DIV 65536;
+			a1 := (ORD(col2[Raster.a]) * xfleft + ORD(col3[Raster.a]) * xfright) DIV 65536;
+
+			yftop := (65536 - fy MOD 65536);
+			yfbottom := (fy MOD 65536);
+			cb := (b0 * yftop + b1 * yfbottom) DIV 65536;
+			cg := (g0 * yftop + g1 * yfbottom) DIV 65536;
+			cr := (r0 * yftop + r1 * yfbottom) DIV 65536;
+			ca := (a0 * yftop + a1 * yfbottom) DIV 65536;
+
+			col[Raster.b] := CHR(cb);
+			col[Raster.g] := CHR(cg);
+			col[Raster.r] := CHR(cr);
+			col[Raster.a] := CHR(ca);
+			INC(fx, sdx);
+			Raster.Put(dst, x, y, col, putMode)
+		END;
+		INC(fy, sdy)
+	END
+END Q1GenericCopy;
+
+(* copy sr in 16.16 fix rectangle  from src to dr integer rectangle in dst *)
+PROCEDURE Q1GenericSrcOverDst(src, dst : Image; VAR dr : Rectangle; sx, sy, sdx, sdy : LONGINT);
+VAR x, y, xfleft, xfright, yftop, yfbottom : LONGINT; col, col0, col1, col2, col3 : Raster.Pixel;
+	b0, g0, r0, a0, b1, g1, r1, a1, cb, cg, cr, ca : LONGINT;
+	getMode, putMode : Raster.Mode;
+	fx, fy : LONGINT; x0, x1, y0, y1 : LONGINT;
+BEGIN
+	Raster.InitMode(getMode, Raster.srcCopy);
+	Raster.InitMode(putMode, Raster.srcOverDst);
+	fy := sy - 8000H; sx := sx - 8000H;
+	FOR y := dr.t TO dr.b - 1 DO
+		fx := sx; y0 := Bounds(fy DIV 65536, 0, src.height - 1); y1 := Bounds(fy DIV 65536 + 1, 0, src.height - 1);
+		FOR x := dr.l TO dr.r - 1 DO
+			x0 := Bounds(fx DIV 65536, 0, src.width - 1); x1 := Bounds(fx DIV 65536 + 1, 0, src.width - 1);
+			Raster.Get(src, x0, y0, col0, getMode);
+			Raster.Get(src, x1, y0, col1, getMode);
+			Raster.Get(src, x0, y1, col2, getMode);
+			Raster.Get(src, x1, y1, col3, getMode);
+
+			xfleft := (65536 - fx MOD 65536);
+			xfright := (fx MOD 65536);
+
+			b0 := (ORD(col0[Raster.b]) * xfleft + ORD(col1[Raster.b]) * xfright) DIV 65536;
+			g0 := (ORD(col0[Raster.g]) * xfleft + ORD(col1[Raster.g]) * xfright) DIV 65536;
+			r0 := (ORD(col0[Raster.r]) * xfleft + ORD(col1[Raster.r]) * xfright) DIV 65536;
+			a0 := (ORD(col0[Raster.a]) * xfleft + ORD(col1[Raster.a]) * xfright) DIV 65536;
+
+			b1 := (ORD(col2[Raster.b]) * xfleft + ORD(col3[Raster.b]) * xfright) DIV 65536;
+			g1 := (ORD(col2[Raster.g]) * xfleft + ORD(col3[Raster.g]) * xfright) DIV 65536;
+			r1 := (ORD(col2[Raster.r]) * xfleft + ORD(col3[Raster.r]) * xfright) DIV 65536;
+			a1 := (ORD(col2[Raster.a]) * xfleft + ORD(col3[Raster.a]) * xfright) DIV 65536;
+
+			yftop := (65536 - fy MOD 65536);
+			yfbottom := (fy MOD 65536);
+			cb := (b0 * yftop + b1 * yfbottom) DIV 65536;
+			cg := (g0 * yftop + g1 * yfbottom) DIV 65536;
+			cr := (r0 * yftop + r1 * yfbottom) DIV 65536;
+			ca := (a0 * yftop + a1 * yfbottom) DIV 65536;
+
+			col[Raster.b] := CHR(cb);
+			col[Raster.g] := CHR(cg);
+			col[Raster.r] := CHR(cr);
+			col[Raster.a] := CHR(ca);
+			INC(fx, sdx);
+			Raster.Put(dst, x, y, col, putMode)
+		END;
+		INC(fy, sdy)
+	END
+END Q1GenericSrcOverDst;
+
+
+PROCEDURE XQ0BGR565BGR565(srcadr,dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y : LONGINT; yadr, adr, sa, col : LONGINT;
+	fx, fy : LONGINT;
+BEGIN
+	fy := sy;
+	yadr := dstadr + dl * 2 + dt * dstbpr;
+	FOR y := dt TO db - 1 DO
+		fx := sx;
+		adr := yadr;
+		sa := srcadr + (fy DIV 65536) * srcbpr;
+		FOR x := dl TO dr - 1 DO
+			col := SYSTEM.GET16(sa + (fx DIV 65536) * 2);
+			INC(fx, sdx);
+			SYSTEM.PUT16(adr, col);
+			INC(adr, 2);
+		END;
+		INC(fy, sdy);
+		INC(yadr, dstbpr)
+	END
+END XQ0BGR565BGR565;
+
+(*
+(* this asm version is 2.3 times faster than the portable version. (P3/600/Dell precision 420 (dual)) *)
+PROCEDURE XQ0BGR565BGR565(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR yadr : LONGINT;
+CODE {SYSTEM.i386}
+	MOV	EDX, [EBP+dstadr]
+	MOV	EBX, [EBP+dl]
+	SHL	EBX, 1
+	ADD	EDX, EBX
+	MOV	EBX, [EBP+dt]
+	IMUL	EBX, [EBP+dstbpr]
+	ADD	EDX, EBX	; edx = dstadr + 2 * dl + dt * dstbpr
+	MOV	[EBP+yadr], EDX
+	; init first EDI
+	MOV	EDI, EDX
+
+	MOV	ECX, [EBP+dt]
+	SUB	[EBP+db], ECX	; counter in db
+
+	MOV	EDX, [EBP+sdx]	; keep EDX
+
+	; init first ESI
+	MOV	ESI, [EBP+srcadr]	; calc new source adr
+	MOV	EAX, [EBP+sy]
+	SHR	EAX, 16	; integer part of sy
+	IMUL 	EAX, [EBP+srcbpr]	; sy * srcbpr
+	ADD	ESI, EAX	; first source adr in ESI
+
+outerloop:
+	MOV	EBX, [EBP+sx]
+	MOV	ECX, [EBP+dr]	; FOR x := dl TO dr - 1 DO
+	SUB	ECX, [EBP+dl]
+innerloop:
+	MOV	EAX, EBX
+	SHR	EAX, 16
+	MOV	AX, WORD [ESI + EAX * 2]	; read the pixel
+	ADD	EBX, EDX	; INC fx, sdx
+	MOV	[EDI], AX	; set the pixel
+	ADD	EDI, 2	; inc adr
+	LOOP	innerloop
+
+	; free : EAX, EBX, ECX
+	MOV	EAX, [EBP+sy]	; sy := sy + sdy
+	ADD	EAX, [EBP+sdy]
+	MOV	[EBP+sy], EAX	; keep sy in EAX
+
+	MOV	ESI, [EBP+srcadr]	; calc new source adr
+	SHR	EAX, 16	; integer part of sy
+	IMUL 	EAX, [EBP+srcbpr]	; sy * srcbpr
+	ADD	ESI, EAX	; new source adr in ESI
+
+	; new dst address
+	MOV	ECX, [EBP+dstbpr]
+	MOV	EAX, [EBP+yadr]
+	ADD	EAX, ECX
+	MOV	EDI, EAX
+	MOV	[EBP+yadr], EAX
+
+	DEC	DWORD [EBP+db]
+	JNLE	outerloop
+END XQ0BGR565BGR565;
+*)
+
+(*PROCEDURE SSE2Q0BGR565BGR565(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT (*; VAR  mysrc, mydest, myres: ARRAY OF LONGINT*));
+VAR yadr : LONGINT;
+CODE {SYSTEM.i386, SYSTEM.MMX, SYSTEM.SSE, SYSTEM.SSE2}
+	PUSHFD
+	PUSH 		EBX
+	; CLI
+
+	MOV		EDX, [EBP+dstadr]
+	MOV		EBX, [EBP+dl]
+	SHL			EBX, 1
+	ADD		EDX, EBX
+	MOV		EBX, [EBP+dt]
+	IMUL		EBX, [EBP+dstbpr]
+	ADD		EDX, EBX	; edx = dstadr + 2 * dl + dt * dstbpr
+	MOV		[EBP+yadr], EDX
+
+	; init first EDI
+	MOV		EDI, EDX
+
+	MOV		ECX, [EBP+dt]
+	SUB		[EBP+db], ECX	; counter in db
+	JLE			endyloop
+	MOV		EDX, [EBP+sdx]	; keep EDX
+
+	; init first ESI
+	MOV		ESI, [EBP+srcadr]	; calc new source adr
+	MOV		EAX, [EBP+sy]
+	SHR		EAX, 16			; integer part of sy
+	IMUL 		EAX, [EBP+srcbpr]	; sy * srcbpr
+	ADD		ESI, EAX		; first source adr in ESI
+
+outerloop:
+	MOV		EBX, [EBP+sx]
+	MOV		ECX, [EBP+dr]	; FOR x := dl TO dr - 1 DO
+	SUB		ECX, [EBP+dl]
+	JLE			endyloop
+
+innerloop:
+	CMP 		ECX, 8
+	 JLE			singlepixel
+
+	PXOR 		XMM0, XMM0
+
+	; 8pixels at the time
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2] ; read the pixel
+	PINSRW  	XMM0, EAX,0
+	ADD		EBX, EDX			; INC fx, sdx
+
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2] ; read the pixel
+	PINSRW  	XMM0, EAX,1
+	ADD		EBX, EDX			; INC fx, sdx
+
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2] ; read the pixel
+	PINSRW  	XMM0, EAX,2
+	ADD		EBX, EDX			; INC fx, sdx
+
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2] ; read the pixel
+	PINSRW  	XMM0, EAX,3
+	ADD		EBX, EDX			; INC fx, sdx
+
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2] ; read the pixel
+	PINSRW  	XMM0, EAX,4
+	ADD		EBX, EDX			; INC fx, sdx
+
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2] ; read the pixel
+	PINSRW  	XMM0, EAX,5
+	ADD		EBX, EDX			; INC fx, sdx
+
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2] ; read the pixel
+	PINSRW  	XMM0, EAX,6
+	ADD		EBX, EDX			; INC fx, sdx
+
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2] ; read the pixel
+	PINSRW  	XMM0, EAX,7
+	ADD		EBX, EDX			; INC fx, sdx
+
+	MOVDQU 	[EDI], XMM0 		;	MOV	[EDI], AX							; set the pixels
+	ADD		EDI, 16				; inc adr
+	SUB 		ECX, 8
+	CMP 		ECX, 0
+	JE			outside2
+	; LOOP 	innerloop
+	JMP 		innerloop
+
+singlepixel:
+	MOV		EAX, EBX
+	SHR		EAX, 16
+	MOV		AX, WORD [ESI + EAX * 2]	; read the pixel
+	ADD		EBX, EDX			; INC fx, sdx
+	MOV		[EDI], AX			; set the pixel
+	ADD		EDI, 2				; inc adr
+	SUB 		ECX, 1
+	CMP 		ECX, 0
+	JE			outside2
+	; LOOP 	innerloop
+	JMP 		innerloop
+
+outside2:
+	; free : EAX, EBX, ECX
+	MOV		EAX, [EBP+sy]		; sy := sy + sdy
+	ADD		EAX, [EBP+sdy]
+	MOV		[EBP+sy], EAX		; keep sy in EAX
+
+	MOV		ESI, [EBP+srcadr]		; calc new source adr
+	SHR		EAX, 16				; integer part of sy
+	IMUL 		EAX, [EBP+srcbpr]	; sy * srcbpr
+	ADD		ESI, EAX			; new source adr in ESI
+
+	; new dst address
+	MOV		ECX, [EBP+dstbpr]
+	MOV		EAX, [EBP+yadr]
+	ADD		EAX, ECX
+	MOV		EDI, EAX
+	MOV		[EBP+yadr], EAX
+
+	DEC		DWORD [EBP+db]
+	JNLE		outerloop
+
+endyloop:
+	EMMS 							; declare FPU registers free
+	POP 		EBX
+	POPFD
+END SSE2Q0BGR565BGR565;
+*)
+
+PROCEDURE Q1BGR565BGR565(srcadr,dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y, xfleft, xfright, yftop, yfbottom : LONGINT; yadr, adr: ADDRESS; col0, col1, col2, col3  : LONGINT;
+	b0, g0, r0, b1, g1, r1, cb, cg, cr : LONGINT;
+	fx, fy, xadd1, xadd2 : LONGINT; yadd1, yadd2: ADDRESS;
+BEGIN
+
+	yadr := dstadr + dl * 2 + dt * dstbpr;
+	fy := sy - 8000H; sx := sx - 8000H;
+	FOR y := dt TO db - 1 DO
+		fx := sx;
+		adr := yadr;
+		yadd1 := srcadr + Bounds(fy DIV 65536, 0, sh - 1) * srcbpr;
+		yadd2 := srcadr + Bounds(fy DIV 65536 + 1, 0, sh - 1) * srcbpr;
+		FOR x := dl TO dr - 1 DO
+			xadd1 := Bounds(fx DIV 65536, 0, sw - 1) * 2;
+			xadd2 := Bounds(fx DIV 65536 + 1, 0, sw - 1) * 2;
+			col0 := SYSTEM.GET16(yadd1 + xadd1);
+			col1 := SYSTEM.GET16(yadd1 + xadd2);
+			col2 := SYSTEM.GET16(yadd2 + xadd1);
+			col3 := SYSTEM.GET16(yadd2 + xadd2);
+
+			xfleft := (65536 - fx MOD 65536);
+			xfright := (fx MOD 65536);
+
+			b0 := ((col0 MOD 32) * 8 * xfleft + (col1 MOD 32) * 8 * xfright) DIV 65536;
+			g0 := ((col0 DIV 32 MOD 64) * 4 * xfleft + (col1 DIV 32 MOD 64) * 4 * xfright) DIV 65536;
+			r0 := ((col0 DIV 2048 MOD 32) * 8 * xfleft + (col1 DIV 2048 MOD 32) * 8 * xfright) DIV 65536;
+
+			b1 := ((col2 MOD 32) * 8 * xfleft + (col3 MOD 32) * 8 * xfright) DIV 65536;
+			g1 := ((col2 DIV 32 MOD 64) * 4 * xfleft + (col3 DIV 32 MOD 64) * 4 * xfright) DIV 65536;
+			r1 := ((col2 DIV 2048 MOD 32) * 8 * xfleft + (col3 DIV 2048 MOD 32) * 8 * xfright) DIV 65536;
+
+
+			yftop := (65536 - fy MOD 65536);
+			yfbottom := (fy MOD 65536);
+			cb := (b0 * yftop + b1 * yfbottom) DIV 65536;
+			cg := (g0 * yftop + g1 * yfbottom) DIV 65536;
+			cr := (r0 * yftop + r1 * yfbottom) DIV 65536;
+			INC(fx, sdx);
+			SYSTEM.PUT16(adr, ASH(cb, -3) + ASH(ASH(cg, -2), 5) + ASH(ASH(cr, -3), 11));
+			INC(adr, 2);
+		END;
+		INC(fy, sdy);
+		INC(yadr, dstbpr)
+	END
+END Q1BGR565BGR565;
+
+PROCEDURE SSE2Q1BGR565BGR565(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y, xfleft, xfright, yftop, yfbottom : LONGINT; yadr, adr, col, col0, col1, col2, col3 : LONGINT;
+	b0, g0, r0, b1, g1, r1, cb, cg, cr : LONGINT;
+	fx, fy, yadd1, yadd2, xadd1, xadd2 : LONGINT;
+END SSE2Q1BGR565BGR565;
+
+PROCEDURE Q1BGRA8888BGR565(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y, xfleft, xfright, yftop, yfbottom : LONGINT; yadr: ADDRESS; col, col0, col1, col2, col3 : LONGINT;
+	b0, g0, r0, a0, b1, g1, r1, a1, cb, cg, cr, ca, dstb, dstg, dstr : LONGINT;
+	fx, fy, xadd0, xadd1: LONGINT; yadd0, yadd1: ADDRESS;
+BEGIN
+	yadr := dstadr + dl * 2 + dt * dstbpr;
+	fy := sy - 8000H; sx := sx - 8000H;
+	FOR y := dt TO db - 1 DO
+		fx := sx;
+		dstadr := yadr;
+		yadd0 := srcadr + Bounds(fy DIV 65536, 0, sh - 1) * srcbpr;
+		yadd1 := srcadr + Bounds(fy DIV 65536 + 1, 0, sh - 1) * srcbpr;
+		FOR x := dl TO dr - 1 DO
+			(* destination color *)
+			col := SYSTEM.GET16(dstadr);
+			dstb := (col MOD 32) * 8; dstg := (col DIV 32 MOD 64) * 4; dstr := (col DIV 2048 MOD 32) * 8;
+			xadd0 := Bounds(fx DIV 65536, 0, sw - 1) * 4;
+			xadd1 := Bounds(fx DIV 65536 + 1, 0, sw - 1) * 4;
+			col0 := SYSTEM.GET32(yadd0 + xadd0);
+			col1 := SYSTEM.GET32(yadd0 + xadd1);
+			col2 := SYSTEM.GET32(yadd1 + xadd0);
+			col3 := SYSTEM.GET32(yadd1 + xadd1);
+
+			xfleft := (65536 - fx MOD 65536);
+			xfright := (fx MOD 65536);
+			yftop := (65536 - fy MOD 65536);
+			yfbottom := (fy MOD 65536);
+			a0 := ((col0 DIV 1000000H MOD 100H) * xfleft + (col1 DIV 1000000H MOD 100H) * xfright) DIV 65536;
+			a1 := ((col2 DIV 1000000H MOD 100H) * xfleft + (col3 DIV 1000000H MOD 100H) * xfright) DIV 65536;
+			ca := (a0 * yftop + a1 * yfbottom) DIV 65536;
+
+			IF ca # 0 THEN
+				b0 := ((col0 MOD 100H) * xfleft + (col1 MOD 100H) * xfright) DIV 65536;
+				g0 := ((col0 DIV 100H MOD 100H) * xfleft + (col1 DIV 100H MOD 100H) * xfright) DIV 65536;
+				r0 := ((col0 DIV 10000H MOD 100H) * xfleft + (col1 DIV 10000H MOD 100H) * xfright) DIV 65536;
+
+				b1 := ((col2 MOD 100H) * xfleft + (col3 MOD 100H) * xfright) DIV 65536;
+				g1 := ((col2 DIV 100H MOD 100H) * xfleft + (col3 DIV 100H MOD 100H) * xfright) DIV 65536;
+				r1 := ((col2 DIV 10000H MOD 100H) * xfleft + (col3 DIV 10000H MOD 100H) * xfright) DIV 65536;
+
+				cb := (b0 * yftop + b1 * yfbottom) DIV 65536;
+				cg := (g0 * yftop + g1 * yfbottom) DIV 65536;
+				cr := (r0 * yftop + r1 * yfbottom) DIV 65536;
+				IF ca # 255 THEN
+					cb := (cb * 256 + (256 - ca) * dstb) DIV 256; IF cb > 256 THEN cb := 256 END;
+					cg := (cg * 256 + (256 - ca) * dstg) DIV 256; IF cg > 256 THEN cg := 256 END;
+					cr := (cr * 256 + (256 - ca) * dstr) DIV 256; IF cr > 256 THEN cr := 256 END
+				END;
+				SYSTEM.PUT16(dstadr, ASH(cb, -3) + ASH(ASH(cg, -2), 5) + ASH(ASH(cr, -3), 11))
+			END;
+			INC(fx, sdx);
+			INC(dstadr, 2);
+		END;
+		INC(fy, sdy);
+		INC(yadr, dstbpr)
+	END
+END Q1BGRA8888BGR565;
+
+(*
+PROCEDURE SSE2Q1BGRA8888BGR565(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh:LONGINT);
+	VAR x, y, z,xfleft, xfright, yftop, yfbottom : LONGINT; yadr, adr, col, col0, col1, col2, col3 : LONGINT;
+	b0, g0, r0, a0, a01,b1, g1, r1, a1, cb, cg, cr,cb2, cg2, cr2, ca, ca2,dstb, dstg, dstr,res : LONGINT;
+	fx, fy, yadd1, yadd2, xadd1, xadd2: LONGINT;
+
+CODE {SYSTEM.i386, SYSTEM.MMX, SYSTEM.SSE, SYSTEM.SSE2}
+	PUSHFD
+	PUSH 		EBX
+	; CLI
+	PXOR		MMX3,MMX3
+	PXOR		MMX4,MMX4
+	PXOR		MMX5, MMX5
+	PXOR		MMX6, MMX6
+	PXOR		XMM1, XMM1
+	PXOR		XMM3, XMM3
+	PXOR		XMM4, XMM4
+	PXOR		XMM6, XMM6
+	PXOR		XMM7, XMM7
+
+	MOV		EDX, [EBP+dstadr]
+	MOV		EBX, [EBP+dl]
+	SHL			EBX, 1
+	ADD		EDX, EBX
+	MOV		EBX, [EBP+dt]
+	IMUL		EBX, [EBP+dstbpr]
+	ADD		EDX, EBX
+	MOV		[EBP+yadr], EDX
+
+	MOV		EDX, [EBP+sy]
+	SUB		EDX, 8000H 			;edx = sy-8000H
+	MOV	 	[EBP+fy], EDX
+
+	; sx := sx - 8000H;
+	MOV		EDX, [EBP+sx]
+	SUB		EDX, 8000H 			;sx = sx-8000H
+	MOV		[EBP+sx] , EDX
+
+	MOV		ECX, [EBP+db]
+	SUB		ECX, [EBP+dt]			; counter in y
+	JLE			endyloop				;exit
+	MOV		[EBP+y], ECX
+
+outerloop:
+	MOV		EDX, [EBP+yadr]
+	MOV		EDI, EDX ; adr in EDI
+
+	MOV		[EBP+adr], EDX
+
+	MOV		EDX, [EBP+sx]			; keep EDX
+	MOV		[EBP+fx], EDX
+
+	MOV 		EAX, [EBP+fy]
+	MOVD		XMM3, EAX 			; prepare for top, bottom
+	SAR 		EAX, 16
+	CMP 		EAX, 0
+	JE			zero
+	JL			negativ
+	MOV		EBX, [EBP+sh]
+	SUB		EBX, 1
+	CMP		EAX, EBX
+	JGE			bigger
+
+ok:
+	MOV		EBX, EAX
+	ADD		EBX, 1
+	JMP 		different
+
+zero:
+	MOV 		EAX, 0
+	MOV		EBX, 1
+	JMP 		different
+
+negativ:
+	MOV 		EAX, 0
+	MOV		EBX, 0
+	JMP			samepixel
+
+bigger:
+	MOV		EAX, EBX
+	JMP			samepixel
+
+different:
+	MOV		ECX, [EBP+srcbpr]
+	MUL		EAX, ECX
+	MOV		EBX, EAX
+	ADD		EBX, ECX
+	MOV		ECX, [EBP+srcadr]
+	ADD		EAX, ECX
+	ADD		EBX, ECX
+	JMP			endyadd
+
+samepixel:
+	MOV		ECX, [EBP+srcbpr]
+	MUL		EAX, ECX
+	MOV		ECX, [EBP+srcadr]
+	ADD		EAX, ECX
+	MOV		EBX, EAX
+
+endyadd:
+	MOV		[EBP+yadd1], EAX
+	MOV		[EBP+yadd2], EBX
+
+	; yfbottom := (fy MOD 65536);
+	; yftop := (65536 - fy MOD 65536);
+
+	MOVD		ECX, XMM3
+	AND		ECX, 0FFFFH
+	MOV		[EBP+yfbottom],ECX
+	PINSRW 	XMM3, ECX, 1
+
+	NEG		ECX
+	ADD		ECX, 65535
+	MOV		[EBP+yftop],ECX
+	PINSRW 	XMM3, ECX, 0
+
+	PSRLW		XMM3, 1
+
+	MOV		ECX, [EBP+dr]
+	SUB		ECX, [EBP+dl]			; counter in x
+	JLE			endyloop				;exit
+	MOV		[EBP+x], ECX
+
+innerloop:
+	MOV 		ECX, [EBP+x]
+	; if x < 8 then do one pixel at the time
+	CMP		ECX, 8
+	JL 			singlepixel
+	; else
+	; take 8 at the time
+
+	MOV		EBX, EDI
+	AND 		EBX, 0FH
+	CMP		EBX, 0
+	JNE	 		singlepixel
+
+alleightpixels:
+	MOV	 	EAX, 0000000FFH
+	MOVD		MMX3, EAX
+
+	; dest red -> MMX4
+	MOV	 	EAX, 0F800F800H
+	MOVD		MMX4, EAX
+
+	; dest green -> MMX5
+	MOV	 	EAX, 07E007E0H
+	MOVD		MMX5, EAX
+
+	; dest blue -> MMX6 ; moved as MMX6 is used in singlepixel
+	; MOV	 EAX, 001F001FH
+	; MOVD		MMX6, EAX
+
+	MOV		ECX, [EBP+yfbottom]
+	PINSRW 	XMM3, ECX, 1
+	MOV		ECX, [EBP+yftop]
+	PINSRW 	XMM3, ECX, 0
+	PSRLW 		XMM3,1
+
+	PXOR		XMM5, XMM5
+	PXOR 		XMM2,XMM2
+	MOV		DWORD [EBP+z], 4
+
+loop03:
+	; shift everything left
+	MOV 		ECX, [EBP+fx]
+	PSLLDQ		XMM5, 4
+
+	PINSRW	XMM7, ECX,0 ; prepare for l,r
+
+	SAR 		ECX, 16
+	CMP 		ECX, 0
+	JE			zerox03
+	JL			negativx03
+	MOV		EDX, [EBP+sw]
+	SUB		EDX, 1
+	CMP		ECX, EDX
+	JGE			biggerx03
+
+okx03:
+	MOV		EDX, ECX
+	ADD		EDX, 1
+	JMP			endbound203
+zerox03:
+	MOV 		ECX, 0
+	MOV		EDX, 1
+	JMP 		endbound203
+
+negativx03:
+	MOV 		ECX, 0
+	MOV		EDX, 0
+	JMP			endbound203
+
+biggerx03:
+	MOV		ECX, EDX
+endbound203:
+	SHL			ECX, 2 					; xadd1
+	SHL			EDX, 2 					; xadd2
+
+	MOV		EAX, [EBP+yadd1]
+	MOV		EBX, [EBP+yadd2]
+
+	MOVD		XMM2, [EBX+EDX]
+	PSLLDQ   	XMM2,4
+	MOVD		XMM1, [EBX+ECX]
+	POR		XMM2,XMM1
+	PSLLDQ   	XMM2,4
+	MOVD		XMM1, [EAX+EDX]
+	POR		XMM2,XMM1
+	PSLLDQ		XMM2,4
+	MOVD		XMM1, [EAX+ECX]
+	POR		XMM2,XMM1
+
+	PEXTRW	EAX,XMM7,0
+	AND		EAX, 0FFFFH
+	PINSRW 	XMM7, EAX,1
+	PINSRW	XMM7, EAX, 3 			;xfright
+
+	NEG		AX
+	ADD		EAX, 65535
+	PINSRW 	XMM7, EAX, 0
+	PINSRW	XMM7, EAX, 2 			;xfleft
+
+	PSRLW 		XMM7, 1
+
+	MOVDQU	XMM0, XMM2
+	PSRLD		XMM0, 24
+	PXOR		XMM1, XMM1
+
+	MOV		ECX, 0FFH 				; ECX locked for ca
+
+	PINSRW	XMM1, ECX,0
+	PINSRW	XMM1, ECX,2
+	PINSRW	XMM1, ECX,4
+	PINSRW	XMM1, ECX,6
+
+	PCMPEQW	XMM1, XMM0
+
+	PMOVMSKB	EAX, XMM1
+	CMP		EAX, 0FFFFH
+	JE 			endofalpha03
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 				; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   				; XMM3 already shifted by 1
+	PEXTRW	ECX, XMM0, 0
+
+endofalpha03:
+	; alpha done
+
+	CMP		ECX,0
+	JE			alphazero03
+
+	SHL			ECX, 24
+
+	; calculate red
+
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0, 8
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 				; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   				; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+
+	SHL	EBX,16
+	OR	ECX,EBX
+
+	; red done
+
+	; calculate green
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0, 16
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 				; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   				; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+	SHL 		EBX,8
+	OR 			ECX,EBX
+
+	; green done
+
+	; calculate blue
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0,24
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0, XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 				; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   				; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+
+	OR			ECX,EBX
+	; blue done
+
+	; put color in correct position
+	MOVD		XMM4,ECX
+	POR		XMM5, XMM4 ; results in XMM5
+
+	; prepared source
+
+alphazero03: ; set mask is done later
+	MOV		ECX,[EBP+fx]
+	ADD		ECX, [EBP+sdx]
+	MOV		[EBP+fx],ECX
+
+	SUB 		DWORD [EBP+z], 1
+	JNZ 		loop03
+
+endofloop03:
+	MOV		DWORD [EBP+z], 4
+
+loop47:
+	; shift everything left
+	PSLLDQ		XMM6, 4
+
+	PINSRW	XMM7, ECX,0 ; prepare for l,r
+
+	SAR 		ECX, 16
+	CMP 		ECX, 0
+	JE			zerox47
+	JL			negativx47
+	MOV		EDX, [EBP+sw]
+	SUB		EDX, 1
+	CMP		ECX, EDX
+	JGE			biggerx47
+
+okx47:
+	MOV		EDX, ECX
+	ADD		EDX, 1
+	JMP			endbound247
+zerox47:
+	MOV 		ECX, 0
+	MOV		EDX, 1
+	JMP 		endbound247
+
+negativx47:
+	MOV 		ECX, 0
+	MOV		EDX, 0
+	JMP			endbound247
+
+biggerx47:
+	MOV		ECX, EDX
+endbound247:
+	SHL			ECX, 2 						; xadd1
+	SHL			EDX, 2 						; xadd2
+
+	MOV		EAX, [EBP+yadd1]
+	MOV		EBX, [EBP+yadd2]
+
+	MOVD		XMM2, [EBX+EDX]
+	PSLLDQ   	XMM2,4
+	MOVD		XMM1, [EBX+ECX]
+	POR		XMM2,XMM1
+	PSLLDQ   	XMM2,4
+	MOVD		XMM1, [EAX+EDX]
+	POR		XMM2,XMM1
+	PSLLDQ		XMM2,4
+	MOVD		XMM1, [EAX+ECX]
+	POR		XMM2,XMM1
+
+	PEXTRW	EAX,XMM7,0
+	AND		EAX, 0FFFFH
+	PINSRW 	XMM7, EAX,1
+	PINSRW	XMM7, EAX, 3 				;xfright
+
+	NEG		EAX
+	ADD		EAX, 65535
+	PINSRW 	XMM7, EAX, 0
+	PINSRW	XMM7, EAX, 2 				;xfleft
+
+	PSRLW 		XMM7, 1
+
+	MOVDQU	XMM0, XMM2
+	PSRLD		XMM0, 24
+	PXOR		XMM1, XMM1
+
+	MOV		ECX, 0FFH 					; ECX locked for ca
+
+	PINSRW	XMM1, ECX,0
+	PINSRW	XMM1, ECX,2
+	PINSRW	XMM1, ECX,4
+	PINSRW	XMM1, ECX,6
+
+	PCMPEQW	XMM1, XMM0
+
+	PMOVMSKB	EAX, XMM1
+	CMP		EAX, 0FFFFH
+	JE 			endofalpha47
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 				; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   				; XMM3 already shifted by 1
+	PEXTRW	ECX, XMM0, 0
+
+endofalpha47:
+	; alpha done
+	CMP		ECX,0
+	JE			alphazero47
+
+	SHL			ECX, 24
+
+	; calculate red
+
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0, 8
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 				; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   				; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+
+	SHL			EBX,16
+	OR			ECX,EBX
+
+	; red done
+
+	; calculate green
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0, 16
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 				; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   				; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+	SHL 		EBX,8
+	OR 			ECX,EBX
+
+	; green done
+
+	; calculate blue
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0,24
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 				; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   				; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+
+	OR			ECX,EBX
+
+	; blue done
+
+	; put color in correct position
+	MOVD		XMM4,ECX
+	POR		XMM6, XMM4 			; results in XMM6
+
+	; prepared source
+
+alphazero47: ; set mask is done later
+	MOV		ECX,[EBP+fx]
+	ADD		ECX, [EBP+sdx]
+	MOV		[EBP+fx],ECX
+
+	SUB		 DWORD [EBP+z], 1
+	JNZ 		loop47
+
+endofloop47:
+	; all sources calculated, but in reversed order
+	PSHUFD 	XMM2,XMM5, 1AH
+	PSHUFD 	XMM1,XMM6, 1AH
+
+	; now sources ready for further calculation with destination
+	; get alphas
+	MOVQ2DQ	XMM4,  MMX3
+	MOVDQU 	XMM6, XMM2
+	PSHUFD		XMM4, XMM4, 0
+	MOVDQU 	XMM5, XMM1
+	PSLLD 		XMM4, 24
+	PAND 		XMM6, XMM4 			; alpha 5-8 in XMM6
+	PAND 		XMM5, XMM4  			; alpha 1-4 in XMM5
+	PSRLD 		XMM5, 24
+	PSHUFHW 	XMM5, XMM5, 85H
+	PSRLD 		XMM6, 24
+
+	; put both alphas into 1 register
+	PSHUFHW 	XMM6, XMM6, 85H
+	PSHUFLW 	XMM5, XMM5, 85H
+	PSHUFLW 	XMM6, XMM6, 58H
+	PSHUFD		XMM5, XMM5, 0D0H  	; 0102030400000000
+	PSHUFD 	XMM6, XMM6, 5CH 		; 0000000005060708
+	PXOR 		XMM0,XMM0
+	POR		XMM5, XMM6            	; XMM5 = alphas 0102030405060708
+
+	PCMPEQD 	XMM0, XMM5
+	PMOVMSKB EAX, XMM0
+	CMP 		EAX, 0FFFFH				; all alphas = zero; TEST not possible, because only 8 bits compared
+	JE      		endloop
+
+	; mask out alpha = zero
+
+	; fd := 255-ORD(src[a]); fd = XMM4
+	; MOV 	XMM4, 00FF00FF00FF00FF00FF00FF00FF00FFH
+	PXOR 		XMM4, XMM4
+	MOV	 	EAX, 00FFH
+	PINSRW	XMM4, EAX ,0
+	PSHUFLW 	XMM4, XMM4, 0
+	PSHUFD 	XMM4, XMM4, 0
+	PSUBW 		XMM4, XMM5
+	MOV 		EAX,1H
+	PINSRW	XMM3, EAX ,0
+	PSHUFLW 	XMM3, XMM3, 0
+	PSHUFD 	XMM3, XMM3, 0
+	PADDUSW 	XMM4, XMM3
+
+	; new red
+	; calculate red 2
+
+	; get source
+
+	; sred14 = src14 && (srcMask <<16)
+	; srcMask << 16
+	MOVQ2DQ 	XMM3, MMX3
+	PSHUFD 	XMM3, XMM3, 0
+	MOVDQU 	XMM5, XMM1
+	MOVDQU 	XMM6, XMM2
+	PSLLD 		XMM3, 16
+
+	; sred14 = src14 && (srcMask << 24)
+	; src14 must be copied because it mustn't be changed
+	PAND 		XMM5, XMM3 				; sred14
+	PSRLD 		XMM5, 16
+
+	; sred14s = shuffled sred14
+	PSHUFHW 	XMM5, XMM5,85H
+	PAND 		XMM6, XMM3 				; sred58
+	PSRLD 		XMM6, 16
+
+	PSHUFLW 	XMM5, XMM5,85H
+	PSHUFHW 	XMM6, XMM6,85H
+	PSHUFD  	XMM5, XMM5,0D0H 		; sred14s
+	PSHUFLW 	XMM6, XMM6,58H
+	PSHUFD  	XMM6, XMM6,5CH 			; sred58s
+	POR 		XMM5, XMM6 				; sred18
+
+	; sred18255 = sred18 * 256- sred18
+	MOVDQU 	XMM7, XMM5
+	PSLLW 		XMM5, 8
+	PSUBUSW 	XMM5, XMM7 				; sred18255
+
+	; src is now ready
+
+	; destination
+	; dest18 must be copied because it mustn't be changed
+	; Load data into memory
+	MOV 		EDI, [EBP+adr]
+	MOVDQU 	XMM3, [EDI]  				;dest 1-8
+	MOVQ2DQ  XMM6, MMX4
+	PSHUFD 	XMM6, XMM6, 0
+	MOVDQU 	XMM7, XMM3
+
+	PAND 		XMM7, XMM6 				; dred18
+	PSRLW 		XMM7, 8
+	;  dred18alpha = dred18 * negalpha
+	PMULLW 	XMM7, XMM4 				; dred18alpha
+
+	; dest is prepared
+	; combining dest and src
+
+	; dred18big = sred18255 + dred18alpha
+
+	PADDUSW 	XMM7, XMM5 ; dred18big
+	; dred18f = dred18big && destMaskred128  because >> 11 and << 11 is && mask
+	PAND 		XMM7, XMM6 ; dred18f
+
+ 	; dest18nr0 = dest18 && (~destMaskred128)
+ 	PANDN 	XMM6, XMM3  				; dest18nr0
+
+ 	; dest18nrf = dest18nr0 || dred18f
+ 	POR 		XMM6, XMM7
+
+	MOVDQU 	XMM3, XMM6
+
+	; red is calculated
+
+	; calculate green:
+	; get source
+
+	; sgreen14 = src14 && (srcMask <<8)
+	; srcMask << 8
+	MOVQ2DQ 	XMM7, MMX3
+
+	PSHUFD 	XMM7, XMM7, 0
+	MOVDQU 	XMM5, XMM1
+	PSLLD 		XMM7, 8
+	PAND 		XMM5, XMM7 				; sgreen14
+	PSRLD 		XMM5, 8
+
+	; sgreen14s = shuffled sgreen14
+	PSHUFHW 	XMM5, XMM5,85H
+	MOVDQU 	XMM6, XMM2
+	PSHUFLW 	XMM5, XMM5,85H
+	PAND 		XMM6, XMM7 				; sgreen58
+	PSRLD 		XMM6, 8
+	PSHUFD  	XMM5, XMM5,0D0H 		; sgreen14s
+
+	; sgreen58 = src58&& (srcMask << 8)
+	; src58 must be copied because it mustn't be changed
+
+	; sgreen58s = shuffled sgreen58
+	PSHUFHW 	XMM6, XMM6,85H
+	PSHUFLW	XMM6, XMM6,58H
+	PSHUFD  	XMM6, XMM6,5CH 			; sgreen58s
+
+	; sgreen18 = sgreen14s || sgreen58s
+	POR 		XMM5, XMM6 ; sgreen18
+
+	; sgreen18255 = sgreen18 * 256- sgreen18
+	MOVDQU 	XMM7, XMM5
+	MOVQ2DQ	XMM6, MMX5
+
+	PSLLW 		XMM5, 8
+	PSUBUSW 	XMM5, XMM7 				; sgreen18255
+	PSHUFD 	XMM6, XMM6, 0
+
+	MOVDQU 	XMM7, XMM3
+
+	PAND 		XMM7, XMM6 ; dgreen18
+	PSRLW 		XMM7,3
+	;  dgreen18alpha = dgreen18 * negalpha
+	PMULLW 	XMM7, XMM4 				; dgreen18alpha
+
+	; dest is prepared
+	; combining dest and src
+
+	; dgreen18big = sgreen18255 + dgreen18alpha
+	PADDUSW 	XMM7, XMM5 				; dgreen18big
+	PANDN 	XMM6, XMM3  ; dest18ng0
+
+	; dgreen18f = (dgreen18big >> 11) <<5
+	PSRLW 		XMM7, 10 					; dgreen18f
+	PSLLW 		XMM7, 5
+
+ 	; dest18ng0 = dest18 && (~destMaskgreen128)
+
+ 	; dest18ngf = dest18ng0 || dred18f
+ 	POR 		XMM6, XMM7
+	MOVDQU 	XMM3, XMM6
+	; green is calculated
+
+	; calculate blue
+
+	MOV	 	EAX, 001F001FH
+	MOVD		MMX6, EAX
+
+	; get source
+
+	; sblue14 = src14 && (srcMask)
+	; srcMask
+	MOVQ2DQ 	XMM7, MMX3
+	MOVDQU 	XMM5, XMM1
+
+	PSHUFD 	XMM7, XMM7, 0
+	MOVDQU 	XMM6, XMM2
+
+	; sblue14 = src14 && (srcMask)
+	; src14 must be copied because it mustn't be changed
+	PAND 		XMM5, XMM7 				; sblue14
+
+	; sblue14s = shuffled sblue14
+	PSHUFHW 	XMM5, XMM5,85H
+	PAND 		XMM6, XMM7 ; sblue58
+	PSHUFHW 	XMM6, XMM6,85H
+
+	PSHUFLW 	XMM5, XMM5,85H
+	PSHUFLW 	XMM6, XMM6,58H
+
+	PSHUFD  	XMM5, XMM5,0D0H 		; sblue14s
+	PSHUFD  	XMM6, XMM6,5CH 			; sblue58s
+
+	POR 		XMM5, XMM6 				; sblue18
+
+	; sblue18255 = sblue18 * 256- sblue18
+	MOVDQU 	XMM7, XMM5
+	PSLLW 		XMM5, 8
+	PSUBUSW 	XMM5, XMM7 				; sblue18255
+	MOVQ2DQ	XMM6, MMX6
+	PSHUFD 	XMM6, XMM6, 0
+	MOVDQU 	XMM7, XMM3
+	PAND 		XMM7, XMM6 				; dblue18
+	PSLLW 		XMM7, 3
+
+	PMULLW 	XMM7, XMM4 				; dblue18alpha
+
+	; dest is prepared
+	; combining dest and src
+
+	; dblue18big = sblue18255 + dblue18alpha
+
+	PADDUSW 	XMM7, XMM5 				; dblue18big
+	; dblue18f = (dblue18big >> 11)
+	PANDN 	XMM6, XMM3  				; dest18nr0
+ 	PSRLW 		XMM7, 11 					; dblue18f
+
+  	; dest18nr0 = dest18 && (~destMaskblue128)
+
+ 	; dest18nbf = dest18nb0 || dblue18f
+ 	POR 		XMM6, XMM7
+	MOVDQU 	XMM3, XMM6
+
+	; blue is calculated
+
+	; now dest is calculated, store it
+	; get 0 stuff
+
+	MOVDQU	XMM5, [EDI]
+	PAND		XMM5,XMM0
+	PANDN		XMM0, XMM3
+	POR		XMM0, XMM5
+
+	MOVDQU [EDI],XMM0
+
+endloop:
+	;fx already inc  ; by sdx
+	ADD 		EDI, 16
+	MOV 		[EBP+adr],EDI
+	SUB 		DWORD [EBP+x], 8
+	JNZ 		innerloop 					; x>=0
+	JZ 			endxloop
+
+singlepixel: 									; original code from MMXBGRA8888Over565, adjusted to fit this procedure
+	MOV 		EDI, [EBP+adr]
+	MOV	 	EAX, 0000000FFH
+	MOVD		MMX3, EAX
+
+	; dest red -> MMX4
+	MOV	 	EAX, 0F800F800H
+	MOVD		MMX4, EAX
+
+	; dest green -> MMX5
+	MOV	 	EAX, 07E007E0H
+	MOVD		MMX5, EAX
+
+	; dest blue -> MMX6 ; moved as MMX6 is used in singlepixel
+	; MOV	 EAX, 001F001FH
+	; MOVD		MMX6, EAX
+
+	MOV		ECX, [EBP+yfbottom]
+	PINSRW 	XMM3, ECX, 1
+	MOV		ECX, [EBP+yftop]
+	PINSRW 	XMM3, ECX, 0
+	PSRLW 		XMM3,1
+
+	MOV 		ECX, [EBP+fx]
+
+	PINSRW	XMM7, ECX,0 				; prepare for l,r
+
+	SAR 		ECX, 16
+	CMP 		ECX, 0
+	JE			zerox
+	JL			negativx
+	MOV		EDX, [EBP+sw]
+	SUB		EDX, 1
+	CMP		ECX, EDX
+	JGE			biggerx
+
+okx:
+	MOV		EDX, ECX
+	ADD		EDX, 1
+	JMP			endbound2
+zerox:
+	MOV 		ECX, 0
+	MOV		EDX, 1
+	JMP 		endbound2
+
+negativx:
+	MOV 		ECX, 0
+	MOV		EDX, 0
+	JMP			endbound2
+
+biggerx:
+	MOV		ECX, EDX
+endbound2:
+	SHL			ECX, 2 						; xadd1
+	SHL			EDX, 2 						; xadd2
+
+	MOV		EAX, [EBP+yadd1]
+	MOV		EBX, [EBP+yadd2]
+
+	MOVD		XMM2, [EBX+EDX]
+	PSLLDQ   	XMM2,4
+	MOVD		XMM1, [EBX+ECX]
+	POR		XMM2,XMM1
+	PSLLDQ  	XMM2,4
+	MOVD		XMM1, [EAX+EDX]
+	POR		XMM2,XMM1
+	PSLLDQ		XMM2,4
+	MOVD		XMM1, [EAX+ECX]
+	POR		XMM2,XMM1
+
+	PEXTRW	EAX,XMM7,0
+	AND		EAX, 0FFFFH
+	PINSRW 	XMM7, EAX,1
+	PINSRW	XMM7, EAX, 3 				;xfright
+
+	NEG		EAX
+	ADD		EAX, 65535
+	PINSRW 	XMM7, EAX, 0
+	PINSRW	XMM7, EAX, 2 				;xfleft
+
+	PSRLW 		XMM7, 1
+
+	MOVDQU	XMM0, XMM2
+	PSRLD		XMM0, 24
+	PXOR		XMM1, XMM1
+
+	MOV		ECX, 0FFH 					; ECX locked for ca
+
+	PINSRW	XMM1, ECX,0
+	PINSRW	XMM1, ECX,2
+	PINSRW	XMM1, ECX,4
+	PINSRW	XMM1, ECX,6
+
+	PCMPEQW	XMM1, XMM0
+
+	PMOVMSKB	EAX, XMM1
+	CMP		EAX, 0FFFFH
+	JE 			endofalpha
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 					; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   					; XMM3 already shifted by 1
+	PEXTRW	ECX, XMM0, 0
+
+endofalpha:
+	; alpha done
+	CMP		ECX,0
+	JE			alphazero
+
+	; calculate red
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0, 8
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 					; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   					; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+	PINSRW	XMM4, EBX, 4
+	; red done
+
+	; calculate green
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0, 16
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 					; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   					; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+	PINSRW	XMM4, EBX, 2
+
+	; green done
+
+	; calculate blue
+	MOVDQU	XMM0, XMM2
+	PSLLD		XMM0,24
+	PSRLD		XMM0, 24
+
+	PSHUFLW	XMM0, XMM0,58H
+	PSHUFHW	XMM0, XMM0,58H
+	PSHUFD		XMM0,XMM0,58H
+
+	PMADDWD XMM0,XMM7
+	PSRLD		XMM0, 15 					; XMM7 already shifted by 1
+	PSHUFLW	XMM0, XMM0, 58H
+	PMADDWD	XMM0, XMM3
+	PSRLD		XMM0,15   					; XMM3 already shifted by 1
+	PEXTRW 	EBX, XMM0,0
+	PINSRW	XMM4, EBX, 0
+
+	; blue done
+
+	; prepared source
+	CMP		ECX, 0FFH   					; ECX released
+	JE			alpha255
+
+	NEG		ECX
+	ADD		ECX, 0FFH
+	PINSRW	XMM1, ECX, 1  				; 255-ca
+	PINSRW	XMM1, ECX, 3  				; 255-ca
+	PINSRW	XMM1, ECX, 5 				; 255-ca
+
+	MOV		EAX, 0FFH
+	PINSRW	XMM1, EAX, 0 				; 255
+	PINSRW	XMM1, EAX, 2  				; 255
+	PINSRW	XMM1, EAX, 4  				; 255
+
+	;prepare destination
+	MOV		EBX, [EBP+adr]
+
+	MOV		EBX, [EBX]
+
+	MOV		EAX, EBX
+	AND 		EAX, 01FH
+	SHL			EAX,3
+	PINSRW	XMM4, EAX, 1  				; dstb
+
+	MOV		EAX, EBX
+	AND 		EAX, 07E0H
+	SHR		EAX, 3
+	PINSRW	XMM4, EAX, 3  				; dstg
+
+	AND 		EBX, 0F800H
+	SHR		EBX,8
+	PINSRW	XMM4, EBX, 5  				; dstr
+
+	PMADDWD	XMM4, XMM1
+
+	PSRLD		XMM4, 8
+	PXOR		XMM1,XMM1
+	PACKUSWB	XMM4,XMM1
+
+	; put results into their words
+	PEXTRW	EAX, XMM4, 2 				; end red
+	PINSRW	XMM4,  EAX, 4
+
+	PEXTRW	EAX, XMM4, 1 				; end green
+	PINSRW	XMM4,  EAX, 2
+
+alpha255:
+	; red in XMM4,4; green in XMM4, 2; blue in XMM4,0
+	;SYSTEM.PUT16(adr, ASH(cb, -3) + ASH(ASH(cg, -2), 5) + ASH(ASH(cr, -3), 11))
+	PEXTRW	EAX, XMM4, 0 				; end blue
+	SHR		EAX,3
+	AND		EAX, 001FH
+
+	PEXTRW	EBX, XMM4, 2 				; end green
+	SHL			EBX,3
+	AND		EBX, 07E0H
+	OR			EAX, EBX
+
+	PEXTRW	EBX, XMM4, 4				; end red
+	SHL			EBX,8
+	AND		EBX, 0F800H
+	OR			EAX, EBX
+
+	MOV		EDI,[EBP+adr]
+	MOV		[EDI], AX
+
+alphazero: 									; alpha = 0, no writeback
+	MOV		ECX,[EBP+fx]
+	ADD		ECX, [EBP+sdx]
+	MOV		[EBP+fx],ECX
+
+	MOV		EDI,[EBP+adr]
+	ADD		EDI, 2						; inc adr
+	MOV		[EBP+adr],EDI
+
+
+	SUB		DWORD [EBP+x], 1
+	JNZ			innerloop
+
+endxloop:
+	MOV		EAX,[EBP+fy]					; fy := fy + sdy
+	ADD		EAX, [EBP+sdy]
+	MOV		[EBP+fy], EAX
+
+	MOV		EAX,[EBP+yadr]
+	ADD		EAX, [EBP+dstbpr]
+	;MOV	EDI, EAX
+	MOV		[EBP+yadr], EAX
+
+	SUB		DWORD [EBP+y], 1
+	JNZ			outerloop
+
+endyloop:
+	EMMS									; declare FPU registers free
+	POP 		EBX
+	POPFD
+END SSE2Q1BGRA8888BGR565;
+*)
+
+PROCEDURE Q0BGRA8888BGR565(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y : LONGINT; yadr, adr: ADDRESS; col, col0 : LONGINT;
+	cb, cg, cr, ca, dstb, dstg, dstr: LONGINT; yadd: ADDRESS;
+	fx, fy : LONGINT;
+BEGIN
+	fy := sy;
+	yadr := dstadr + dl * 2 + dt * dstbpr;
+	FOR y := dt TO db - 1 DO
+		fx := sx;
+		adr := yadr;
+		yadd := srcadr + (fy DIV 65536) * srcbpr;
+		FOR x := dl TO dr - 1 DO
+			(* destination color *)
+			col := SYSTEM.GET16(adr);
+			dstb := (col MOD 32) * 8; dstg := (col DIV 32 MOD 64) * 4; dstr := (col DIV 2048 MOD 32) * 8;
+
+			col0 := SYSTEM.GET32(yadd + (fx DIV 65536) * 4);
+
+			ca := (col0 DIV 1000000H MOD 100H);
+			IF ca # 0 THEN
+				cb := (col0 MOD 100H);
+				cg := (col0 DIV 100H MOD 100H);
+				cr := (col0 DIV 10000H MOD 100H);
+
+				IF ca # 255 THEN
+					cb := (cb * 256 + (256 - ca) * dstb) DIV 256; IF cb > 256 THEN cb := 256 END;
+					cg := (cg * 256 + (256 - ca) * dstg) DIV 256; IF cg > 256 THEN cg := 256 END;
+					cr := (cr * 256 + (256 - ca) * dstr) DIV 256; IF cr > 256 THEN cr := 256 END
+				END;
+				SYSTEM.PUT16(adr, ASH(cb, -3) + ASH(ASH(cg, -2), 5) + ASH(ASH(cr, -3), 11))
+			END;
+			INC(fx, sdx);
+			INC(adr, 2)
+		END;
+		INC(fy, sdy);
+		INC(yadr, dstbpr)
+	END
+END Q0BGRA8888BGR565;
+
+PROCEDURE Q0BGRA8888BGRA8888(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y : LONGINT; yadr, adr: ADDRESS; col, col0 : LONGINT;
+	cb, cg, cr, ca, dstb, dstg, dstr, dsta : LONGINT; yadd: ADDRESS;
+	fx, fy : LONGINT;
+BEGIN
+	fy := sy;
+	yadr := dstadr + dl * 4 + dt * dstbpr;
+	FOR y := dt TO db - 1 DO
+		fx := sx;
+		adr := yadr;
+		yadd := srcadr + (fy DIV 65536) * srcbpr;
+		FOR x := dl TO dr - 1 DO
+			(* destination color *)
+			col := SYSTEM.GET32(adr);
+			dstb := (col MOD 100H);
+			dstg := (col DIV 100H) MOD 100H;
+			dstr := (col DIV 10000H) MOD 100H;
+			dsta := (col DIV 1000000H) MOD 100H;
+
+			col0 := SYSTEM.GET32(yadd + (fx DIV 65536) * 4);
+
+			ca := (col0 DIV 1000000H MOD 100H);
+			IF ca # 0 THEN
+				cb := (col0 MOD 100H);
+				cg := (col0 DIV 100H MOD 100H);
+				cr := (col0 DIV 10000H MOD 100H);
+
+				IF ca # 255 THEN
+					cb := (cb * 256 + (256 - ca) * dstb) DIV 256; IF cb > 256 THEN cb := 256 END;
+					cg := (cg * 256 + (256 - ca) * dstg) DIV 256; IF cg > 255 THEN cg := 256 END;
+					cr := (cr * 256 + (256 - ca) * dstr) DIV 256; IF cr > 256 THEN cr := 256 END;
+					ca := (ca * 256 + (256 - ca) * dsta) DIV 256; IF ca > 256 THEN ca := 256; END;
+				END;
+
+				SYSTEM.PUT32(adr, cb + LSH(cg, 8) + LSH(cr, 16) + LSH(ca, 24));
+			END;
+			INC(fx, sdx);
+			INC(adr, 4)
+		END;
+		INC(fy, sdy);
+		INC(yadr, dstbpr)
+	END
+END Q0BGRA8888BGRA8888;
+
+PROCEDURE Q0BGRA8888BGRA8888Copy(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y  : LONGINT; yadr, adr: ADDRESS; col, col0 : LONGINT; yadd : ADDRESS;
+	fx, fy : LONGINT;
+BEGIN
+	fy := sy;
+	yadr := dstadr + dl * 4 + dt * dstbpr;
+	FOR y := dt TO db - 1 DO
+		fx := sx;
+		adr := yadr;
+		yadd := srcadr + (fy DIV 65536) * srcbpr;
+		FOR x := dl TO dr - 1 DO
+			col0 := SYSTEM.GET32(yadd + (fx DIV 65536) * 4);
+			SYSTEM.PUT32(adr, col0);
+			INC(fx, sdx);
+			INC(adr, 4)
+		END;
+		INC(fy, sdy);
+		INC(yadr, dstbpr)
+	END
+END Q0BGRA8888BGRA8888Copy;
+
+PROCEDURE Q1BGRA8888BGRA8888(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y, xfleft, xfright, yftop, yfbottom : LONGINT; yadr: ADDRESS; col, col0, col1, col2, col3 : LONGINT;
+	b0, g0, r0, a0, b1, g1, r1, a1, cb, cg, cr, ca, dstb, dstg, dstr, dsta : LONGINT;
+	fx, fy, xadd0, xadd1: LONGINT; yadd0, yadd1: ADDRESS;
+BEGIN
+	yadr := dstadr + dl * 4 + dt * dstbpr;
+	fy := sy - 8000H; sx := sx - 8000H;
+	FOR y := dt TO db - 1 DO
+		fx := sx;
+		dstadr := yadr;
+		yadd0 := srcadr + Bounds(fy DIV 65536, 0, sh - 1) * srcbpr;
+		yadd1 := srcadr + Bounds(fy DIV 65536 + 1, 0, sh - 1) * srcbpr;
+		FOR x := dl TO dr - 1 DO
+			(* destination color *)
+			col := SYSTEM.GET32(dstadr);
+			dstb := col MOD 100H;
+			dstg := col DIV 100H MOD 100H;
+			dstr := col DIV 10000H MOD 100H;
+			dsta := col DIV 1000000H MOD 100H;
+			xadd0 := Bounds(fx DIV 65536, 0, sw - 1) * 4;
+			xadd1 := Bounds(fx DIV 65536 + 1, 0, sw - 1) * 4;
+			col0 := SYSTEM.GET32(yadd0 + xadd0);
+			col1 := SYSTEM.GET32(yadd0 + xadd1);
+			col2 := SYSTEM.GET32(yadd1 + xadd0);
+			col3 := SYSTEM.GET32(yadd1 + xadd1);
+
+			xfleft := (65536 - fx MOD 65536);
+			xfright := (fx MOD 65536);
+			yftop := (65536 - fy MOD 65536);
+			yfbottom := (fy MOD 65536);
+			a0 := ((col0 DIV 1000000H MOD 100H) * xfleft + (col1 DIV 1000000H MOD 100H) * xfright) DIV 65536;
+			a1 := ((col2 DIV 1000000H MOD 100H) * xfleft + (col3 DIV 1000000H MOD 100H) * xfright) DIV 65536;
+			ca := (a0 * yftop + a1 * yfbottom) DIV 65536;
+
+			IF ca # 0 THEN
+				b0 := ((col0 MOD 100H) * xfleft + (col1 MOD 100H) * xfright) DIV 65536;
+				g0 := ((col0 DIV 100H MOD 100H) * xfleft + (col1 DIV 100H MOD 100H) * xfright) DIV 65536;
+				r0 := ((col0 DIV 10000H MOD 100H) * xfleft + (col1 DIV 10000H MOD 100H) * xfright) DIV 65536;
+				a0 := ((col0 DIV 1000000H MOD 100H) * xfleft + (col1 DIV 1000000H MOD 100H) * xfright) DIV 65536;
+
+				b1 := ((col2 MOD 100H) * xfleft + (col3 MOD 100H) * xfright) DIV 65536;
+				g1 := ((col2 DIV 100H MOD 100H) * xfleft + (col3 DIV 100H MOD 100H) * xfright) DIV 65536;
+				r1 := ((col2 DIV 10000H MOD 100H) * xfleft + (col3 DIV 10000H MOD 100H) * xfright) DIV 65536;
+				a1 := ((col2 DIV 1000000H MOD 100H) * xfleft + (col3 DIV 1000000H MOD 100H) * xfright) DIV 65536;
+
+				cb := (b0 * yftop + b1 * yfbottom) DIV 65536;
+				cg := (g0 * yftop + g1 * yfbottom) DIV 65536;
+				cr := (r0 * yftop + r1 * yfbottom) DIV 65536;
+				ca := (a0 * yftop + a1 * yfbottom) DIV 65536;
+				IF ca # 255 THEN
+					cb := (cb * 256 + (256 - ca) * dstb) DIV 256; IF cb > 256 THEN cb := 256 END;
+					cg := (cg * 256 + (256 - ca) * dstg) DIV 256; IF cg > 256 THEN cg := 256 END;
+					cr := (cr * 256 + (256 - ca) * dstr) DIV 256; IF cr > 256 THEN cr := 256 END;
+					ca := (ca * 256 + (256 - ca) * dsta) DIV 256; IF ca > 256 THEN ca := 256; END;
+				END;
+				SYSTEM.PUT32(dstadr, cb + LSH(cg, 8) + LSH(cr, 16) + LSH(ca, 24));
+			END;
+			INC(fx, sdx);
+			INC(dstadr, 4);
+		END;
+		INC(fy, sdy);
+		INC(yadr, dstbpr)
+	END
+END Q1BGRA8888BGRA8888;
+
+PROCEDURE Q1BGRA8888BGRA8888Copy(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y, xfleft, xfright, yftop, yfbottom : LONGINT; yadr: ADDRESS; col, col0, col1, col2, col3 : LONGINT;
+	b0, g0, r0, a0, b1, g1, r1, a1, cb, cg, cr, ca : LONGINT;
+	fx, fy, xadd0, xadd1: LONGINT; yadd0, yadd1: ADDRESS;
+BEGIN
+	yadr := dstadr + dl * 4 + dt * dstbpr;
+	fy := sy - 8000H; sx := sx - 8000H;
+	FOR y := dt TO db - 1 DO
+		fx := sx;
+		dstadr := yadr;
+		yadd0 := srcadr + Bounds(fy DIV 65536, 0, sh - 1) * srcbpr;
+		yadd1 := srcadr + Bounds(fy DIV 65536 + 1, 0, sh - 1) * srcbpr;
+		FOR x := dl TO dr - 1 DO
+			(* destination color *)
+			xadd0 := Bounds(fx DIV 65536, 0, sw - 1) * 4;
+			xadd1 := Bounds(fx DIV 65536 + 1, 0, sw - 1) * 4;
+			col0 := SYSTEM.GET32(yadd0 + xadd0);
+			col1 := SYSTEM.GET32(yadd0 + xadd1);
+			col2 := SYSTEM.GET32(yadd1 + xadd0);
+			col3 := SYSTEM.GET32(yadd1 + xadd1);
+
+			xfleft := (65536 - fx MOD 65536);
+			xfright := (fx MOD 65536);
+			yftop := (65536 - fy MOD 65536);
+			yfbottom := (fy MOD 65536);
+			a0 := ((col0 DIV 1000000H MOD 100H) * xfleft + (col1 DIV 1000000H MOD 100H) * xfright) DIV 65536;
+			a1 := ((col2 DIV 1000000H MOD 100H) * xfleft + (col3 DIV 1000000H MOD 100H) * xfright) DIV 65536;
+			ca := (a0 * yftop + a1 * yfbottom) DIV 65536;
+
+			IF ca # 0 THEN
+				b0 := ((col0 MOD 100H) * xfleft + (col1 MOD 100H) * xfright) DIV 65536;
+				g0 := ((col0 DIV 100H MOD 100H) * xfleft + (col1 DIV 100H MOD 100H) * xfright) DIV 65536;
+				r0 := ((col0 DIV 10000H MOD 100H) * xfleft + (col1 DIV 10000H MOD 100H) * xfright) DIV 65536;
+
+				b1 := ((col2 MOD 100H) * xfleft + (col3 MOD 100H) * xfright) DIV 65536;
+				g1 := ((col2 DIV 100H MOD 100H) * xfleft + (col3 DIV 100H MOD 100H) * xfright) DIV 65536;
+				r1 := ((col2 DIV 10000H MOD 100H) * xfleft + (col3 DIV 10000H MOD 100H) * xfright) DIV 65536;
+
+				cb := (b0 * yftop + b1 * yfbottom) DIV 65536;
+				cg := (g0 * yftop + g1 * yfbottom) DIV 65536;
+				cr := (r0 * yftop + r1 * yfbottom) DIV 65536;
+				SYSTEM.PUT32(dstadr, cb + LSH(cg, 8) + LSH(cr, 16) + LSH(ca, 24));
+			END;
+			INC(fx, sdx);
+			INC(dstadr, 4);
+		END;
+		INC(fy, sdy);
+		INC(yadr, dstbpr)
+	END
+END Q1BGRA8888BGRA8888Copy;
+
+PROCEDURE SSE2Q0BGRA8888BGR565(srcadr, dstadr: ADDRESS; srcbpr, dstbpr, dl, dt, dr, db, sx, sy, sdx, sdy, sw, sh : LONGINT);
+VAR x, y, xfleft, xfright, yftop, yfbottom : LONGINT; yadr, adr, col, col0, col1, col2, col3 : LONGINT;
+	cb, cg, cr, ca, dstb, dstg, dstr, yadd : LONGINT;
+	fx, fy : LONGINT;
+	w : LONGINT;
+END SSE2Q0BGRA8888BGR565;
+
+
+PROCEDURE Scale*(src : Image; sr : Rectangle; dst : Image; dr : Rectangle; clip : Rectangle; copyMode, scaleMode : LONGINT);
+VAR dw, dh, sw, sh : LONGINT;
+	fw, fh : LONGREAL; sx, sy : LONGINT;
+	scaler : ScalerProc; xscaler : XScalerProc;
+	mode : Raster.Mode;
+	SSE2enabled : BOOLEAN;
+BEGIN
+	ASSERT((clip.l >= 0) & (clip.t >= 0) & (clip.r <= dst.width) & (clip.b <= dst.height));
+	ASSERT((sr.l >= 0) & (sr.t >= 0) & (sr.r <= src.width) & (sr.b <= src.height));
+	dw := dr.r - dr.l; dh := dr.b - dr.t;
+	sw := sr.r - sr.l; sh := sr.b - sr.t;
+
+	IF (sw = dw) & (sh = dh) THEN (* optimize special case *)
+		IF ~Rect.IsContained(clip, dr) THEN
+			IF dr.l < clip.l THEN DEC(dw, (clip.l - dr.l)); INC(sr.l, (clip.l - dr.l)); dr.l := clip.l END;
+			IF dr.t < clip.t THEN DEC(dh, (clip.t - dr.t)); INC(sr.t, (clip.t - dr.t)); dr.t := clip.t END;
+			IF dr.r > clip.r THEN DEC(dw, (dr.r - clip.r)) END;
+			IF dr.b > clip.b THEN DEC(dh, (dr.b - clip.b)) END;
+		END;
+		IF (dw > 0) & (dh > 0) THEN
+			IF copyMode = ModeCopy THEN Raster.InitMode(mode, Raster.srcCopy)
+			ELSE Raster.InitMode(mode, Raster.srcOverDst)
+			END;
+			Raster.Copy(src, dst, sr.l, sr.t, sr.l + dw, sr.t + dh, dr.l, dr.t, mode)
+		END;
+		RETURN
+	END;
+
+	fw := sw / dw;
+	fh := sh / dh;
+	sx := sr.l * 65536;
+	sy := sr.t * 65536;
+	(* clipping *)
+	IF ~Rect.IsContained(clip, dr) THEN
+		sw := sr.r - sr.l; sh := sr.b - sr.t;
+		dw := dr.r - dr.l; dh := dr.b - dr.t;
+		IF dr.r > clip.r THEN dr.r := clip.r END;
+		IF dr.b > clip.b THEN dr.b := clip.b END;
+		IF dr.l < clip.l THEN sx := ENTIER(65536 * (sr.l +  sw * (clip.l - dr.l) / dw)); dr.l := clip.l END;
+		IF dr.t < clip.t THEN sy := ENTIER(65536 * (sr.t + sh * (clip.t - dr.t) / dh)); dr.t := clip.t END;
+	END;
+	IF Rect.RectEmpty(dr) THEN RETURN END;
+	xscaler := NIL;
+	SSE2enabled :=Raster.SSE2enabled; (*Machine.SSE2Support; *)
+	(*IF SSE2enabled THEN
+		IF (src.fmt.code = Raster.bgr565) & (dst.fmt.code = Raster.bgr565) THEN
+			IF copyMode = ModeCopy THEN
+				IF scaleMode = 0 THEN xscaler := SSE2Q0BGR565BGR565;
+				ELSIF scaleMode = 1 THEN xscaler:= SSE2Q1BGR565BGR565;
+				END;
+			END;
+		ELSIF (src.fmt.code = Raster.bgra8888) & (dst.fmt.code = Raster.bgr565) THEN
+			IF copyMode = ModeSrcOverDst THEN
+				IF scaleMode = 0 THEN xscaler :=  SSE2Q0BGRA8888BGR565;
+				ELSIF scaleMode = 1 THEN xscaler := SSE2Q1BGRA8888BGR565;
+				END;
+			END;
+		END;
+	END;*)
+	IF (xscaler = NIL) THEN
+		IF (src.fmt.code = Raster.bgr565) & (dst.fmt.code = Raster.bgr565) THEN
+			IF copyMode = ModeCopy THEN
+				IF scaleMode = 0 THEN xscaler := XQ0BGR565BGR565;
+				ELSIF scaleMode = 1 THEN xscaler := Q1BGR565BGR565;
+		 		END;
+			END;
+		ELSIF (src.fmt.code = Raster.bgra8888) & (dst.fmt.code = Raster.bgr565) THEN
+			IF copyMode = ModeSrcOverDst THEN
+				IF scaleMode = 0 THEN xscaler := Q0BGRA8888BGR565;
+				ELSIF scaleMode = 1 THEN xscaler := Q1BGRA8888BGR565;
+				END;
+			END;
+		ELSIF (src.fmt.code = Raster.bgra8888) & (dst.fmt.code = Raster.bgra8888) THEN
+			IF (copyMode = ModeSrcOverDst) THEN
+				IF (scaleMode = 0) THEN xscaler := Q0BGRA8888BGRA8888;
+				ELSIF (scaleMode = 1) THEN xscaler := Q1BGRA8888BGRA8888;
+				END;
+			ELSIF (copyMode = ModeCopy) THEN
+				IF (scaleMode = 0) THEN xscaler := Q0BGRA8888BGRA8888Copy;
+				ELSIF (scaleMode = 1) THEN xscaler := Q1BGRA8888BGRA8888Copy;
+				END;
+			END;
+		END;
+	END;
+
+	IF xscaler # NIL THEN
+		xscaler(src.adr, dst.adr, src.bpr, dst.bpr, dr.l, dr.t, dr.r, dr.b, sx, sy,
+			ENTIER(fw * 65536), ENTIER(fh * 65536), src.width, src.height)
+	ELSE
+		scaler := Q0GenericSrcOverDst; (* fallback case *)
+		IF copyMode = ModeCopy THEN
+			IF scaleMode = 0 THEN scaler := Q0GenericCopy
+			ELSIF scaleMode = 1 THEN scaler := Q1GenericCopy
+			END
+		ELSIF copyMode = ModeSrcOverDst THEN
+			IF scaleMode = 0 THEN scaler := Q0GenericSrcOverDst
+			ELSIF scaleMode = 1 THEN scaler := Q1GenericSrcOverDst
+			END;
+		END;
+		scaler(src, dst, dr, sx, sy, ENTIER(fw * 65536), ENTIER(fh * 65536));
+	END;
+END Scale;
+
+PROCEDURE Bounds(val, min, max : LONGINT) : LONGINT;
+BEGIN
+	IF val < min THEN RETURN min ELSIF val > max THEN RETURN max ELSE RETURN val END
+END Bounds;
+
+END WMRasterScale.
+
+
+SpeedTest.Mod

source/AcAxiDma.Mod

@@ -0,0 +1,132 @@
+(** 
+	AUTHOR: Alexey Morozov, HighDim GmbH, 2013-2018
+	PURPOSE: ActiveCells AXI DMA component driver
+*)
+module AcAxiDma;
+
+import
+	system, AcAxisIo;
+
+const
+	CmdSetAddr = 1*2;
+	CmdSetCount = 2*2;
+	CmdSetBurstLen = 3*2;
+	CmdSetWrapFlag = 4*2;
+	CmdGetAddr = 5*2;
+	CmdGetCount = 6*2;
+	CmdGetPendingFlag = 7*2;
+
+type
+
+	Address = longint;
+
+	Controller* = record
+		cfgCmd-: AcAxisIo.Output;
+		cfgData-: AcAxisIo.Output;
+		status-: AcAxisIo.Input;
+
+		dataSize-: longint; (** size of one data element in bytes *)
+		addr-: Address;
+		count-: Address;
+		maxBurstLen-: longint;
+		burstLen-: longint;
+		wrap-: boolean;
+
+		transferPending: boolean;
+	end;
+	
+	ReadController* = record(Controller)
+	end;
+	
+	WriteController* = record(Controller)
+	end;
+
+	procedure InitController*(var ctl: Controller; cfgCmd, cfgData: AcAxisIo.Output; status: AcAxisIo.Input; dataSize, maxBurstLen: longint);
+	begin
+		ctl.cfgCmd := cfgCmd;
+		ctl.cfgData := cfgData;
+		ctl.status := status;
+		ctl.dataSize := dataSize;
+		ctl.maxBurstLen := maxBurstLen;
+		
+		Stop(ctl);
+		SetCount(ctl,0);
+		SetBurstLen(ctl,maxBurstLen);
+		SetWrap(ctl,false);
+	end InitController;
+
+	procedure SetAddr*(var ctl: Controller; addr: Address);
+	begin
+		ctl.cfgCmd << CmdSetAddr;
+		ctl.cfgData << addr;
+		ctl.addr := addr;
+	end SetAddr;
+
+	procedure SetCount*(var ctl: Controller; count: Address);
+	begin
+		ctl.cfgCmd << CmdSetCount;
+		ctl.cfgData << count;
+		ctl.count := count;
+	end SetCount;
+
+	procedure SetBurstLen*(var ctl: Controller; burstLen: longint);
+	var d: longint;
+	begin
+		d := (burstLen-1) + lsh(burstLen*ctl.dataSize,4);
+		ctl.cfgCmd << CmdSetBurstLen;
+		ctl.cfgData << d;
+		ctl.burstLen := burstLen;
+	end SetBurstLen;
+
+	procedure SetWrap*(var ctl: Controller; wrap: boolean);
+	begin
+		ctl.cfgCmd << CmdSetWrapFlag;
+		if wrap then ctl.cfgData << 1; else ctl.cfgData << 0; end;
+		ctl.wrap := wrap;
+	end SetWrap;
+
+	procedure Start*(var ctl: Controller);
+	begin
+		ctl.cfgCmd << 1;
+		ctl.cfgData << 0;
+		ctl.transferPending := true;
+	end Start;
+
+	procedure Stop*(var ctl: Controller);
+	begin
+		ctl.cfgCmd << 0;
+		ctl.cfgData << 0;
+		ctl.transferPending := false;
+	end Stop;
+
+	procedure GetCurrentAddr*(var ctl: Controller): Address;
+	var addr: Address;
+	begin
+		ctl.cfgCmd << CmdGetAddr;
+		ctl.cfgData << 0;
+		addr << ctl.status;
+		return addr;
+	end GetCurrentAddr;
+
+	procedure GetCurrentCount*(var ctl: Controller): Address;
+	var count: Address;
+	begin
+		ctl.cfgCmd << CmdGetCount;
+		ctl.cfgData << 0;
+		count << ctl.status;
+		return count;
+	end GetCurrentCount;
+
+	procedure TransferPending*(var ctl: Controller): boolean;
+	var d: longint;
+	begin
+		if ctl.transferPending then
+			ctl.cfgCmd << CmdGetPendingFlag;
+			ctl.cfgData << 0;
+			d << ctl.status;
+			ctl.transferPending := d mod 2 = 1;
+		end;
+		return ctl.transferPending;
+	end TransferPending;
+
+end AcAxiDma.

source/AcStreamVideoOut.Mod

@@ -0,0 +1,138 @@
+module AcStreamVideoOut; (** AUTHOR ""; PURPOSE ""; *)
+
+import
+	system, AcAxisIo;
+
+const
+	CmdSetEnabled = 0;
+	CmdSetHorizActiveSize = 1;
+	CmdSetHorizFrontPorch = 2;
+	CmdSetHorizSyncWidth = 3;
+	CmdSetHorizBackPorch = 4;
+	CmdSetHorizSyncPolarity = 5;
+
+	CmdSetVertActiveSize = 6;
+	CmdSetVertFrontPorch = 7;
+	CmdSetVertSyncWidth = 8;
+	CmdSetVertBackPorch = 9;
+	CmdSetVertSyncPolarity = 10;
+
+type
+	Controller* = record
+		cfg-: AcAxisIo.Output; (** configuration port *)
+		enabled-: boolean; (** TRUE if the PWM output is enabled *)
+		
+		pixelClock-: real; (** pixel clock in Hz *)
+
+		horizActiveSize-: longint;
+		horizFrontPorch-: longint;
+		horizSyncWidth-: longint;
+		horizBackPorch-: longint;
+		horizSyncPolarity-: boolean;
+
+		vertActiveSize-: longint;
+		vertFrontPorch-: longint;
+		vertSyncWidth-: longint;
+		vertBackPorch-: longint;
+		vertSyncPolarity-: boolean;
+	end;
+
+	procedure InitController*(var ctl: Controller; cfg: AcAxisIo.Output; pixelClock: real);
+	begin
+		ctl.cfg := cfg;
+		ctl.pixelClock := pixelClock;
+		Enable(ctl,false);
+(*
+		(* setup default video output settings *)
+		SetHorizActiveSize(ctl,1024);z
+		SetHorizFrontPorch(ctl,24);
+		SetHorizSyncWidth(ctl,136);
+		SetHorizBackPorch(ctl,160);
+		SetHorizSyncPolarity(ctl,true);
+
+		SetVertActiveSize(ctl,768);
+		SetVertFrontPorch(ctl,3);
+		SetVertSyncWidth(ctl,6);
+		SetVertBackPorch(ctl,29);
+		SetVertSyncPolarity(ctl,true);
+*)
+	end InitController;
+
+	(**
+		Enable/disable video output
+	*)
+	procedure Enable*(var ctl: Controller; enable: boolean);
+	begin
+		if enable then
+			ctl.cfg << CmdSetEnabled + lsh(1,4);
+		else
+			ctl.cfg << CmdSetEnabled;
+		end;
+		ctl.enabled := enable;
+	end Enable;
+
+	procedure SetHorizActiveSize*(var ctl: Controller; n: longint);
+	begin
+		ctl.cfg << CmdSetHorizActiveSize + lsh(n-1,4);
+		ctl.horizActiveSize := n;
+	end SetHorizActiveSize;
+
+	procedure SetHorizFrontPorch*(var ctl: Controller; n: longint);
+	begin
+		ctl.cfg << CmdSetHorizFrontPorch + lsh(n-1,4);
+		ctl.horizFrontPorch := n;
+	end SetHorizFrontPorch;
+
+	procedure SetHorizSyncWidth*(var ctl: Controller; n: longint);
+	begin
+		ctl.cfg << CmdSetHorizSyncWidth + lsh(n-1,4);
+		ctl.horizSyncWidth := n;
+	end SetHorizSyncWidth;
+
+	procedure SetHorizBackPorch*(var ctl: Controller; n: longint);
+	begin
+		ctl.cfg << CmdSetHorizBackPorch + lsh(n-1,4);
+		ctl.horizBackPorch := n;
+	end SetHorizBackPorch;
+
+	procedure SetHorizSyncPolarity*(var ctl: Controller; polarity: boolean);
+	var n: longint;
+	begin
+		if polarity then n := 1; else n := 0; end;
+		ctl.cfg << CmdSetHorizSyncPolarity + lsh(n,4);
+		ctl.horizSyncPolarity := polarity;
+	end SetHorizSyncPolarity;
+
+	procedure SetVertActiveSize*(var ctl: Controller; n: longint);
+	begin
+		ctl.cfg << CmdSetVertActiveSize + lsh(n-1,4);
+		ctl.vertActiveSize := n;
+	end SetVertActiveSize;
+
+	procedure SetVertFrontPorch*(var ctl: Controller; n: longint);
+	begin
+		ctl.cfg << CmdSetVertFrontPorch + lsh(n-1,4);
+		ctl.vertFrontPorch := n;
+	end SetVertFrontPorch;
+
+	procedure SetVertSyncWidth*(var ctl: Controller; n: longint);
+	begin
+		ctl.cfg << CmdSetVertSyncWidth + lsh(n-1,4);
+		ctl.vertSyncWidth := n;
+	end SetVertSyncWidth;
+
+	procedure SetVertBackPorch*(var ctl: Controller; n: longint);
+	begin
+		ctl.cfg << CmdSetVertBackPorch + lsh(n-1,4);
+		ctl.vertBackPorch := n;
+	end SetVertBackPorch;
+
+	procedure SetVertSyncPolarity*(var ctl: Controller; polarity: boolean);
+	var n: longint;
+	begin
+		if polarity then n := 1; else n := 0; end;
+		ctl.cfg << CmdSetVertSyncPolarity + lsh(n,4);
+		ctl.vertSyncPolarity := polarity;
+	end SetVertSyncPolarity;
+
+end AcStreamVideoOut.

source/BootConfig.Mod

@@ -0,0 +1,170 @@
+MODULE (*Dyn*)BootConfig; (** AUTHOR "Timothée Martiel"; PURPOSE "Boot configuration from memory area"; *)
+
+IMPORT
+	SYSTEM,
+	Initializer(*,
+	Commands*);
+
+CONST
+	ConfigSize = 4096;
+	CR = 0DX;
+	LF = 0AX;
+
+VAR
+	config: POINTER {UNSAFE,UNTRACED} TO ARRAY ConfigSize OF CHAR;
+	size: LONGINT;
+
+	PROCEDURE GetValue * (CONST key: ARRAY OF CHAR; VAR value: ARRAY OF CHAR);
+	VAR
+		i, len: LONGINT;
+	BEGIN
+		i := 0;
+		len := 0;
+		LOOP
+			IF i = size THEN EXIT END;
+			(* is that the key? *)
+			IF MatchKey(config^, i, key) THEN
+				REPEAT INC(i) UNTIL config[i] = '=';
+				INC(i);
+				WHILE (config[i + len] # CR) & (config[i + len] # LF) DO INC(len) END;
+				ASSERT(len <= LEN(value), 7);
+				SYSTEM.MOVE(ADDRESSOF(config[i]), ADDRESSOF(value[0]), len);
+				EXIT
+			ELSE
+				REPEAT INC(i) UNTIL (i = size) OR (config[i] = LF);
+				IF i = size THEN EXIT END;
+				INC(i)
+			END;
+		END;
+		value[len] := 0X;
+	END GetValue;
+
+	PROCEDURE GetIntValue * (CONST key: ARRAY OF CHAR): LONGINT;
+	VAR
+		str: ARRAY 128 OF CHAR;
+	BEGIN
+		GetValue(key, str);
+		RETURN StrToInt(str)
+	END GetIntValue;
+
+	PROCEDURE GetBoolValue * (CONST key: ARRAY OF CHAR): BOOLEAN;
+	VAR
+		str: ARRAY 128 OF CHAR;
+	BEGIN
+		GetValue(key, str);
+		RETURN StrToBool(str)
+	END GetBoolValue;
+
+	PROCEDURE Init *;
+	BEGIN
+		SYSTEM.MOVE(Initializer.configBase, 1FFFF000H, Initializer.configSize);
+		Initializer.configBase := 1FFFF000H;
+		config := Initializer.configBase;
+		size := Initializer.configSize;
+		IF size >= ConfigSize THEN size := ConfigSize-1; END;
+		config[size] := CR;
+	END Init;
+
+	PROCEDURE StrToInt (CONST str: ARRAY OF CHAR): LONGINT;
+	VAR
+		i, value: LONGINT;
+	BEGIN
+		i := 0;
+		value := 0;
+		LOOP
+			IF (i = LEN(str)) OR (str[i] = 0X) THEN RETURN value END;
+			IF (str[i] > '9') OR (str[i] < '0') THEN RETURN 0 END;
+			value := value * 10;
+			INC(value, ORD(str[i]) - ORD('0'));
+			INC(i)
+		END
+	END StrToInt;
+
+	PROCEDURE StrToBool (CONST str: ARRAY OF CHAR): BOOLEAN;
+	BEGIN
+		IF str = '1' THEN RETURN TRUE END;
+		RETURN FALSE
+	END StrToBool;
+
+	(** return TRUE if str1 from ofs1 until the first '=' (excluded) matches str2 *)
+	PROCEDURE MatchKey (CONST str1: ARRAY OF CHAR; ofs1: LONGINT; CONST str2: ARRAY OF CHAR): BOOLEAN;
+	VAR
+		i: LONGINT;
+	BEGIN
+		i := 0;
+		WHILE (str1[ofs1 + i] # '=') & (str2[i] # 0X) & (str1[ofs1 + i] = str2[i]) DO INC(i) END;
+		RETURN (str1[ofs1 + i] = '=') & (str2[i] = 0X)
+	END MatchKey;
+
+(*	PROCEDURE Get * (context: Commands.Context);
+	VAR
+		key, val: ARRAY 128 OF CHAR;
+	BEGIN
+		IF ~context.arg.GetString(key) THEN RETURN END;
+		context.out.String(key);
+		context.out.String(": ");
+		IF context.arg.GetString(val) THEN
+			IF val = "int" THEN
+				context.out.Int(GetIntValue(key), 0)
+			ELSIF val = "bool" THEN
+				IF GetBoolValue(key) THEN
+					context.out.String("TRUE")
+				ELSE
+					context.out.String("FALSE")
+				END
+			ELSE
+				GetValue(key, val);
+				context.out.String("'");
+				context.out.String(val);
+				context.out.String("'");
+			END
+		ELSE
+			GetValue(key, val);
+			context.out.String("'");
+			context.out.String(val);
+			context.out.String("'");
+		END;
+		context.out.Ln
+	END Get;
+
+	PROCEDURE Print * (context: Commands.Context);
+	VAR
+		i: LONGINT;
+	BEGIN
+		FOR i := 0 TO size - 1 DO
+			context.out.Char(config[i])
+		END
+	END Print;*)
+(*BEGIN
+	cfg := "CpuClockHz=666666666
+UartInputClockHz=50000000
+KernelOutputUart=1
+PsRefClockHz=50000000
+KernelOutputUart=1
+CpuNb=2
+UsbPhyRstGpio0=46
+UsbPhyRstGpio1=-1
+UsbEnable0=1
+UsbEnable1=0
+UsbViewportInit=1
+SdClock0=50000000
+SdClock1=50000000
+SdEnable0=1
+SdEnable1=0
+CpuNb=2
+DDRSize=536870912
+ConfigSize=4096;
+HeapSize=469762048
+KernelLoadAdr=1048576
+StackSize=262144
+EnableCaching=1
+EnableKernelWatchdog=1
+EnableFreeLists=0
+EnableReturnBlocks=0
+TraceHeaps=0
+TraceModules=0
+TraceInterrupts=0
+";
+config := ADDRESSOF(cfg[0]);
+size := Strings.Length(cfg)*)
+END (*Dyn*)BootConfig.

source/Clock.Mod

@@ -0,0 +1,123 @@
+(**
+	AUTHOR: Alexey Morozov, HighDim GmbH, 2018
+	PURPOSE: A2 clock with a plugable RTC get/set interface
+*)
+
+(* Aos, Copyright 2001, Pieter Muller, ETH Zurich *)
+
+MODULE Clock;
+
+TYPE
+	
+	(*
+		Function for getting time from an RTC device
+		
+		second: seconds \in [0,59]
+		minute: minutes \in [0,59]
+		hour: hours \in [0,23]
+		day: days \in [1,31]
+		month: months \in [1,12]
+		year: the actual year minus 1900
+	*)
+	GetRtcTimeFunc = PROCEDURE{DELEGATE}(VAR second, minute, hour, day, month: SHORTINT; VAR year: INTEGER): BOOLEAN;
+	
+	(*
+		Function for setting up time on an RTC device
+		
+		second: seconds \in [0,59]
+		minute: minutes \in [0,59]
+		hour: hours \in [0,23]
+		day: days \in [1,31]
+		month: months \in [1,12]
+		year: the actual year minus 1900
+	*)
+	SetRtcTimeFunc =PROCEDURE{DELEGATE}(second, minute, hour, day, month: SHORTINT; year: INTEGER): BOOLEAN;
+
+VAR
+	getRtcTime: GetRtcTimeFunc;
+	setRtcTime: SetRtcTimeFunc;
+
+	tz*: LONGINT;	(** system time zone offset in minutes (from -720 to 720) *)
+	starttime*, startdate*: LONGINT;	(** time this module was loaded (usually boot time) *)
+
+(** Return the current time and date in Oberon format. *)
+
+PROCEDURE Get*(VAR time, date: LONGINT);
+VAR
+	second, minute, hour, day, month: SHORTINT;
+	year: INTEGER;
+BEGIN{EXCLUSIVE}
+	IF getRtcTime # NIL THEN
+		IF getRtcTime(second, minute, hour, day, month, year) THEN
+			time := LONGINT(hour)*4096 + LONGINT(minute)*64 + second;
+			date := LONGINT(year)*512 + LONGINT(month)*32 + day;
+			RETURN;
+		END;
+	END;
+	time := 0;
+	date := 0;
+END Get;
+
+(** Set the current time and date in Oberon format. *)
+
+PROCEDURE Set*(time, date: LONGINT);
+VAR
+	second, minute, hour, day, month: SHORTINT;
+	year: INTEGER;
+BEGIN{EXCLUSIVE}
+	IF setRtcTime # NIL THEN
+		second := SHORTINT(time MOD 64);
+		minute := SHORTINT(time DIV 64 MOD 64);
+		hour := SHORTINT(time DIV 4096 MOD 32);
+		day := SHORTINT(date MOD 32);
+		month := SHORTINT(date DIV 32 MOD 16);
+		year := INTEGER(date DIV 512);
+		IF setRtcTime(second, minute, hour, day, month, year) THEN
+		END;
+	END;
+END Set;
+
+PROCEDURE Install*(get: GetRtcTimeFunc; set: SetRtcTimeFunc);
+BEGIN
+	BEGIN{EXCLUSIVE}
+		getRtcTime := get;
+		setRtcTime := set;
+	END;
+	Get(starttime, startdate);
+END Install;
+
+BEGIN
+	tz := 2*60;	(* fixme: configurable *)
+END Clock.
+
+(*
+23.08.1999	pjm	Split from Aos.Kernel
+*)
+
+(**
+Notes
+
+The time and date are that of the real-time clock of the system, which may be set to universal time, or to some local time zone.
+
+The tz variable indicates the system time zone offset from universal time in minutes.  It may be updated at any time due to daylight savings time.  E.g. MET DST is 2 * 60 = 120.
+
+The time and date are each represented in an encoded LONGINT.
+
+Converting from year, month, day, hour, minute, second to time, date:
+	time := hour*4096 + minute*64 + second;
+	date := (year-1900)*512 + month*32 + day;
+
+Converting from time to hour, minute, second:
+	hour := time DIV 4096 MOD 32;
+	minute := time DIV 64 MOD 64;
+	second := time MOD 64;
+
+Converting from date to year, month, day:
+	year = 1900+date DIV 512;
+	month = date DIV 32 MOD 16;
+	day = date MOD 32;
+
+All years in the current millenium can be represented.  The 1900 offset is a historical artefact from the Oberon system.
+
+Time and date values (respectively) can be compared with the normal Oberon operators <, <=, =, >=, >, #.  Overflow at midnight has to be handled separately.
+*)

source/Release.Tool

@@ -282,13 +282,13 @@ PACKAGE Kernel ARCHIVE "Kernel.zip" SOURCE "KernelSrc.zip" DESCRIPTION "A2 Kerne
 	RPI & COOP { RPI.CPU.Mod }
 	ZYNQ & COOP { Zynq.CPU.Mod }
 	
-	ARM & ~COOP { ZYNQ:/ARM.Initializer.Mod }
-	ZYNQ & ~COOP { ZYNQ:/Zynq.ARM.Platform.Mod }
+	ARM & ~COOP { ARM.Initializer.Mod }
+	ZYNQ & ~COOP { Zynq.ARM.Platform.Mod }
 	
 	# Low-level trace output
 	Trace.Mod
 	
-	ARM & ~COOP{ ZYNQ:/Dynamic.BootConfig.Mod }
+	ARM & ~COOP{ BootConfig.Mod }
 
 	WIN { Windows.Kernel32.Mod }
 
@@ -320,11 +320,11 @@ PACKAGE Kernel ARCHIVE "Kernel.zip" SOURCE "KernelSrc.zip" DESCRIPTION "A2 Kerne
 	
 	ZYNQ & ~COOP {
 		# Trace using PS UART
-		ZYNQ:/Zynq.PsUartMin.Mod
-		ZYNQ:/Zynq.PsTraceDevice.Mod
+		Zynq.PsUartMin.Mod
+		Zynq.PsTraceDevice.Mod
 
-		ZYNQ:/Zynq.PrivateWatchdog.Mod
-		ZYNQ:/ARM.Machine.Mod
+		Zynq.PrivateWatchdog.Mod
+		ARM.Machine.Mod
 	}
 
 	~COOP { Heaps.Mod }
@@ -339,8 +339,8 @@ PACKAGE Kernel ARCHIVE "Kernel.zip" SOURCE "KernelSrc.zip" DESCRIPTION "A2 Kerne
 	COOP { Coop.Objects.Mod Coop.Kernel.Mod }
 	
 	ARM & ~COOP {
-		ZYNQ:/ARM.Objects.Mod
-		ZYNQ:/ARM.Kernel.Mod
+		ARM.Objects.Mod
+		ARM.Kernel.Mod
 	}
 	
 END
@@ -382,7 +382,7 @@ PACKAGE System ARCHIVE "System.zip" SOURCE "SystemSrc.zip" DESCRIPTION "System"
 	BIOS { BIOS.Clock.Mod }
 	WIN { Windows.Clock.Mod }
 	UNIX { Unix.Clock.Mod }
-	NATIVE & ~BIOS { ZYNQ:Clock.Mod}
+	NATIVE & ~BIOS { Clock.Mod}
 
 	# Memory cache control
 	BIOS { BIOS.MemCache.Mod }
@@ -539,11 +539,11 @@ PACKAGE Drivers ARCHIVE "Drivers.zip" SOURCE "DriversSrc.zip" DESCRIPTION "Devic
 	}
 	
 	ZYNQ & ~COOP {
-		ActiveCells/AcDrivers/Zynq.AcAxisIo.Mod
-		ActiveCells/AcDrivers/AcStreamVideoOut.Mod
-		ActiveCells/AcDrivers/AcAxiDma.Mod
-		ZYNQ:Zynq.PsConfig.Mod
-		ZYNQ:/Zynq.DisplayLinear.Mod
+		Zynq.AcAxisIo.Mod
+		AcStreamVideoOut.Mod
+		AcAxiDma.Mod
+		Zynq.PsConfig.Mod
+		Zynq.DisplayLinear.Mod
 	}
 	
 	NATIVE {
@@ -573,10 +573,10 @@ PACKAGE Drivers ARCHIVE "Drivers.zip" SOURCE "DriversSrc.zip" DESCRIPTION "Devic
 	}
 	
 	NATIVE & ARM {
-		USB:/UsbBuffers.Mod
-		USB:/Usbdi.Mod
-		USB:/UsbHcdi.Mod
-		USB:/UsbVarTdAlloc.Mod
+		ARM.UsbBuffers.Mod
+		ARM.Usbdi.Mod
+		ARM.UsbHcdi.Mod
+		ARM.UsbVarTdAlloc.Mod
 	}
 	
 	BIOS {
@@ -589,17 +589,13 @@ PACKAGE Drivers ARCHIVE "Drivers.zip" SOURCE "DriversSrc.zip" DESCRIPTION "Devic
 	}
 	
 	NATIVE & ARM {
-		USB:UsbEhci.Mod
+		ARM.UsbEhci.Mod
 	}
 	
 	BIOS {
 		BIOS.UsbEhciPCI.Mod
 	}
 	
-	NATIVE & ARM {
-		USB:/UsbEhci.Mod
-	}
-	
 	NATIVE {
 		UsbDriverLoader.Mod UsbUtilities.Mod
 	}
@@ -609,12 +605,12 @@ PACKAGE Drivers ARCHIVE "Drivers.zip" SOURCE "DriversSrc.zip" DESCRIPTION "Devic
 	}
 	
 	NATIVE & ARM {
-		USB:/Usb.Mod
-		USB:/UsbHubDriver.Mod
+		ARM.Usb.Mod
+		ARM.UsbHubDriver.Mod
 	}
 	
 	ZYNQ & ~COOP {
-		ZYNQ:/Zynq.Gpio.Mod ZYNQ:/Zynq.UsbEhciPhy.Mod ZYNQ:/UsbEhciZynq.Mod
+		Zynq.Gpio.Mod Zynq.UsbEhciPhy.Mod Zynq.UsbEhciZynq.Mod
 	}
 	
 	NATIVE & ~ARM {
@@ -624,7 +620,7 @@ PACKAGE Drivers ARCHIVE "Drivers.zip" SOURCE "DriversSrc.zip" DESCRIPTION "Devic
 	}
 	
 	NATIVE & ARM {
-		USB:/UsbStorageBase.Mod USB:/UsbStorageBot.Mod USB:/UsbStorageCbi.Mod USB:/UsbStorageScm.Mod
+		ARM.UsbStorageBase.Mod ARM.UsbStorageBot.Mod ARM.UsbStorageCbi.Mod ARM.UsbStorageScm.Mod
 	}
 	
 	NATIVE {
@@ -637,7 +633,7 @@ PACKAGE Drivers ARCHIVE "Drivers.zip" SOURCE "DriversSrc.zip" DESCRIPTION "Devic
 	}
 	
 	NATIVE & ARM {
-		USB:UsbHid.Mod
+		ARM.UsbHid.Mod
 	}
 	
 	NATIVE & ~ARM {
@@ -650,7 +646,7 @@ PACKAGE Drivers ARCHIVE "Drivers.zip" SOURCE "DriversSrc.zip" DESCRIPTION "Devic
 	BIOS & I386 { I386.UsbKeyboard.Mod }
 	BIOS & AMD64 { AMD64.UsbKeyboard.Mod }
 	
-	NATIVE & ARM { ZYNQ:/ARM.UsbKeyboard.Mod }
+	NATIVE & ARM { ARM.UsbKeyboard.Mod }
 
 	NATIVE {
 		UsbHidUP.Mod UsbHidErrors.Mod UsbHidReport.Mod
@@ -662,7 +658,7 @@ PACKAGE Drivers ARCHIVE "Drivers.zip" SOURCE "DriversSrc.zip" DESCRIPTION "Devic
 	}
 	
 	NATIVE & ARM {
-		USB:UsbHidDriver.Mod
+		ARM.UsbHidDriver.Mod
 	}
 	
 	NATIVE & ~ARM {
@@ -770,7 +766,7 @@ PACKAGE ApplicationsMini ARCHIVE "ApplicationsMini.zip" SOURCE "ApplicationsMini
 	# raster images (eos)
 	I386 { I386.Raster.Mod }
 	AMD64 { AMD64.Raster.Mod }
-	ARM { ZYNQ:ARM.Raster.Mod }
+	ARM { ARM.Raster.Mod }
 
 	Localization.Mod
 	Archives.Mod
@@ -804,7 +800,7 @@ PACKAGE ApplicationsMini ARCHIVE "ApplicationsMini.zip" SOURCE "ApplicationsMini
 	# Move to graphical package!!
 	I386 { I386.WMRasterScale.Mod }
 	AMD64 { AMD64.WMRasterScale.Mod }
-	ARM { ZYNQ:/ARM.WMRasterScale.Mod }
+	ARM { ARM.WMRasterScale.Mod }
 
 	WMGraphics.Mod
 	WMGraphicsSmooth.Mod

source/Zynq.ARM.Platform.Mod

@@ -0,0 +1,510 @@
+MODULE Platform;
+(** Zynq platform-specific constants for A2.
+	Author: Timothee Martiel
+*)
+
+CONST
+	(* ===== Units ===== *)
+	k *		= 1024;
+	M *	= k * k;
+	G *		= k * k * k;
+
+	(* Maximum number of CPUs for that platform *)
+	MaxCpuNb * = 2;
+
+	(* ===== Physical Memory Layout ===== *)
+	DDRStart * = 0;
+	OCMStart * = 0;
+	OCMSize * = 192 * k;
+	IOStart * = 1 * G;
+	IOSize * = 3040 * M;
+
+	(* ===== Registers ===== *)
+	(* ===== MPCORE registers and related devices ===== *)
+	PrivateWatchdogIrq * = 30;
+	PrivateWatchdogDisableKey0 * = 12345678H;
+	PrivateWatchdogDisableKey1 * = 87654321H;
+
+	MpcoreBase* = ADDRESS(0F8F00000H);
+	ICDDCR* = MpcoreBase + 1000H; (* distributor control register *)
+	ICDICFR* = MpcoreBase + 1C00H; (* interrupt configuration register *)
+	ICCICR* = MpcoreBase + 0100H;  (* cpu interface control register *)
+	ICCPMR* = MpcoreBase + 0104H; (* interrupt priority mask register *)
+	ICCEOIR* = MpcoreBase + 0110H; (* end of interrupt register *)
+	ICDIPTR* = MpcoreBase + 1800H; (* interrupt processor target registers *)
+	ICDIPR* =   MpcoreBase + 1400H; (* interrupt priority registers *)
+	ICDICER* =  MpcoreBase + 1180H; (* interrupt clear enable register *)
+	ICDISER* = MpcoreBase + 1100H; (* interrupt set enable register *)
+	ICDISPR* = MpcoreBase + 1200H; (* set pending register *)
+	ICDICPR* = MpcoreBase + 1280H; (* clear pending register *)
+	ICCIAR* = MpcoreBase + 10CH; (* Interrupt Ackknowledge Register *)
+	ICCBPR* = MpcoreBase + 108H; (* Binary Point Register *)
+	GlobalTimerCounterRegister0* = MpcoreBase + 200H;
+	GlobalTimerCounterRegister1* = MpcoreBase + 204H;
+	GlobalTimerControlRegister* = MpcoreBase + 208H;
+	GlobalTimerInterruptStatusRegister* = MpcoreBase+ 20CH;
+	ComparatorValueRegister0* = MpcoreBase + 210H;
+	ComparatorValueRegister1* = MpcoreBase + 214H;
+	AutoIncrementRegister* = MpcoreBase + 218H;
+
+	PrivateLoadValueRegister* = MpcoreBase + 600H;
+	PrivateTimerCounterRegister* = MpcoreBase + 604H;
+	PrivateTimerControlRegister* = MpcoreBase + 608H;
+	PrivateTimerInterruptStatusRegister* = MpcoreBase+ 60CH;
+
+	SCUControlReg* = MpcoreBase;
+
+	(* Interrupt constants *)
+	(*! TODO: USEFUL? *)
+	ICIP* = (* TODO *) 0H;
+	ICMR* = (* TODO *) 0H;
+	ICLR* = (* TODO *) 0H;
+	InterruptVectors* = 0H;
+	FIQMode* 		= 11H;   (* Fast Interrupt Mode *)
+	IRQMode* 		= 12H;   (* Interrupt Mode *)
+	SVCMode* 		= 13H;   (* SuperVisorMode *)
+	UndefMode*	= 1BH;   (* Undefined instruction mode *)
+	UserMode* 		= 10H;   (* User mode *)
+	AbortMode* 	= 17H;   (* Abort mode *)
+	SystemMode*	= 1FH;
+	(* Exceptions *)
+	FIQDisabled* 	= 40H;   (* Fast interrupts disabled globally *)
+	IRQDisabled* 	= 80H;   (* Interrupts disabled globally *)
+	
+	(* ===== System Level Control register access ===== *)
+	SlcrBase* = 0xF8000000; (** Base address for System Level Control (SLCR) registers *)
+	SlcrLockKey* = 0x767B; (** SLCR lock key; write this value to SLCR_LOCK register to disable writing to SLCR registers *)
+	SlcrUnlockKey* = 0xDF0D; (** SLCR unlock key; write this value to SLCR_UNLOCK register to enable writing to SLCR registers *)
+
+	(* ===== System Watchdog registers ===== *)
+	SWDTBase = ADDRESS(0'F800'5000H);
+
+	(* ===== Device Configuration registers ===== *)
+	DevCfgBase * = 0F8007000H;					(** Base address of the devcfg registers *)
+	DevCfgDmaPlAdr * = ADDRESS(0FFFFFFFFH);	(** Devcfg DMA address of the PL *)
+	DevCfgMctrlOffset * = 80H;						(** Offset of the MCTRL register; needed for early initialization in zynq bootloader *)
+	DevCfgMctrlPsVersionOfs * = 28;
+	DevCfgMctrlPsVersion10 * = 0;
+	DevCfgMctrlPsVersion20 * = 1;
+	DevCfgMctrlPsVersion30 * = 2;
+	DevCfgMctrlPsVersion31 * = 3;
+
+	(* ===== AXI interfaces ===== *)
+	MasterAxiGp0Base* = 07F000000H; (** base address for Master AXI General Purpose interface 0 *)
+	MasterAxiGp1Base* = 0BF000000H; (** base address for Master AXI General Purpose interface 1 *)
+
+	(* ===== GPIO Registers ===== *)
+	GpioBase * = ADDRESS(0E000A000H); (** Base address of GPIO registers *)
+	GpioData * = GpioBase + 40H; (** Base address of GPIO data registers *)
+	GpioBankNb * = 4; (** Number of GPIO banks *)
+	GpioBank * = [GpioBase + 204H, GpioBase + 244H, GpioBase + 284H, GpioBase + 2C4H]; (** Base address of GPIO bank control registers *)
+
+	(* ===== UART ===== *)
+	UartBase* = [ADDRESS(0E0000000H),ADDRESS(0E0001000H)]; (* base address for all UART controllers present in the system *)
+	UartModemPinsConnected* = [FALSE,FALSE];
+	UartIrq * = [59,82];
+
+	(* ===== USB ===== *)
+	UsbNb * = 2; (** Number of USB controllers *)
+	UsbBase * = [ADDRESS(0E0002100H), ADDRESS(0E0003100H)]; (** Base addresses of USB controllers: CAPLENGTH must be at offset 0. *)
+	UsbIrq * = [53, 76]; (** USB IRQs *)
+
+	(* ===== SD ===== *)
+	SdNb * = 2;
+	SdBase * = [ADDRESS(0E0100000H), ADDRESS(0E0101000H)];
+	SdIrq * = [56, 79];
+
+	RAMCore * = 100000H;
+	RAMLogicalLow * = 0H;
+	RAMLogicalHigh * = ADDRESS(10000000H);
+	RAMDiskBase * = 100000H;
+	RAMDiskSize * = 140 * 1024;
+	OFSBlockSize * = 4096;
+	HostPort * = '';
+	ModRoot * = 21318H;
+
+TYPE
+	
+	(** System Level Control (SLCR) registers definition *)
+	SlcrRegisters* = POINTER{UNSAFE,UNTRACED} TO RECORD
+		SCL*: LONGINT; (** 0x00000000 32 rw 0x00000000 Secure Configuration Lock *)
+		SLCR_LOCK*: LONGINT; (** 0x00000004 32 wo 0x00000000 SLCR Write Protection Lock *)
+		SLCR_UNLOCK*: LONGINT; (** 0x00000008 32 wo 0x00000000 SLCR Write Protection Unlock *)
+		SLCR_LOCKSTA-: LONGINT; (** 0x0000000C 32 ro 0x00000001 SLCR Write Protection Status *)
+		padding0: ARRAY 60 OF LONGINT;
+		ARM_PLL_CTRL*: LONGINT; (** 0x00000100 32 rw 0x0001A008 ARM PLL Control *)
+		DDR_PLL_CTRL*: LONGINT; (** 0x00000104 32 rw 0x0001A008 DDR PLL Control *)
+		IO_PLL_CTRL*: LONGINT; (** 0x00000108 32 rw 0x0001A008 IO PLL Control *)
+		PLL_STATUS-: LONGINT; (** 0x0000010C 32 ro 0x0000003F PLL Status *)
+		ARM_PLL_CFG*: LONGINT; (** 0x00000110 32 rw 0x00177EA0 ARM PLL Configuration *)
+		DDR_PLL_CFG*: LONGINT; (** 0x00000114 32 rw 0x00177EA0 DDR PLL Configuration *)
+		IO_PLL_CFG*: LONGINT; (** 0x00000118 32 rw 0x00177EA0 IO PLL Configuration *)
+		padding01: LONGINT;
+		ARM_CLK_CTRL*: LONGINT; (** 0x00000120 32 rw 0x1F000400 CPU Clock Control *)
+		DDR_CLK_CTRL*: LONGINT; (** 0x00000124 32 rw 0x18400003 DDR Clock Control *)
+		DCI_CLK_CTRL*: LONGINT; (** 0x00000128 32 rw 0x01E03201 DCI clock control *)
+		APER_CLK_CTRL*: LONGINT; (** 0x0000012C 32 rw 0x01FFCCCD AMBA Peripheral Clock Control *)
+		USB0_CLK_CTRL*: LONGINT; (** 0x00000130 32 rw 0x00101941 USB 0 ULPI Clock Control *)
+		USB1_CLK_CTRL*: LONGINT; (** 0x00000134 32 rw 0x00101941 USB 1 ULPI Clock Control *)
+		GEM0_RCLK_CTRL*: LONGINT; (** 0x00000138 32 rw 0x00000001 GigE 0 Rx Clock and Rx Signals Select *)
+		GEM1_RCLK_CTRL*: LONGINT; (** 0x0000013C 32 rw 0x00000001 GigE 1 Rx Clock and Rx Signals Select *)
+		GEM0_CLK_CTRL*: LONGINT; (** 0x00000140 32 rw 0x00003C01 GigE 0 Ref Clock Control *)
+		GEM1_CLK_CTRL*: LONGINT; (** 0x00000144 32 rw 0x00003C01 GigE 1 Ref Clock Control *)
+		SMC_CLK_CTRL*: LONGINT; (** 0x00000148 32 rw 0x00003C21 SMC Ref Clock Control *)
+		LQSPI_CLK_CTRL*: LONGINT; (** 0x0000014C 32 rw 0x00002821 Quad SPI Ref Clock Control *)
+		SDIO_CLK_CTRL*: LONGINT; (** 0x00000150 32 rw 0x00001E03 SDIO Ref Clock Control *)
+		UART_CLK_CTRL*: LONGINT; (** 0x00000154 32 rw 0x00003F03 UART Ref Clock Control *)
+		SPI_CLK_CTRL*: LONGINT; (** 0x00000158 32 rw 0x00003F03 SPI Ref Clock Control *)
+		CAN_CLK_CTRL*: LONGINT; (** 0x0000015C 32 rw 0x00501903 CAN Ref Clock Control *)
+		CAN_MIOCLK_CTRL*: LONGINT; (** 0x00000160 32 rw 0x00000000 CAN MIO Clock Control *)
+		DBG_CLK_CTRL*: LONGINT; (** 0x00000164 32 rw 0x00000F03 SoC Debug Clock Control *)
+		PCAP_CLK_CTRL*: LONGINT; (** 0x00000168 32 rw 0x00000F01 PCAP Clock Control *)
+		TOPSW_CLK_CTRL*: LONGINT; (** 0x0000016C 32 rw 0x00000000 Central Interconnect Clock Control *)
+		FPGA0_CLK_CTRL*: LONGINT; (** 0x00000170 32 rw 0x00101800 PL Clock 0 Output control *)
+		FPGA0_THR_CTRL*: LONGINT; (** 0x00000174 32 rw 0x00000000 PL Clock 0 Throttle control *)
+		FPGA0_THR_CNT*: LONGINT; (** 0x00000178 32 rw 0x00000000 PL Clock 0 Throttle Count control *)
+		FPGA0_THR_STA-: LONGINT; (** 0x0000017C 32 ro 0x00010000 PL Clock 0 Throttle Status read *)
+		FPGA1_CLK_CTRL*: LONGINT; (** 0x00000180 32 rw 0x00101800 PL Clock 1 Output control *)
+		FPGA1_THR_CTRL*: LONGINT; (** 0x00000184 32 rw 0x00000000 PL Clock 1 Throttle control *)
+		FPGA1_THR_CNT*: LONGINT; (** 0x00000188 32 rw 0x00000000 PL Clock 1 Throttle Count *)
+		FPGA1_THR_STA-: LONGINT; (** 0x0000018C 32 ro 0x00010000 PL Clock 1 Throttle Status control *)
+		FPGA2_CLK_CTRL*: LONGINT; (** 0x00000190 32 rw 0x00101800 PL Clock 2 output control *)
+		FPGA2_THR_CTRL*: LONGINT; (** 0x00000194 32 rw 0x00000000 PL Clock 2 Throttle Control *)
+		FPGA2_THR_CNT*: LONGINT; (** 0x00000198 32 rw 0x00000000 PL Clock 2 Throttle Count *)
+		FPGA2_THR_STA-: LONGINT; (** 0x0000019C 32 ro 0x00010000 PL Clock 2 Throttle Status *)
+		FPGA3_CLK_CTRL*: LONGINT; (** 0x000001A0 32 rw 0x00101800 PL Clock 3 output control *)
+		FPGA3_THR_CTRL*: LONGINT; (** 0x000001A4 32 rw 0x00000000 PL Clock 3 Throttle Control *)
+		FPGA3_THR_CNT*: LONGINT; (** 0x000001A8 32 rw 0x00000000 PL Clock 3 Throttle Count *)
+		FPGA3_THR_STA-: LONGINT; (** 0x000001AC 32 ro 0x00010000 PL Clock 3 Throttle Status *)
+		padding1: ARRAY 5 OF LONGINT;
+		CLK_621_TRUE*: LONGINT; (** 0x000001C4 32 rw 0x00000001 CPU Clock Ratio Mode select *)
+		padding2: ARRAY 14 OF LONGINT;
+		PSS_RST_CTRL*: LONGINT; (** 0x00000200 32 rw 0x00000000 PS Software Reset Control *)
+		DDR_RST_CTRL*: LONGINT; (** 0x00000204 32 rw 0x00000000 DDR Software Reset Control *)
+		TOPSW_RST_CTRL*: LONGINT; (** 0x00000208 32 rw 0x00000000 Central Interconnect Reset Control *)
+		DMAC_RST_CTRL*: LONGINT; (** 0x0000020C 32 rw 0x00000000 DMAC Software Reset Control *)
+		USB_RST_CTRL*: LONGINT; (** 0x00000210 32 rw 0x00000000 USB Software Reset Control *)
+		GEM_RST_CTRL*: LONGINT; (** 0x00000214 32 rw 0x00000000 Gigabit Ethernet SW Reset Control *)
+		SDIO_RST_CTRL*: LONGINT; (** 0x00000218 32 rw 0x00000000 SDIO Software Reset Control *)
+		SPI_RST_CTRL*: LONGINT; (** 0x0000021C 32 rw 0x00000000 SPI Software Reset Control *)
+		CAN_RST_CTRL*: LONGINT; (** 0x00000220 32 rw 0x00000000 CAN Software Reset Control *)
+		I2C_RST_CTRL*: LONGINT; (** 0x00000224 32 rw 0x00000000 I2C Software Reset Control *)
+		UART_RST_CTRL*: LONGINT; (** 0x00000228 32 rw 0x00000000 UART Software Reset Control *)
+		GPIO_RST_CTRL*: LONGINT; (** 0x0000022C 32 rw 0x00000000 GPIO Software Reset Control *)
+		LQSPI_RST_CTRL*: LONGINT; (** 0x00000230 32 rw 0x00000000 Quad SPI Software Reset Control *)
+		SMC_RST_CTRL*: LONGINT; (** 0x00000234 32 rw 0x00000000 SMC Software Reset Control *)
+		OCM_RST_CTRL*: LONGINT; (** 0x00000238 32 rw 0x00000000 OCM Software Reset Control *)
+		padding3: LONGINT;
+		FPGA_RST_CTRL*: SET; (** 0x00000240 32 rw 0x01F33F0F FPGA Software Reset Control *)
+		A9_CPU_RST_CTRL*: LONGINT; (** 0x00000244 32 rw 0x00000000 CPU Reset and Clock control *)
+		padding4: LONGINT;
+		RS_AWDT_CTRL*: LONGINT; (** 0x0000024C 32 rw 0x00000000 Watchdog Timer Reset Control *)
+		padding5: ARRAY 2 OF LONGINT;
+		REBOOT_STATUS*: LONGINT; (** 0x00000258 32 rw 0x00400000 Reboot Status, persistent *)
+		BOOT_MODE*: LONGINT; (** 0x0000025C 32 mixed x Boot Mode Strapping Pins *)
+		padding6: ARRAY 40 OF LONGINT;
+		APU_CTRL*: LONGINT; (** 0x00000300 32 rw 0x00000000 APU Control *)
+		WDT_CLK_SEL*: LONGINT; (** 0x00000304 32 rw 0x00000000 SWDT clock source select *)
+		padding7: ARRAY 78 OF LONGINT;
+		TZ_DMA_NS*: LONGINT; (** 0x00000440 32 rw 0x00000000 DMAC TrustZone Config *)
+		TZ_DMA_IRQ_NS*: LONGINT; (** 0x00000444 32 rw 0x00000000 DMAC TrustZone Config for Interrupts *)
+		TZ_DMA_PERIPH_NS*: LONGINT; (** 0x00000448 32 rw 0x00000000 DMAC TrustZone Config for Peripherals *)
+		padding8: ARRAY 57 OF LONGINT;
+		PSS_IDCODE-: LONGINT; (** 0x00000530 32 ro x PS IDCODE *)
+		padding9: ARRAY 51 OF LONGINT;
+		DDR_URGENT*: LONGINT; (** 0x00000600 32 rw 0x00000000 DDR Urgent Control *)
+		padding10: ARRAY 2 OF LONGINT;
+		DDR_CAL_START*: LONGINT; (** 0x0000060C 32 mixed 0x00000000 DDR Calibration Start Triggers *)
+		padding11: LONGINT;
+		DDR_REF_START*: LONGINT; (** 0x00000614 32 mixed 0x00000000 DDR Refresh Start Triggers *)
+		DDR_CMD_STA*: LONGINT; (** 0x00000618 32 mixed 0x00000000 DDR Command Store Status *)
+		DDR_URGENT_SEL*: LONGINT; (** 0x0000061C 32 rw 0x00000000 DDR Urgent Select *)
+		DDR_DFI_STATUS*: LONGINT; (** 0x00000620 32 mixed 0x00000000 DDR DFI status *)
+		padding12: ARRAY 55 OF LONGINT;
+		MIO_PIN_00*: LONGINT; (** 0x00000700 32 rw 0x00001601 MIO Pin 0 Control *)
+		MIO_PIN_01*: LONGINT; (** 0x00000704 32 rw 0x00001601 MIO Pin 1 Control *)
+		MIO_PIN_02*: LONGINT; (** 0x00000708 32 rw 0x00000601 MIO Pin 2 Control *)
+		MIO_PIN_03*: LONGINT; (** 0x0000070C 32 rw 0x00000601 MIO Pin 3 Control *)
+		MIO_PIN_04*: LONGINT; (** 0x00000710 32 rw 0x00000601 MIO Pin 4 Control *)
+		MIO_PIN_05*: LONGINT; (** 0x00000714 32 rw 0x00000601 MIO Pin 5 Control *)
+		MIO_PIN_06*: LONGINT; (** 0x00000718 32 rw 0x00000601 MIO Pin 6 Control *)
+		MIO_PIN_07*: LONGINT; (** 0x0000071C 32 rw 0x00000601 MIO Pin 7 Control *)
+		MIO_PIN_08*: LONGINT; (** 0x00000720 32 rw 0x00000601 MIO Pin 8 Control *)
+		MIO_PIN_09*: LONGINT; (** 0x00000724 32 rw 0x00001601 MIO Pin 9 Control *)
+		MIO_PIN_10*: LONGINT; (** 0x00000728 32 rw 0x00001601 MIO Pin 10 Control *)
+		MIO_PIN_11*: LONGINT; (** 0x0000072C 32 rw 0x00001601 MIO Pin 11 Control *)
+		MIO_PIN_12*: LONGINT; (** 0x00000730 32 rw 0x00001601 MIO Pin 12 Control *)
+		MIO_PIN_13*: LONGINT; (** 0x00000734 32 rw 0x00001601 MIO Pin 13 Control *)
+		MIO_PIN_14*: LONGINT; (** 0x00000738 32 rw 0x00001601 MIO Pin 14 Control *)
+		MIO_PIN_15*: LONGINT; (** 0x0000073C 32 rw 0x00001601 MIO Pin 15 Control *)
+		MIO_PIN_16*: LONGINT; (** 0x00000740 32 rw 0x00001601 MIO Pin 16 Control *)
+		MIO_PIN_17*: LONGINT; (** 0x00000744 32 rw 0x00001601 MIO Pin 17 Control *)
+		MIO_PIN_18*: LONGINT; (** 0x00000748 32 rw 0x00001601 MIO Pin 18 Control *)
+		MIO_PIN_19*: LONGINT; (** 0x0000074C 32 rw 0x00001601 MIO Pin 19 Control *)
+		MIO_PIN_20*: LONGINT; (** 0x00000750 32 rw 0x00001601 MIO Pin 20 Control *)
+		MIO_PIN_21*: LONGINT; (** 0x00000754 32 rw 0x00001601 MIO Pin 21 Control *)
+		MIO_PIN_22*: LONGINT; (** 0x00000758 32 rw 0x00001601 MIO Pin 22 Control *)
+		MIO_PIN_23*: LONGINT; (** 0x0000075C 32 rw 0x00001601 MIO Pin 23 Control *)
+		MIO_PIN_24*: LONGINT; (** 0x00000760 32 rw 0x00001601 MIO Pin 24 Control *)
+		MIO_PIN_25*: LONGINT; (** 0x00000764 32 rw 0x00001601 MIO Pin 25 Control *)
+		MIO_PIN_26*: LONGINT; (** 0x00000768 32 rw 0x00001601 MIO Pin 26 Control *)
+		MIO_PIN_27*: LONGINT; (** 0x0000076C 32 rw 0x00001601 MIO Pin 27 Control *)
+		MIO_PIN_28*: LONGINT; (** 0x00000770 32 rw 0x00001601 MIO Pin 28 Control *)
+		MIO_PIN_29*: LONGINT; (** 0x00000774 32 rw 0x00001601 MIO Pin 29 Control *)
+		MIO_PIN_30*: LONGINT; (** 0x00000778 32 rw 0x00001601 MIO Pin 30 Control *)
+		MIO_PIN_31*: LONGINT; (** 0x0000077C 32 rw 0x00001601 MIO Pin 31 Control *)
+		MIO_PIN_32*: LONGINT; (** 0x00000780 32 rw 0x00001601 MIO Pin 32 Control *)
+		MIO_PIN_33*: LONGINT; (** 0x00000784 32 rw 0x00001601 MIO Pin 33 Control *)
+		MIO_PIN_34*: LONGINT; (** 0x00000788 32 rw 0x00001601 MIO Pin 34 Control *)
+		MIO_PIN_35*: LONGINT; (** 0x0000078C 32 rw 0x00001601 MIO Pin 35 Control *)
+		MIO_PIN_36*: LONGINT; (** 0x00000790 32 rw 0x00001601 MIO Pin 36 Control *)
+		MIO_PIN_37*: LONGINT; (** 0x00000794 32 rw 0x00001601 MIO Pin 37 Control *)
+		MIO_PIN_38*: LONGINT; (** 0x00000798 32 rw 0x00001601 MIO Pin 38 Control *)
+		MIO_PIN_39*: LONGINT; (** 0x0000079C 32 rw 0x00001601 MIO Pin 39 Control *)
+		MIO_PIN_40*: LONGINT; (** 0x000007A0 32 rw 0x00001601 MIO Pin 40 Control *)
+		MIO_PIN_41*: LONGINT; (** 0x000007A4 32 rw 0x00001601 MIO Pin 41 Control *)
+		MIO_PIN_42*: LONGINT; (** 0x000007A8 32 rw 0x00001601 MIO Pin 42 Control *)
+		MIO_PIN_43*: LONGINT; (** 0x000007AC 32 rw 0x00001601 MIO Pin 43 Control *)
+		MIO_PIN_44*: LONGINT; (** 0x000007B0 32 rw 0x00001601 MIO Pin 44 Control *)
+		MIO_PIN_45*: LONGINT; (** 0x000007B4 32 rw 0x00001601 MIO Pin 45 Control *)
+		MIO_PIN_46*: LONGINT; (** 0x000007B8 32 rw 0x00001601 MIO Pin 46 Control *)
+		MIO_PIN_47*: LONGINT; (** 0x000007BC 32 rw 0x00001601 MIO Pin 47 Control *)
+		MIO_PIN_48*: LONGINT; (** 0x000007C0 32 rw 0x00001601 MIO Pin 48 Control *)
+		MIO_PIN_49*: LONGINT; (** 0x000007C4 32 rw 0x00001601 MIO Pin 49 Control *)
+		MIO_PIN_50*: LONGINT; (** 0x000007C8 32 rw 0x00001601 MIO Pin 50 Control *)
+		MIO_PIN_51*: LONGINT; (** 0x000007CC 32 rw 0x00001601 MIO Pin 51 Control *)
+		MIO_PIN_52*: LONGINT; (** 0x000007D0 32 rw 0x00001601 MIO Pin 52 Control *)
+		MIO_PIN_53*: LONGINT; (** 0x000007D4 32 rw 0x00001601 MIO Pin 53 Control *)
+		padding13: ARRAY 11 OF LONGINT;
+		MIO_LOOPBACK*: LONGINT; (** 0x00000804 32 rw 0x00000000 Loopback function within MIO *)
+		padding14: LONGINT;
+		MIO_MST_TRI0*: LONGINT; (** 0x0000080C 32 rw 0xFFFFFFFF MIO pin Tri-state Enables, 31:0 *)
+		MIO_MST_TRI1*: LONGINT; (** 0x00000810 32 rw 0x003FFFFF MIO pin Tri-state Enables, 53:32 *)
+		padding15: ARRAY 7 OF LONGINT;
+		SD0_WP_CD_SEL*: LONGINT; (** 0x00000830 32 rw 0x00000000 SDIO 0 WP CD select *)
+		SD1_WP_CD_SEL*: LONGINT; (** 0x00000834 32 rw 0x00000000 SDIO 1 WP CD select *)
+		padding16: ARRAY 50 OF LONGINT;
+		LVL_SHFTR_EN*: LONGINT; (** 0x00000900 32 rw 0x00000000 Level Shifters Enable *)
+		padding17: ARRAY 3 OF LONGINT;
+		OCM_CFG*: LONGINT; (** 0x00000910 32 rw 0x00000000 OCM Address Mapping *)
+		padding18: ARRAY 66 OF LONGINT;
+		Reserved*: LONGINT; (** 0x00000A1C 32 rw 0x00010101 Reserved *)
+		padding19: ARRAY 56 OF LONGINT;
+		GPIOB_CTRL*: LONGINT; (** 0x00000B00 32 rw 0x00000000 PS IO Buffer Control *)
+		GPIOB_CFG_CMOS18*: LONGINT; (** 0x00000B04 32 rw 0x00000000 MIO GPIOB CMOS 1.8V config *)
+		GPIOB_CFG_CMOS25*: LONGINT; (** 0x00000B08 32 rw 0x00000000 MIO GPIOB CMOS 2.5V config *)
+		GPIOB_CFG_CMOS33*: LONGINT; (** 0x00000B0C 32 rw 0x00000000 MIO GPIOB CMOS 3.3V config *)
+		padding20: LONGINT;
+		GPIOB_CFG_HSTL*: LONGINT; (** 0x00000B14 32 rw 0x00000000 MIO GPIOB HSTL config *)
+		GPIOB_DRVR_BIAS_CTRL*: LONGINT; (** 0x00000B18 32 mixed 0x00000000 MIO GPIOB Driver Bias Control *)
+		padding21: ARRAY 9 OF LONGINT;
+		DDRIOB_ADDR0*: LONGINT; (** 0x00000B40 32 rw 0x00000800 DDR IOB Config for A[14:0], CKE and DRST_B *)
+		DDRIOB_ADDR1*: LONGINT; (** 0x00000B44 32 rw 0x00000800 DDR IOB Config for BA[2:0], ODT, CS_B, WE_B, RAS_B and CAS_B *)
+		DDRIOB_DATA0*: LONGINT; (** 0x00000B48 32 rw 0x00000800 DDR IOB Config for Data 15:0 *)
+		DDRIOB_DATA1*: LONGINT; (** 0x00000B4C 32 rw 0x00000800 DDR IOB Config for Data 31:16 *)
+		DDRIOB_DIFF0*: LONGINT; (** 0x00000B50 32 rw 0x00000800 DDR IOB Config for DQS 1:0 *)
+		DDRIOB_DIFF1*: LONGINT; (** 0x00000B54 32 rw 0x00000800 DDR IOB Config for DQS 3:2 *)
+		DDRIOB_CLOCK*: LONGINT; (** 0x00000B58 32 rw 0x00000800 DDR IOB Config for Clock Output *)
+		DDRIOB_DRIVE_SLEW_ADDR*: LONGINT; (** 0x00000B5C 32 rw 0x00000000 Drive and Slew controls for Address and Command pins of the DDR Interface *)
+		DDRIOB_DRIVE_SLEW_DATA*: LONGINT; (** 0x00000B60 32 rw 0x00000000 Drive and Slew controls for DQ pins of the DDR Interface *)
+		DDRIOB_DRIVE_SLEW_DIFF*: LONGINT; (** 0x00000B64 32 rw 0x00000000 Drive and Slew controls for DQS pins of the DDR Interface *)
+		DDRIOB_DRIVE_SLEW_CLOCK*: LONGINT; (** 0x00000B68 32 rw 0x00000000 Drive and Slew controls for Clock pins of the DDR Interface *)
+		DDRIOB_DDR_CTRL*: LONGINT; (** 0x00000B6C 32 rw 0x00000000 DDR IOB Buffer Control *)
+		DDRIOB_DCI_CTRL*: LONGINT; (** 0x00000B70 32 rw 0x00000020 DDR IOB DCI Config *)
+		DDRIOB_DCI_STATUS*: LONGINT; (** 0x00000B74 32 mixed 0x00000000 DDR IO Buffer DCI Status *)
+	END;
+
+	(** Device Configuration (Devcfg) register definitions *)
+	DevCfgRegisters * = POINTER {UNSAFE,UNTRACED} TO RECORD
+		CTRL *,								(** 000H 32 mixed 0C006000H Control Register *)
+		LOCK *,								(** 004H 32 mixed 00000000H Lock Register *)
+		CFG *,									(** 008H 32 rw 00000508H Configuration Register *)
+		INT_STS *,								(** 00CH 32 mixed 00000000H Interrupt Status Register *)
+		INT_MASK *,							(** 010H 32 rw FFFFFFFFH Interrupt Mask Register *)
+		STATUS *,								(** 014H 32 mixed 40000820H Status Register *)
+		DMA_SRC_ADDR *,						(** 018H 32 rw 00000000H DMA Source Address Register *)
+		DMA_DST_ADDR *,					(** 01CH 32 rw 00000000H DMA Destination Address Register *)
+		DMA_SRC_LEN *,						(** 020H 32 rw 00000000H DMA Source Transfer Length *)
+		DMA_DST_LEN *,						(** 024H 32 rw 00000000H DMA Destination Transfer Length *)
+		ROM_SHADOW *,						(** 028H 32 wo 00000000H ROM Shadow Register *)
+		MULTIBOOT_ADDR *,					(** 02CH 32 rw 00000000H MULTI Boot Address Pointer *)
+		SW_ID *,								(** 030H 32 mixed 00000000H Software ID Register *)
+		UNLOCK *: LONGINT;				(** 034H 32 rw 00000000 Unlock Register *)
+		padding0: ARRAY 18 OF LONGINT;	(** padding: 038H to 080H *)
+		MCTRL *: LONGINT;					(** 080H 32 mixed xxxxxxxxH Miscellanous Control Register *)
+		padding1: ARRAY 31 OF LONGINT;	(** padding: 084H to 100H *)
+		XADCIF_CFG *,							(** 100H 32 rw 00001114H XADC Interface Configuration *)
+		XADCIF_INT_STS *,						(** 104H 32 mixed 00000200H XADC Interface Interrupt Status *)
+		XADCIF_INT_MASK *,					(** 108H 32 rw FFFFFFFFH XADC Interface Interrupt Mask *)
+		XADCIF_MSTS *,						(** 10CH 32 ro 00000500H XADC Interface Miscellanous Status Register *)
+		XADCIF_CMDFIFO *,					(** 110H 32 wo 00000000H XADC Interface Command FIFO Register *)
+		XADCIF_RDFIFO *,						(** 114H 32 ro 00000000H XADC Interface Data FIFO Register *)
+		XADCIF_MCTL *: LONGINT;			(** 118H 32 rw 00000010H XADC Interface Miscellanous Control Register *)
+	END;
+
+	MpcoreRegisters * = POINTER {UNSAFE, UNTRACED} TO RECORD
+		SCU_CONTROL_REGISTER * {ALIGNED(1)}: LONGINT; (* 0x00000000 32 rw 0x00000002 SCU Control Register *)
+		SCU_CONFIGURATION_REGISTER * {ALIGNED(1)}: LONGINT; (* 0x00000004 32 ro 0x00000501 SCU Configuration Register *)
+		SCU_CPU_Power_Status_Register * {ALIGNED(1)}: LONGINT; (* 0x00000008 32 rw 0x00000000 SCU CPU Power Status Register *)
+		SCU_Invalidate_All_Registers_in_Secure_State * {ALIGNED(1)}: LONGINT; (* 0x0000000C 32 wo 0x00000000 SCU Invalidate All Registers in Secure State *)
+		padd0 {ALIGNED(1)}: ARRAY 48 OF CHAR;
+		Filtering_Start_Address_Register * {ALIGNED(1)}: LONGINT; (* 0x00000040 32 rw 0x00100000 Filtering Start Address Register *)
+		Filtering_End_Address_Register * {ALIGNED(1)}: LONGINT; (* 0x00000044 32 rw 0x00000000 Defined by FILTEREND input *)
+		padd1 {ALIGNED(1)}: ARRAY 8 OF CHAR;
+		SCU_Access_Control_Register_SAC * {ALIGNED(1)}: LONGINT; (* 0x00000050 32 rw 0x0000000F SCU Access Control (SAC) Register *)
+		SCU_Non_secure_Access_Control_Register * {ALIGNED(1)}: LONGINT; (* 0x00000054 32 ro 0x00000000 SCU Non-secure Access Control Register SNSAC *)
+		padd2 {ALIGNED(1)}: ARRAY 168 OF CHAR;
+		ICCICR * {ALIGNED(1)}: LONGINT; (* 0x00000100 32 rw 0x00000000 CPU Interface Control Register *)
+		ICCPMR * {ALIGNED(1)}: LONGINT; (* 0x00000104 32 rw 0x00000000 Interrupt Priority Mask Register *)
+		ICCBPR * {ALIGNED(1)}: LONGINT; (* 0x00000108 32 rw 0x00000002 Binary Point Register *)
+		ICCIAR * {ALIGNED(1)}: LONGINT; (* 0x0000010C 32 rw 0x000003FF Interrupt Acknowledge Register *)
+		ICCEOIR * {ALIGNED(1)}: LONGINT; (* 0x00000110 32 rw 0x00000000 End Of Interrupt Register *)
+		ICCRPR * {ALIGNED(1)}: LONGINT; (* 0x00000114 32 rw 0x000000FF Running Priority Register *)
+		ICCHPIR * {ALIGNED(1)}: LONGINT; (* 0x00000118 32 rw 0x000003FF Highest Pending Interrupt Register *)
+		ICCABPR * {ALIGNED(1)}: LONGINT; (* 0x0000011C 32 rw 0x00000003 Aliased Non-secure Binary Point Register *)
+		padd3 {ALIGNED(1)}: ARRAY 220 OF CHAR;
+		ICCIDR * {ALIGNED(1)}: LONGINT; (* 0x000001FC 32 ro 0x3901243B CPU Interface Implementer Identification Register *)
+		Global_Timer_Counter_Register0 * {ALIGNED(1)}: LONGINT; (* 0x00000200 32 rw 0x00000000 Global Timer Counter Register 0 *)
+		Global_Timer_Counter_Register1 * {ALIGNED(1)}: LONGINT; (* 0x00000204 32 rw 0x00000000 Global Timer Counter Register 1 *)
+		Global_Timer_Control_Register * {ALIGNED(1)}: LONGINT; (* 0x00000208 32 rw 0x00000000 Global Timer Control Register *)
+		Global_Timer_Interrupt_Status_Register * {ALIGNED(1)}: LONGINT; (* 0x0000020C 32 rw 0x00000000 Global Timer Interrupt Status Register *)
+		Comparator_Value_Register0 * {ALIGNED(1)}: LONGINT; (* 0x00000210 32 rw 0x00000000 Comparator Value Register_0 *)
+		Comparator_Value_Register1 * {ALIGNED(1)}: LONGINT; (* 0x00000214 32 rw 0x00000000 Comparator Value Register_1 *)
+		Auto_increment_Register * {ALIGNED(1)}: LONGINT; (* 0x00000218 32 rw 0x00000000 Auto-increment Register *)
+		padd4 {ALIGNED(1)}: ARRAY 996 OF CHAR;
+		Private_Timer_Load_Register * {ALIGNED(1)}: LONGINT; (* 0x00000600 32 rw 0x00000000 Private Timer Load Register *)
+		Private_Timer_Counter_Register * {ALIGNED(1)}: LONGINT; (* 0x00000604 32 rw 0x00000000 Private Timer Counter Register *)
+		Private_Timer_Control_Register * {ALIGNED(1)}: LONGINT; (* 0x00000608 32 rw 0x00000000 Private Timer Control Register *)
+		Private_Timer_Interrupt_Status_Register * {ALIGNED(1)}: LONGINT; (* 0x0000060C 32 rw 0x00000000 Private Timer Interrupt Status Register *)
+		padd5 {ALIGNED(1)}: ARRAY 16 OF CHAR;
+		Watchdog_Load_Register * {ALIGNED(1)}: LONGINT; (* 0x00000620 32 rw 0x00000000 Watchdog Load Register *)
+		Watchdog_Counter_Register * {ALIGNED(1)}: LONGINT; (* 0x00000624 32 rw 0x00000000 Watchdog Counter Register *)
+		Watchdog_Control_Register * {ALIGNED(1)}: LONGINT; (* 0x00000628 32 rw 0x00000000 Watchdog Control Register *)
+		Watchdog_Interrupt_Status_Register * {ALIGNED(1)}: LONGINT; (* 0x0000062C 32 rw 0x00000000 Watchdog Interrupt Status Register *)
+		Watchdog_Reset_Status_Register * {ALIGNED(1)}: LONGINT; (* 0x00000630 32 rw 0x00000000 Watchdog Reset Status Register *)
+		Watchdog_Disable_Register * {ALIGNED(1)}: LONGINT; (* 0x00000634 32 rw 0x00000000 Watchdog Disable Register *)
+		padd6 {ALIGNED(1)}: ARRAY 2504 OF CHAR;
+		ICDDCR * {ALIGNED(1)}: LONGINT; (* 0x00001000 32 rw 0x00000000 Distributor Control Register *)
+		ICDICTR * {ALIGNED(1)}: LONGINT; (* 0x00001004 32 ro 0x0000FC22 Interrupt Controller Type Register *)
+		ICDIIDR * {ALIGNED(1)}: LONGINT; (* 0x00001008 32 ro 0x0102043B Distributor Implementer Identification Register *)
+		padd7 {ALIGNED(1)}: ARRAY 116 OF CHAR;
+		ICDISR0 * {ALIGNED(1)}: LONGINT; (* 0x00001080 32 rw 0x00000000 Interrupt Security Register_0 *)
+		ICDISR1 * {ALIGNED(1)}: LONGINT; (* 0x00001084 32 rw 0x00000000 Interrupt Security Register_1 *)
+		ICDISR2 * {ALIGNED(1)}: LONGINT; (* 0x00001088 32 rw 0x00000000 Interrupt Security Register_2 *)
+		padd8 {ALIGNED(1)}: ARRAY 116 OF CHAR;
+		ICDISER0 * {ALIGNED(1)}: LONGINT; (* 0x00001100 32 rw 0x0000FFFF Interrupt Set-enable Register 0 *)
+		ICDISER1 * {ALIGNED(1)}: LONGINT; (* 0x00001104 32 rw 0x00000000 Interrupt Set-enable Register 1 *)
+		ICDISER2 * {ALIGNED(1)}: LONGINT; (* 0x00001108 32 rw 0x00000000 Interrupt Set-enable Register 2 *)
+		padd9 {ALIGNED(1)}: ARRAY 116 OF CHAR;
+		ICDICER0 * {ALIGNED(1)}: LONGINT; (* 0x00001180 32 rw 0x0000FFFF Interrupt Clear-Enable Register 0 *)
+		ICDICER1 * {ALIGNED(1)}: LONGINT; (* 0x00001184 32 rw 0x00000000 Interrupt Clear-Enable Register 1 *)
+		ICDICER2 * {ALIGNED(1)}: LONGINT; (* 0x00001188 32 rw 0x00000000 Interrupt Clear-Enable Register 2 *)
+		padd10 {ALIGNED(1)}: ARRAY 116 OF CHAR;
+		ICDISPR0 * {ALIGNED(1)}: LONGINT; (* 0x00001200 32 rw 0x00000000 Interrupt Set-pending Register_0 *)
+		ICDISPR1 * {ALIGNED(1)}: LONGINT; (* 0x00001204 32 rw 0x00000000 Interrupt Set-pending Register_1 *)
+		ICDISPR2 * {ALIGNED(1)}: LONGINT; (* 0x00001208 32 rw 0x00000000 Interrupt Set-pending Register_2 *)
+		padd11 {ALIGNED(1)}: ARRAY 116 OF CHAR;
+		ICDICPR0 * {ALIGNED(1)}: LONGINT; (* 0x00001280 32 rw 0x00000000 Interrupt Clear-Pending Register_0 *)
+		ICDICPR1 * {ALIGNED(1)}: LONGINT; (* 0x00001284 32 rw 0x00000000 Interrupt Clear-Pending Register_1 *)
+		ICDICPR2 * {ALIGNED(1)}: LONGINT; (* 0x00001288 32 rw 0x00000000 Interrupt Clear-Pending Register_2 *)
+		padd12 {ALIGNED(1)}: ARRAY 116 OF CHAR;
+		ICDABR0 * {ALIGNED(1)}: LONGINT; (* 0x00001300 32 rw 0x00000000 Active Bit register_0 *)
+		ICDABR1 * {ALIGNED(1)}: LONGINT; (* 0x00001304 32 rw 0x00000000 Active Bit register_1 *)
+		ICDABR2 * {ALIGNED(1)}: LONGINT; (* 0x00001308 32 rw 0x00000000 Active Bit register_2 *)
+		padd13 {ALIGNED(1)}: ARRAY 244 OF CHAR;
+		ICDIPR0 * {ALIGNED(1)}: LONGINT; (* 0x00001400 32 rw 0x00000000 Interrupt Priority Register_0 *)
+		ICDIPR1 * {ALIGNED(1)}: LONGINT; (* 0x00001404 32 rw 0x00000000 Interrupt Priority Register_1 *)
+		ICDIPR2 * {ALIGNED(1)}: LONGINT; (* 0x00001408 32 rw 0x00000000 Interrupt Priority Register_2 *)
+		ICDIPR3 * {ALIGNED(1)}: LONGINT; (* 0x0000140C 32 rw 0x00000000 Interrupt Priority Register_3 *)
+		ICDIPR4 * {ALIGNED(1)}: LONGINT; (* 0x00001410 32 rw 0x00000000 Interrupt Priority Register_4 *)
+		ICDIPR5 * {ALIGNED(1)}: LONGINT; (* 0x00001414 32 rw 0x00000000 Interrupt Priority Register_5 *)
+		ICDIPR6 * {ALIGNED(1)}: LONGINT; (* 0x00001418 32 rw 0x00000000 Interrupt Priority Register_6 *)
+		ICDIPR7 * {ALIGNED(1)}: LONGINT; (* 0x0000141C 32 rw 0x00000000 Interrupt Priority Register_7 *)
+		ICDIPR8 * {ALIGNED(1)}: LONGINT; (* 0x00001420 32 rw 0x00000000 Interrupt Priority Register_8 *)
+		ICDIPR9 * {ALIGNED(1)}: LONGINT; (* 0x00001424 32 rw 0x00000000 Interrupt Priority Register_9 *)
+		ICDIPR10 * {ALIGNED(1)}: LONGINT; (* 0x00001428 32 rw 0x00000000 Interrupt Priority Register_10 *)
+		ICDIPR11 * {ALIGNED(1)}: LONGINT; (* 0x0000142C 32 rw 0x00000000 Interrupt Priority Register_11 *)
+		ICDIPR12 * {ALIGNED(1)}: LONGINT; (* 0x00001430 32 rw 0x00000000 Interrupt Priority Register_12 *)
+		ICDIPR13 * {ALIGNED(1)}: LONGINT; (* 0x00001434 32 rw 0x00000000 Interrupt Priority Register_13 *)
+		ICDIPR14 * {ALIGNED(1)}: LONGINT; (* 0x00001438 32 rw 0x00000000 Interrupt Priority Register_14 *)
+		ICDIPR15 * {ALIGNED(1)}: LONGINT; (* 0x0000143C 32 rw 0x00000000 Interrupt Priority Register_15 *)
+		ICDIPR16 * {ALIGNED(1)}: LONGINT; (* 0x00001440 32 rw 0x00000000 Interrupt Priority Register_16 *)
+		ICDIPR17 * {ALIGNED(1)}: LONGINT; (* 0x00001444 32 rw 0x00000000 Interrupt Priority Register_17 *)
+		ICDIPR18 * {ALIGNED(1)}: LONGINT; (* 0x00001448 32 rw 0x00000000 Interrupt Priority Register_18 *)
+		ICDIPR19 * {ALIGNED(1)}: LONGINT; (* 0x0000144C 32 rw 0x00000000 Interrupt Priority Register_19 *)
+		ICDIPR20 * {ALIGNED(1)}: LONGINT; (* 0x00001450 32 rw 0x00000000 Interrupt Priority Register_20 *)
+		ICDIPR21 * {ALIGNED(1)}: LONGINT; (* 0x00001454 32 rw 0x00000000 Interrupt Priority Register_21 *)
+		ICDIPR22 * {ALIGNED(1)}: LONGINT; (* 0x00001458 32 rw 0x00000000 Interrupt Priority Register_22 *)
+		ICDIPR23 * {ALIGNED(1)}: LONGINT; (* 0x0000145C 32 rw 0x00000000 Interrupt Priority Register_23 *)
+		padd14 {ALIGNED(1)}: ARRAY 928 OF CHAR;
+		ICDIPTR0 * {ALIGNED(1)}: LONGINT; (* 0x00001800 32 ro 0x01010101 Interrupt Processor Targets Register 0 *)
+		ICDIPTR1 * {ALIGNED(1)}: LONGINT; (* 0x00001804 32 ro 0x01010101 Interrupt Processor Targets Register 1 *)
+		ICDIPTR2 * {ALIGNED(1)}: LONGINT; (* 0x00001808 32 ro 0x01010101 Interrupt Processor Targets Register 2 *)
+		ICDIPTR3 * {ALIGNED(1)}: LONGINT; (* 0x0000180C 32 ro 0x01010101 Interrupt Processor Targets Register 3 *)
+		ICDIPTR4 * {ALIGNED(1)}: LONGINT; (* 0x00001810 32 rw 0x00000000 Interrupt Processor Targets Register 4 *)
+		ICDIPTR5 * {ALIGNED(1)}: LONGINT; (* 0x00001814 32 ro 0x00000000 Interrupt Processor Targets Register 5 *)
+		ICDIPTR6 * {ALIGNED(1)}: LONGINT; (* 0x00001818 32 ro 0x01000000 Interrupt Processor Targets Register 6 *)
+		ICDIPTR7 * {ALIGNED(1)}: LONGINT; (* 0x0000181C 32 ro 0x01010101 Interrupt Processor Targets Register 7 *)
+		ICDIPTR8 * {ALIGNED(1)}: LONGINT; (* 0x00001820 32 rw 0x00000000 Interrupt Processor Targets Register 8 *)
+		ICDIPTR9 * {ALIGNED(1)}: LONGINT; (* 0x00001824 32 rw 0x00000000 Interrupt Processor Targets Register 9 *)
+		ICDIPTR10 * {ALIGNED(1)}: LONGINT; (* 0x00001828 32 rw 0x00000000 Interrupt Processor Targets Register 10 *)
+		ICDIPTR11 * {ALIGNED(1)}: LONGINT; (* 0x0000182C 32 rw 0x00000000 Interrupt Processor Targets Register 11 *)
+		ICDIPTR12 * {ALIGNED(1)}: LONGINT; (* 0x00001830 32 rw 0x00000000 Interrupt Processor Targets Register 12 *)
+		ICDIPTR13 * {ALIGNED(1)}: LONGINT; (* 0x00001834 32 rw 0x00000000 Interrupt Processor Targets Register 13 *)
+		ICDIPTR14 * {ALIGNED(1)}: LONGINT; (* 0x00001838 32 rw 0x00000000 Interrupt Processor Targets Register 14 *)
+		ICDIPTR15 * {ALIGNED(1)}: LONGINT; (* 0x0000183C 32 rw 0x00000000 Interrupt Processor Targets Register 15 *)
+		ICDIPTR16 * {ALIGNED(1)}: LONGINT; (* 0x00001840 32 rw 0x00000000 Interrupt Processor Targets Register 16 *)
+		ICDIPTR17 * {ALIGNED(1)}: LONGINT; (* 0x00001844 32 rw 0x00000000 Interrupt Processor Targets Register 17 *)
+		ICDIPTR18 * {ALIGNED(1)}: LONGINT; (* 0x00001848 32 rw 0x00000000 Interrupt Processor Targets Register 18 *)
+		ICDIPTR19 * {ALIGNED(1)}: LONGINT; (* 0x0000184C 32 rw 0x00000000 Interrupt Processor Targets Register 19 *)
+		ICDIPTR20 * {ALIGNED(1)}: LONGINT; (* 0x00001850 32 rw 0x00000000 Interrupt Processor Targets Register 20 *)
+		ICDIPTR21 * {ALIGNED(1)}: LONGINT; (* 0x00001854 32 rw 0x00000000 Interrupt Processor Targets Register 21 *)
+		ICDIPTR22 * {ALIGNED(1)}: LONGINT; (* 0x00001858 32 rw 0x00000000 Interrupt Processor Targets Register 22 *)
+		ICDIPTR23 * {ALIGNED(1)}: LONGINT; (* 0x0000185C 32 rw 0x00000000 Interrupt Processor Targets Register 23 *)
+		padd15 {ALIGNED(1)}: ARRAY 928 OF CHAR;
+		ICDICFR0 * {ALIGNED(1)}: LONGINT; (* 0x00001C00 32 ro 0xAAAAAAAA Interrupt Configuration Register 0 *)
+		ICDICFR1 * {ALIGNED(1)}: LONGINT; (* 0x00001C04 32 rw 0x7DC00000 Interrupt Configuration Register 1 *)
+		ICDICFR2 * {ALIGNED(1)}: LONGINT; (* 0x00001C08 32 rw 0x55555555 Interrupt Configuration Register 2 *)
+		ICDICFR3 * {ALIGNED(1)}: LONGINT; (* 0x00001C0C 32 rw 0x55555555 Interrupt Configuration Register 3 *)
+		ICDICFR4 * {ALIGNED(1)}: LONGINT; (* 0x00001C10 32 rw 0x55555555 Interrupt Configuration Register 4 *)
+		ICDICFR5 * {ALIGNED(1)}: LONGINT; (* 0x00001C14 32 rw 0x55555555 Interrupt Configuration Register 5 *)
+		padd16 {ALIGNED(1)}: ARRAY 232 OF CHAR;
+		ppi_status * {ALIGNED(1)}: LONGINT; (* 0x00001D00 32 ro 0x00000000 PPI Status Register *)
+		spi_status_0 * {ALIGNED(1)}: LONGINT; (* 0x00001D04 32 ro 0x00000000 SPI Status Register 0 *)
+		spi_status_1 * {ALIGNED(1)}: LONGINT; (* 0x00001D08 32 ro 0x00000000 SPI Status Register 1 *)
+		padd17 {ALIGNED(1)}: ARRAY 500 OF CHAR;
+		ICDSGIR * {ALIGNED(1)}: LONGINT; (* 0x00001F00 32 rw 0x00000000 Software Generated Interrupt Register *)
+		padd18 {ALIGNED(1)}: ARRAY 204 OF CHAR;
+		ICPIDR4 * {ALIGNED(1)}: LONGINT; (* 0x00001FD0 32 rw 0x00000004 Peripheral ID4 *)
+		ICPIDR5 * {ALIGNED(1)}: LONGINT; (* 0x00001FD4 32 rw 0x00000000 Peripheral ID5 *)
+		ICPIDR6 * {ALIGNED(1)}: LONGINT; (* 0x00001FD8 32 rw 0x00000000 Peripheral ID6 *)
+		ICPIDR7 * {ALIGNED(1)}: LONGINT; (* 0x00001FDC 32 rw 0x00000000 Peripheral ID7 *)
+		ICPIDR0 * {ALIGNED(1)}: LONGINT; (* 0x00001FE0 32 rw 0x00000090 Peripheral ID0 *)
+		ICPIDR1 * {ALIGNED(1)}: LONGINT; (* 0x00001FE4 32 rw 0x000000B3 Peripheral ID1 *)
+		ICPIDR2 * {ALIGNED(1)}: LONGINT; (* 0x00001FE8 32 rw 0x0000001B Peripheral ID2 *)
+		ICPIDR3 * {ALIGNED(1)}: LONGINT; (* 0x00001FEC 32 rw 0x00000000 Peripheral ID3 *)
+		ICCIDR0 * {ALIGNED(1)}: LONGINT; (* 0x00001FF0 32 rw 0x0000000D Component ID0 *)
+		ICCIDR1 * {ALIGNED(1)}: LONGINT; (* 0x00001FF4 32 rw 0x000000F0 Component ID1 *)
+		ICCIDR2 * {ALIGNED(1)}: LONGINT; (* 0x00001FF8 32 rw 0x00000005 Component ID2 *)
+		ICCIDR3 * {ALIGNED(1)}: LONGINT; (* 0x00001FFC 32 rw 0x000000B1 Component ID3 *)
+	END;
+
+	SWDTRegisters * = POINTER {UNSAFE, UNTRACED} TO RECORD
+		XWDTPS_ZMR_OFFSET * {ALIGNED(1)}: LONGINT; (* 0x00000000 24 mixed 0x000001C0 WD zero mode register *)
+		XWDTPS_CCR_OFFSET * {ALIGNED(1)}: LONGINT; (* 0x00000004 26 mixed 0x00003FFC Counter Control Register *)
+		XWDTPS_RESTART_OFFSET * {ALIGNED(1)}: LONGINT; (* 0x00000008 16 wo 0x00000000 Restart key register - this not a real register as no data is stored *)
+		XWDTPS_SR_OFFSET * {ALIGNED(1)}: LONGINT; (* 0x0000000C 1 ro 0x00000000 Status Register *)
+	END;
+
+VAR
+	slcr*: SlcrRegisters; (** System Level Control (SLCR) registers set *)
+	devcfg*: DevCfgRegisters; (** Device Configuration (Devcfg) register set *)
+	mpcore*: MpcoreRegisters; (** Cortex-A9 multiprocessor extensions (MPCore) register set *)
+	swdt*: SWDTRegisters; (** System watchdog (SWDT) register set *)
+
+BEGIN
+	slcr := SlcrBase;
+	devcfg := DevCfgBase;
+	mpcore := MpcoreBase;
+	swdt := SWDTBase;
+END Platform.

source/Zynq.AcAxisIo.Mod

@@ -0,0 +1,523 @@
+(**
+	AUTHOR "Alexey Morozov, HighDim GmbH, 2015";
+	PURPOSE "ActiveCells AXI4 Stream IO interface";
+*)
+MODULE AcAxisIo;
+
+IMPORT
+	SYSTEM, Platform;
+
+CONST
+	ChanOffset  = 256*4; (* ActiveCells AXI4 Stream channel offset in bytes *)
+(*
+	OutDataOffset = 0;
+	OutReadyOffset = SIZEOF(LONGINT);
+
+	InpAvailableOffset = 0;
+	InpDataOffset = 2*SIZEOF(LONGINT);
+*)
+
+TYPE
+	(* AXI4 Stream port descriptor *)
+	PortDesc = RECORD
+		lockAddr: ADDRESS; (**  *)
+		portAddr: ADDRESS;
+	END;
+
+	Output* = PORT OUT; (** AXI4 Stream output port *)
+	Input* = PORT IN; (** AXI4 Stream input port *)
+
+VAR
+	inputs: ARRAY 2 OF ARRAY 256 OF PortDesc;
+	outputs: ARRAY 2 OF ARRAY 256 OF PortDesc;
+
+	locks: ARRAY 2 OF LONGINT; (* locks for each physical channel *)
+
+(*
+	(** AXI4 Stream output interface *)
+	Output* = POINTER{UNSAFE,UNTRACED} TO RECORD
+		data: LONGINT; (** output data, write only *)
+		ready: BOOLEAN; (** TRUE when output is available for sending data *)
+		padding: ARRAY 3 OF CHAR;
+	END;
+
+	(** AXI4 Stream input port description *)
+	Input* = POINTER{UNSAFE,UNTRACED} TO RECORD
+		available: BOOLEAN; (** TRUE when input data is available *)
+		padding: ARRAY 7 OF CHAR;
+		data: LONGINT; (** input data, read only *)
+	END;
+*)
+
+	(**
+		Get AXI4 Stream input port
+
+		physicalPortNum: physical port number
+		logicalPortNum: logical port number
+	*)
+	PROCEDURE GetInput*(physicalPortNum, logicalPortNum: LONGINT; VAR port: Input): BOOLEAN;
+	VAR
+		portDesc: POINTER {UNSAFE} TO PortDesc;
+		portAddr: ADDRESS;
+	BEGIN
+		CASE physicalPortNum OF
+			|0: portAddr := Platform.MasterAxiGp0Base + logicalPortNum*ChanOffset;
+			|1: portAddr := Platform.MasterAxiGp1Base + logicalPortNum*ChanOffset;
+		ELSE
+			RETURN FALSE;
+		END;
+
+		portDesc := ADDRESSOF(inputs[physicalPortNum,logicalPortNum]);
+		portDesc.portAddr := portAddr;
+		portDesc.lockAddr := ADDRESSOF(locks[physicalPortNum]);
+		port := SYSTEM.VAL(Input,portDesc);
+
+		RETURN TRUE;
+	END GetInput;
+
+	(**
+		Get AXI4 Stream output port
+
+		physicalPortNum: physical port number
+		logicalPortNum: logical port number
+	*)
+	PROCEDURE GetOutput*(physicalPortNum, logicalPortNum: LONGINT; VAR port: Output): BOOLEAN;
+	VAR
+		portDesc: POINTER {UNSAFE} TO PortDesc;
+		portAddr: ADDRESS;
+	BEGIN
+		CASE physicalPortNum OF
+			|0: portAddr := Platform.MasterAxiGp0Base + logicalPortNum*ChanOffset;
+			|1: portAddr := Platform.MasterAxiGp1Base + logicalPortNum*ChanOffset;
+		ELSE
+			RETURN FALSE;
+		END;
+
+		portDesc := ADDRESSOF(outputs[physicalPortNum,logicalPortNum]);
+		portDesc.portAddr := portAddr;
+		portDesc.lockAddr := ADDRESSOF(locks[physicalPortNum]);
+		port := SYSTEM.VAL(Output,portDesc);
+
+		RETURN TRUE;
+	END GetOutput;
+
+	(*
+		Acquire exclusive access to a resource
+
+		R1: lock address
+
+		R0, R5 are used in addition
+
+		Based on the code presented in "Barrier Litmus Tests and Cookbook" by Richard Grisenthwaite, ARM, 26.11.2009,
+	*)
+	PROCEDURE {NOPAF} -AcquireResource;
+	CODE
+		MOV R0, #1
+	Loop:
+		LDREX R5, R1 ; read lock
+		CMP R5, #0 ; check if 0
+		WFENE ; sleep if the lock is held
+
+		STREXEQ R5, R0, R1 ; attempt to store new value
+		CMPEQ R5, #0 ; test if store suceeded
+		BNE Loop ; retry if not
+
+		DMB ; ensures that all subsequent accesses are observed after the gaining of the lock is observed
+		
+		; loads and stores in the critical region can now be performed
+	END AcquireResource;
+
+	(*
+		Release exclusive access to a resource
+
+		R1: lock address
+
+		R0 is used in addition
+
+		Based on the code presented in "Barrier Litmus Tests and Cookbook" by Richard Grisenthwaite, ARM, 26.11.2009,
+	*)
+	PROCEDURE {NOPAF} -ReleaseResource;
+	CODE
+		MOV R0, #0
+		DMB ; ensure all previous accesses are observed before the lock is cleared
+		STR R0, [R1, #0]
+		; clear the lock.
+		DSB ; ensure completion of the store that cleared the lock before sending the event
+		SEV
+	END ReleaseResource;
+
+	(*PROCEDURE AcquireObject(lockAddr: ADDRESS);
+	BEGIN
+		CODE
+			LDR R1, [FP, #lockAddr] ; R1 := address of lock
+		END;
+		AcquireResource;
+	END AcquireObject;
+
+	PROCEDURE ReleaseObject(lockAddr: ADDRESS);
+	BEGIN
+		CODE
+			LDR R1, [FP, #lockAddr] ; R1 := address of lock
+		END;
+		ReleaseResource
+	END ReleaseObject;*)
+
+	(**
+		Returns TRUE if the given output port is ready to accept new data
+	*)
+	PROCEDURE Ready*(out: Output): BOOLEAN;
+	VAR b: BOOLEAN;
+	(*VAR p: POINTER {UNSAFE} TO PortDesc;*)
+	BEGIN
+		CODE
+			LDR R3, [FP, #out] ; R3 := address of PortDesc.lockAddr
+			LDR R1, [R3, #0] ; R1 := PortDesc.lockAddr
+		END;
+		AcquireResource;
+		CODE
+			LDR R3, [R3, #4] ; R3 := PortDesc.portAddr
+			LDR R4, [R3, #4] ; R4 := out.Ready
+			STRB R4, [FP, #b] ; b := R4
+		END;
+		ReleaseResource;
+
+		(*p := SYSTEM.VAL(ADDRESS,out);
+		AcquireObject(p.lockAddr);
+		b := SYSTEM.VAL(BOOLEAN,SYSTEM.GET32(p.portAddr+OutReadyOffset));
+		ReleaseObject(p.lockAddr);*)
+
+		RETURN b;
+	END Ready;
+
+	(** Returns number of data elements available to read from an input port *)
+	PROCEDURE Available*(inp: Input): LONGINT;
+	VAR available: LONGINT;
+	(*VAR p: POINTER {UNSAFE} TO PortDesc;*)
+	BEGIN
+		CODE
+			LDR R3, [FP, #inp] ; R3 := address of PortDesc.lockAddr
+			LDR R1, [R3, #0] ; R1 := PortDesc.lockAddr
+		END;
+		AcquireResource;
+		CODE
+			LDR R3, [R3, #4] ; R3 := PortDesc.portAddr
+			LDR R4, [R3, #0] ; R4 := inp.Available
+			STR R4, [FP, #available] ; available := R4
+		END;
+		ReleaseResource;
+
+		(*p := SYSTEM.VAL(ADDRESS,inp);
+		AcquireObject(p.lockAddr);
+		available := SYSTEM.GET32(p.portAddr+InpAvailableOffset);
+		ReleaseObject(p.lockAddr);*)
+
+		RETURN available;
+	END Available;
+
+	(** Send data to an output port (blocking version) *)
+	PROCEDURE Send*(out: Output; x: LONGINT);
+	(*VAR p: POINTER {UNSAFE} TO PortDesc;*)
+	BEGIN
+		CODE
+			LDR R3, [FP, #out] ; R3 := address of PortDesc.lockAddr
+			LDR R1, [R3, #0] ; R1 := PortDesc.lockAddr
+		END;
+		AcquireResource;
+		CODE
+			LDR R2, [FP, #x] ; R2 := x
+			LDR R3, [R3, #4] ; R3 := PortDesc.portAddr
+			STR R2, [R3, #0] ; out.Data := R2
+		END;
+		ReleaseResource;
+
+		(*p := SYSTEM.VAL(ADDRESS,out);
+		AcquireObject(p.lockAddr);
+		SYSTEM.PUT32(p.portAddr+OutDataOffset,x);
+		ReleaseObject(p.lockAddr);*)
+	END Send;
+
+	(** Send data to an output port (non-blocking version) *)
+	PROCEDURE SendNonBlocking*(out: Output; x: LONGINT): BOOLEAN;
+	VAR b: BOOLEAN;
+	(*VAR p: POINTER {UNSAFE} TO PortDesc;*)
+	BEGIN
+		CODE
+			LDR R3, [FP, #out] ; R3 := address of PortDesc.lockAddr
+			LDR R1, [R3, #0] ; R1 := PortDesc.lockAddr
+		END;
+		AcquireResource;
+		CODE
+			LDR R2, [FP, #x] ; R2 := x
+			LDR R3, [R3, #4] ; R3 := PortDesc.portAddr
+			LDR R4, [R3, #4] ; R4 := out.Ready
+			STRB R4, [FP, #b] ; b := R4
+
+			CMP R4, #0
+			BEQ Exit
+
+			STR R2, [R3, #0] ; out.Data := R2
+		Exit:
+		END;
+		ReleaseResource;
+
+		(*p := SYSTEM.VAL(ADDRESS,out);
+		AcquireObject(p.lockAddr);
+		b := SYSTEM.VAL(BOOLEAN,SYSTEM.GET32(p.portAddr+OutReadyOffset));
+		IF b THEN SYSTEM.PUT32(p.portAddr+OutDataOffset,x); END;
+		ReleaseObject(p.lockAddr);*)
+
+		RETURN b;
+	END SendNonBlocking;
+
+	OPERATOR "<<"*(out: Output; x: LONGINT);
+	BEGIN
+		Send(out,x);
+	END "<<";
+
+	OPERATOR ">>"*(x: LONGINT; out: Output);
+	BEGIN
+		Send(out,x);
+	END ">>";
+
+	OPERATOR "<<?"*(out: Output; x: LONGINT): BOOLEAN;
+	BEGIN
+		RETURN SendNonBlocking(out,x);
+	END "<<?";
+
+	OPERATOR ">>?"*(x: LONGINT; out: Output): BOOLEAN;
+	BEGIN
+		RETURN SendNonBlocking(out,x);
+	END ">>?";
+
+	(** Receive data from an input port (blocking version) *)
+	PROCEDURE Receive*(inp: Input; VAR x: LONGINT);
+	(*VAR p: POINTER {UNSAFE} TO PortDesc;*)
+	BEGIN
+		CODE
+			LDR R3, [FP, #inp] ; R3 := address of PortDesc.lockAddr
+			LDR R1, [R3, #0] ; R1 := PortDesc.lockAddr
+		END;
+		AcquireResource;
+		CODE
+			LDR R2, [FP, #x] ; R2 := address of x
+			LDR R3, [R3, #4] ; R3 := PortDesc.portAddr
+			LDR R4, [R3, #8] ; R4 := inp.Data
+			STR R4, [R2, #0] ; x := R4
+		END;
+		ReleaseResource;
+
+		(*p := SYSTEM.VAL(ADDRESS,inp);
+		AcquireObject(p.lockAddr);
+		x := SYSTEM.GET32(p.portAddr+InpDataOffset);
+		ReleaseObject(p.lockAddr);*)
+	END Receive;
+
+	(** Receive data from an input port (non-blocking version) *)
+	PROCEDURE ReceiveNonBlocking*(inp: Input; VAR x: LONGINT): BOOLEAN;
+	VAR b: BOOLEAN;
+	(*VAR p: POINTER {UNSAFE} TO PortDesc;*)
+	BEGIN
+		CODE
+			LDR R3, [FP, #inp] ; R3 := address of PortDesc.lockAddr
+			LDR R1, [R3, #0] ; R1 := PortDesc.lockAddr
+		END;
+		AcquireResource;
+		CODE
+			LDR R3, [R3, #4] ; R3 := PortDesc.portAddr
+			LDR R4, [R3, #0] ; R4 := inp.Available
+			STRB R4, [FP, #b] ; b := R4
+
+			CMP R4, #0
+			BEQ Exit
+
+			LDR R2, [FP, #x] ; R2 := address of x
+			LDR R4, [R3, #8] ; R4 := inp.Data
+			STR R4, [R2, #0] ; x := R4
+		Exit:
+		END;
+		ReleaseResource;
+
+		(*p := SYSTEM.VAL(ADDRESS,inp);
+		AcquireObject(p.lockAddr);
+		b := SYSTEM.VAL(BOOLEAN,SYSTEM.GET32(p.portAddr+InpAvailableOffset));
+		IF b THEN x := SYSTEM.GET32(p.portAddr+InpDataOffset); END;
+		ReleaseObject(p.lockAddr);*)
+
+		RETURN b;
+	END ReceiveNonBlocking;
+
+	OPERATOR ">>"*(inp: Input; VAR x: LONGINT);
+	BEGIN
+		Receive(inp,x);
+	END ">>";
+
+	OPERATOR "<<"*(VAR x: LONGINT; inp: Input);
+	BEGIN
+		Receive(inp,x);
+	END "<<";
+
+	OPERATOR ">>?"*(inp: Input; VAR x: LONGINT): BOOLEAN;
+	BEGIN
+		RETURN ReceiveNonBlocking(inp,x);
+	END ">>?";
+
+	OPERATOR "<<?"*(VAR x: LONGINT; inp: Input): BOOLEAN;
+	BEGIN
+		RETURN ReceiveNonBlocking(inp,x);
+	END "<<?";
+
+	PROCEDURE SendMultiple(portAddr, dataAddr: ADDRESS; numElements: LONGINT);
+	CODE
+		LDR R0, [FP,#portAddr]
+		LDR R1, [FP,#dataAddr]
+		LDR R2, [FP,#numElements]
+
+		CMP R2, #8
+		BLT CheckLoop4
+
+	Loop8: ; numElements >= 8, coalescing of 8 transfers
+		LDR R3, [R1,#0]
+		LDR R4, [R1,#4]
+		LDR R5, [R1,#8]
+		LDR R6, [R1,#12]
+		LDR R7, [R1,#16]
+		LDR R8, [R1,#20]
+		LDR R9, [R1,#24]
+		LDR R10, [R1,#28]
+
+		STR R3, [R0,#0]
+		STR R4, [R0,#0]
+		STR R5, [R0,#0]
+		STR R6, [R0,#0]
+		STR R7, [R0,#0]
+		STR R8, [R0,#0]
+		STR R9, [R0,#0]
+		STR R10, [R0,#0]
+
+		ADD R1, R1, #32
+		SUBS R2, R2, #8
+		BGT Loop8
+
+	CheckLoop4:
+		CMP R2, #4
+		BLT CheckLoop1
+
+	Loop4: ; numElements >= 4, coalescing of 4 transfers
+		LDR R3, [R1,#0]
+		LDR R4, [R1,#4]
+		LDR R5, [R1,#8]
+		LDR R6, [R1,#12]
+
+		STR R3, [R0,#0]
+		STR R4, [R0,#0]
+		STR R5, [R0,#0]
+		STR R6, [R0,#0]
+
+		ADD R1, R1, #16
+		SUBS R2, R2, #4
+		BGT Loop4
+
+	CheckLoop1:
+		CMP R2, #1
+		BLT Exit
+
+	Loop1: ; numElements >= 1, transfer element by element
+		LDR R3, [R1,#0]
+		STR R3, [R0,#0]
+
+		ADD R1, R1, #4
+		SUBS R2, R2, #1
+		BGT Loop1
+
+	Exit:
+
+	END SendMultiple;
+
+	PROCEDURE ReceiveMultiple(portAddr, dataAddr: ADDRESS; numElements: LONGINT);
+	CODE
+		LDR R0, [FP,#portAddr]
+		ADD R0, R0, #8
+		LDR R1, [FP,#dataAddr]
+		LDR R2, [FP,#numElements]
+
+		CMP R2, #8
+		BLT CheckLoop4
+
+	Loop8: ; numElements >= 8, coalescing of 8 transfers
+		LDR R3, [R0,#0]
+		LDR R4, [R0,#0]
+		LDR R5, [R0,#0]
+		LDR R6, [R0,#0]
+		LDR R7, [R0,#0]
+		LDR R8, [R0,#0]
+		LDR R9, [R0,#0]
+		LDR R10, [R0,#0]
+
+		STR R3, [R1,#0]
+		STR R4, [R1,#4]
+		STR R5, [R1,#8]
+		STR R6, [R1,#12]
+		STR R7, [R1,#16]
+		STR R8, [R1,#20]
+		STR R9, [R1,#24]
+		STR R10, [R1,#28]
+
+		ADD R1, R1, #32
+		SUBS R2, R2, #8
+		BGT Loop8
+
+	CheckLoop4:
+		CMP R2, #4
+		BLT CheckLoop1
+
+	Loop4: ; numElements >= 4, coalescing of 4 transfers
+		LDR R3, [R0,#0]
+		LDR R4, [R0,#0]
+		LDR R5, [R0,#0]
+		LDR R6, [R0,#0]
+
+		STR R3, [R1,#0]
+		STR R4, [R1,#4]
+		STR R5, [R1,#8]
+		STR R6, [R1,#12]
+
+		ADD R1, R1, #16
+		SUBS R2, R2, #4
+		BGT Loop4
+
+	CheckLoop1:
+		CMP R2, #1
+		BLT Exit
+
+	Loop1: ; numElements >= 1, transfer element by element
+		LDR R3, [R0,#0]
+		STR R3, [R1,#0]
+
+		ADD R1, R1, #4
+		SUBS R2, R2, #1
+		BGT Loop1
+
+	Exit:
+
+	END ReceiveMultiple;
+
+	OPERATOR "<<"*(port: Output; x: SET); BEGIN Send(port,SYSTEM.VAL(LONGINT,x)); END "<<";
+	OPERATOR ">>"*(x: SET; port: Output); BEGIN Send(port,SYSTEM.VAL(LONGINT,x)); END ">>";
+	OPERATOR "<<?"*(port: Output; x: SET): BOOLEAN; BEGIN RETURN SendNonBlocking(port,SYSTEM.VAL(LONGINT,x)); END "<<?";
+	OPERATOR ">>?"*(x: SET; port: Output): BOOLEAN; BEGIN RETURN SendNonBlocking(port,SYSTEM.VAL(LONGINT,x)); END ">>?";
+	OPERATOR ">>"*(port: Input; VAR x: SET); BEGIN Receive(port,SYSTEM.VAL(LONGINT,x)); END ">>";
+	OPERATOR "<<"*(VAR x: SET; port: Input); BEGIN Receive(port,SYSTEM.VAL(LONGINT,x)); END "<<";
+	OPERATOR ">>?"*(port: Input; VAR x: SET): BOOLEAN; BEGIN RETURN ReceiveNonBlocking(port,SYSTEM.VAL(LONGINT,x)); END ">>?";
+	OPERATOR "<<?"*(VAR x: SET; port: Input): BOOLEAN; BEGIN RETURN ReceiveNonBlocking(port,SYSTEM.VAL(LONGINT,x)); END "<<?";
+
+	OPERATOR "<<"*(port: Output; x: REAL); BEGIN Send(port,SYSTEM.VAL(LONGINT,x)); END "<<";
+	OPERATOR ">>"*(x: REAL; port: Output); BEGIN Send(port,SYSTEM.VAL(LONGINT,x)); END ">>";
+	OPERATOR "<<?"*(port: Output; x: REAL): BOOLEAN; BEGIN RETURN SendNonBlocking(port,SYSTEM.VAL(LONGINT,x)); END "<<?";
+	OPERATOR ">>?"*(x: REAL; port: Output): BOOLEAN; BEGIN RETURN SendNonBlocking(port,SYSTEM.VAL(LONGINT,x)); END ">>?";
+	OPERATOR ">>"*(port: Input; VAR x: REAL); BEGIN Receive(port,SYSTEM.VAL(LONGINT,x)); END ">>";
+	OPERATOR "<<"*(VAR x: REAL; port: Input); BEGIN Receive(port,SYSTEM.VAL(LONGINT,x)); END "<<";
+	OPERATOR ">>?"*(port: Input; VAR x: REAL): BOOLEAN; BEGIN RETURN ReceiveNonBlocking(port,SYSTEM.VAL(LONGINT,x)); END ">>?";
+	OPERATOR "<<?"*(VAR x: REAL; port: Input): BOOLEAN; BEGIN RETURN ReceiveNonBlocking(port,SYSTEM.VAL(LONGINT,x)); END "<<?";
+
+END AcAxisIo.
+

source/Zynq.DisplayLinear.Mod

@@ -0,0 +1,338 @@
+MODULE DisplayLinear;
+
+IMPORT SYSTEM, Displays, Plugins, Machine, Kernel, Commands, Options,
+	PsConfig, AcAxisIo, Video := AcStreamVideoOut, AcAxiDma, Trace;
+
+CONST
+	MaxWidth = 1920;
+	MaxHeight = 1080;
+
+	CacheLineSize = 32; (* cache line size in bytes *)
+	DmaBurstLen = 16;
+
+	(*
+		Video settings for 1024 x 768 @ 62 Hz
+	*)
+	Width* = 1024;
+	Height* = 768;
+
+	PlClkDiv0* = 10;
+	PlClkDiv1* = 15;
+	PlClkDiv2* = 3;
+
+	HorizFrontPorch* = 24;
+	HorizSyncWidth* = 136;
+	HorizBackPorch* = 160;
+	HorizSyncPolarity* = TRUE;
+
+	VertFrontPorch* = 3;
+	VertSyncWidth* = 6;
+	VertBackPorch* = 29;
+	VertSyncPolarity* = TRUE;
+
+(*
+	(*
+		Video settings for 800 x 480 @ 65 Hz
+	*)
+	Width* = 800;
+	Height* = 480;
+
+	PlClkDiv0* = 10;
+	PlClkDiv1* = 15*2;
+	PlClkDiv2* = 3*2;
+
+	HorizFrontPorch* = 40;
+	HorizSyncWidth* = 48;
+	HorizBackPorch* = 88;
+	HorizSyncPolarity* = TRUE;
+
+	VertFrontPorch* = 13;
+	VertSyncWidth* = 3;
+	VertBackPorch* = 32;
+	VertSyncPolarity* = TRUE;
+*)
+
+	DefaultColor* = LONGINT(0FFFFFFFFH); (** the color of the booting screen  *)
+
+TYPE
+
+	Display = OBJECT(Displays.Display)
+	CONST
+		Format = 4;
+
+		(** Transfer a block of pixels in "raw" display format to (op = set) or from (op = get) the display.  Pixels in the rectangular area are transferred from left to right and top to bottom.  The pixels are transferred to or from "buf", starting at "ofs".  The line byte increment is "stride", which may be positive, negative or zero. *)
+		PROCEDURE Transfer*(VAR buf: ARRAY OF CHAR; ofs, stride, x, y, w, h, op: LONGINT);
+		VAR bufadr, buflow, bufhigh, dispadr,w0,b,d: ADDRESS;
+		BEGIN
+
+			IF w > 0 THEN
+				ASSERT(fbadr # 0);
+				bufadr := ADDRESSOF(buf[ofs]);
+				dispadr := fbadr + y * fbstride + x * Format;
+				IF Displays.reverse THEN
+					dispadr := fbadr + (height-y-1) * fbstride + (width-x-1) * Format;
+				END;
+
+				ASSERT((dispadr >= fbadr) & ((y+h-1)*fbstride + (x+w-1)*Format <=  fbsize));	(* display index check *)
+				w := w * Format;	(* convert to bytes *)
+				CASE op OF
+					Displays.set:
+						IF Displays.reverse THEN
+							WHILE h > 0 DO
+								w0 := w DIV Format; b:= bufadr; d := dispadr;
+								WHILE w0 > 0 DO
+									SYSTEM.MOVE(b, d, Format);
+									INC(b,Format);
+									DEC(d, Format);
+									DEC(w0);
+								END;
+								INC(bufadr, stride); DEC(dispadr, fbstride);
+								DEC(h)
+							END
+						ELSE
+							w0 := w DIV Format;
+							WHILE h > 0 DO
+								Copy32(bufadr,dispadr,w0); (*SYSTEM.MOVE(bufadr, dispadr, w);*) Machine.FlushDCacheRange(dispadr,w);
+								INC(bufadr, stride); INC(dispadr, fbstride);
+								DEC(h)
+							END
+						END;
+					|Displays.get:
+						IF Displays.reverse THEN
+							buflow := ADDRESSOF(buf[0]); bufhigh := buflow + LEN(buf);
+							WHILE h > 0 DO
+								ASSERT((bufadr >= buflow) & (bufadr+w <= bufhigh));	(* index check *)
+								w0 := w DIV Format; b:= bufadr; d := dispadr;
+								WHILE w0 > 0 DO
+									SYSTEM.MOVE(d, b, Format);
+									INC(b,Format);
+									DEC(d, Format);
+									DEC(w0);
+								END;
+								INC(bufadr, stride); DEC(dispadr, fbstride);
+								DEC(h)
+							END;
+						ELSE
+							buflow := ADDRESSOF(buf[0]); bufhigh := buflow + LEN(buf);
+							WHILE h > 0 DO
+								ASSERT((bufadr >= buflow) & (bufadr+w <= bufhigh));	(* index check *)
+								SYSTEM.MOVE(dispadr, bufadr, w);
+								INC(bufadr, stride); INC(dispadr, fbstride);
+								DEC(h)
+							END;
+						END;
+					ELSE (* skip *)
+				END
+			END
+		END Transfer;
+
+	END Display;
+
+VAR
+	display: Display;
+	vout: Video.Controller;
+
+	pixelClock: REAL; (* pixel clock in Hz *)
+
+	buf: POINTER TO ARRAY OF CHAR;
+	bufAddr: ADDRESS;
+
+	rCfgCmd, rCfgData: AcAxisIo.Output;
+	rStatus: AcAxisIo.Input;
+	videoCfg: AcAxisIo.Output;
+	rdma: AcAxiDma.ReadController;
+
+	(*
+		Reset programming logic
+
+		polarity: reset signal polarity, TRUE for active high and FALSE for active low
+	*)
+	PROCEDURE ResetPl(polarity: BOOLEAN);
+	VAR res, t: LONGINT;
+	BEGIN
+		IF polarity THEN (* active high *)
+			ASSERT(PsConfig.SetPlResets({},res));
+			t := Kernel.GetTicks();
+			WHILE Kernel.GetTicks() - t < 1 DO END;
+			ASSERT(PsConfig.SetPlResets({0..3},res));
+		ELSE (* active low *)
+			ASSERT(PsConfig.SetPlResets({0..3},res));
+			t := Kernel.GetTicks();
+			WHILE Kernel.GetTicks() - t < 1 DO END;
+			ASSERT(PsConfig.SetPlResets({},res));
+		END;
+	END ResetPl;
+
+	(*
+		Setup clocks required for video streaming
+	*)
+	PROCEDURE SetupClocks;
+	VAR
+		res: LONGINT;
+		freq: HUGEINT;
+	BEGIN
+		(*
+			Setup DMA frequency
+		*)
+		freq := PsConfig.GetPllClockFrequency(PsConfig.IoPll,res);
+		Trace.String("IO PLL frequency is "); Trace.Int(freq,0); Trace.String(" Hz"); Trace.Ln;
+		ASSERT(PsConfig.SetPlResets({0,1,2,3},res));
+		IF PsConfig.SetPlClock(0,PsConfig.IoPll,PlClkDiv0,1,res) THEN
+			Trace.String("FPGA clock 0 frequency has been changed to "); Trace.Int(PsConfig.GetPlClockFrequency(0,res),0); Trace.String(" Hz"); Trace.Ln;
+		ELSE Trace.String("Error while setting FPGA clock 0 frequency, res="); Trace.Int(res,0); Trace.Ln;
+		END;
+
+		(*
+			Setup display clocks
+		*)		
+		(* pixel clock *)
+		pixelClock := REAL(freq)/PlClkDiv1;
+		IF PsConfig.SetPlClock(1,PsConfig.IoPll,PlClkDiv1,1,res) THEN
+			Trace.String("FPGA clock 1 frequency has been changed to "); Trace.Int(PsConfig.GetPlClockFrequency(1,res),0); Trace.String(" Hz"); Trace.Ln;
+		ELSE Trace.String("Error while setting FPGA clock 1 frequency, res="); Trace.Int(res,0); Trace.Ln;
+		END;
+		(* clock used for serialization *)
+		IF PsConfig.SetPlClock(2,PsConfig.IoPll,PlClkDiv2,1,res) THEN
+			Trace.String("FPGA clock 2 frequency has been changed to "); Trace.Int(PsConfig.GetPlClockFrequency(2,res),0); Trace.String(" Hz"); Trace.Ln;
+		ELSE Trace.String("Error while setting FPGA clock 2 frequency, res="); Trace.Int(res,0); Trace.Ln;
+		END;
+	END SetupClocks;
+
+	PROCEDURE Init;
+	VAR
+		res: LONGINT;
+		d: LONGINT;
+	BEGIN
+		SetupClocks;
+
+		(*
+			Reset the programming logic
+		*)
+		ResetPl(FALSE);
+
+		(*
+			Setup ActiveCells components ports
+		*)
+		ASSERT(AcAxisIo.GetOutput(0,0, rCfgCmd));
+		ASSERT(AcAxisIo.GetOutput(0,1, rCfgData));
+		ASSERT(AcAxisIo.GetInput(0,0, rStatus));
+		ASSERT(AcAxisIo.GetOutput(0,2, videoCfg));
+
+		(*
+			Allocate frame buffer
+		*)
+		NEW(buf,CacheLineSize*((MaxHeight*MaxWidth*4+CacheLineSize-1) DIV CacheLineSize)+CacheLineSize);
+		bufAddr := ADDRESSOF(buf[0]);
+		bufAddr := bufAddr + (CacheLineSize - bufAddr MOD CacheLineSize); (* align to cache line size boundary *)
+		Trace.String("DisplayLinear: bufAddr0="); Trace.Hex(ADDRESSOF(buf[0]),-8); Trace.Ln;
+		Trace.String("DisplayLinear: bufAddr="); Trace.Hex(bufAddr,-8); Trace.Ln;
+		ASSERT(bufAddr MOD CacheLineSize = 0);
+		ASSERT(ADDRESSOF(buf[LEN(buf)-1]) >= bufAddr+MaxHeight*MaxWidth*4-1);
+
+		(*
+			Setup video streaming
+		*)
+		Video.InitController(vout,videoCfg,pixelClock);
+
+		Video.SetHorizActiveSize(vout,Width);
+		Video.SetHorizFrontPorch(vout,HorizFrontPorch);
+		Video.SetHorizSyncWidth(vout,HorizSyncWidth);
+		Video.SetHorizBackPorch(vout,HorizBackPorch);
+		Video.SetHorizSyncPolarity(vout,HorizSyncPolarity);
+
+		Video.SetVertActiveSize(vout,Height);
+		Video.SetVertFrontPorch(vout,VertFrontPorch);
+		Video.SetVertSyncWidth(vout,VertSyncWidth);
+		Video.SetVertBackPorch(vout,VertBackPorch);
+		Video.SetVertSyncPolarity(vout,VertSyncPolarity);
+
+		(*
+			Setup AXI DMA for transfering data from the frame buffer to the video output
+		*)
+
+		(* configure read channel of S_AXI_HP0 as a 32-bit interface *)
+		d := SYSTEM.GET32(0xF8008000); SYSTEM.PUT32(0xF8008000,SYSTEM.MSK(d,0xFFFFFFFE)+1);
+		
+		Machine.Fill32(bufAddr,Width*Height*4,DefaultColor); (* fill the framebuffer with the default color *)
+		Machine.FlushDCacheRange(bufAddr,Width*Height*4);
+
+		AcAxiDma.InitController(rdma,rCfgCmd,rCfgData,rStatus,4,16);
+		AcAxiDma.SetBurstLen(rdma,DmaBurstLen);
+
+		(* configure read DMA transfer *)
+		AcAxiDma.SetAddr(rdma,bufAddr);
+		AcAxiDma.SetCount(rdma,Width*Height);
+		AcAxiDma.SetWrap(rdma,TRUE); (* recurring transfer *)
+		
+		(*
+			Enable video output
+		*)
+		Video.Enable(vout,TRUE);
+		AcAxiDma.Start(rdma);
+		
+		(*
+			Install the display
+		*)
+		NEW(display);
+		display.width := Width;
+		display.height := Height;
+		display.offscreen := 0;
+		display.format := 4;
+		display.unit := 10000;
+		display.InitFrameBuffer(bufAddr,Width*Height*4,Width*4);
+		display.desc := "Linear framebuffer driver for Zynq";
+		display.Update;
+		Displays.registry.Add(display,res);
+		ASSERT(res = Plugins.Ok);
+	END Init;	
+
+	PROCEDURE Install*(context: Commands.Context);
+	VAR options: Options.Options;
+	BEGIN
+		IF context # NIL THEN
+			NEW(options);
+			options.Add("r", "reverse", Options.Flag);
+			IF options.Parse(context.arg, context.error) THEN
+				IF options.GetFlag("r") THEN Displays.Reverse() END;
+			END;
+		END;
+	END Install;
+
+	PROCEDURE -Copy32(sadr: ADDRESS; dadr: ADDRESS; len: LONGINT);
+	CODE
+		LDR	R0, [SP, #dadr]
+		LDR	R1, [SP, #len]
+		LDR	R2, [SP, #sadr]
+		MOV	R3, #0
+	loop:
+		CMP	R3, R1
+		BGE	end
+		LDR	R4, [R2, #0]
+		STR	R4, [R0, #0]
+		ADD	R0, R0, #4
+		ADD	R2, R2, #4
+		ADD	R3, R3, #1
+		B loop
+	end:
+		ADD SP, SP, 12
+	(*BEGIN
+		SYSTEM.MOVE(sadr, dadr, 4*len)*)
+	END Copy32;
+
+BEGIN
+	Init;
+END DisplayLinear.
+
+SystemTools.DoCommands
+
+WinDisks.Install I: RW ~
+FSTools.Mount AOS AosFS PhysicalDrive4#3 ~
+
+SystemTools.DoCommands
+
+FSTools.CopyFiles -o WORK:/build/DisplayLinear.Gof => AOS:/DisplayLinear.Gof ~
+FSTools.CopyFiles -o WORK:/build/DisplayLinear.Sym => AOS:/DisplayLinear.Sym ~
+
+FSTools.Unmount AOS ~
+WinDisks.Uninstall PhysicalDrive4 ~
+~

source/Zynq.Gpio.Mod

@@ -0,0 +1,103 @@
+MODULE Gpio; (** AUTHOR "Timothée Martiel"; PURPOSE "GPIO driver"; *)
+
+(*! TODO:
+	- Interrupt configuration
+*)
+
+IMPORT Platform;
+
+CONST
+	(** Direction: Input *)
+	Input *		= FALSE;
+	(** Direction: Output *)
+	Output *	= TRUE;
+
+TYPE
+	DataReg = POINTER {UNSAFE,UNTRACED} TO RECORD
+		DATA: ARRAY 4 OF SET;
+		DATA_RO: ARRAY 4 OF SET;
+	END;
+
+	BankCtrlReg = POINTER {UNSAFE,UNTRACED} TO RECORD
+		DIRM, OEN, INT_MASK, INT_EN, INT_DIS, INT_STAT, INT_TYPE, INT_POLARITY, INT_ANY: SET;
+	END;
+
+VAR
+	bankCtrlRegs: ARRAY 4 OF BankCtrlReg;
+	dataRegs: DataReg;
+	i: LONGINT;
+
+	(** Set the direction of 'gpio' to output if 'out' is TRUE and to input otherwise. *)
+	PROCEDURE SetDirection * (gpio: LONGINT; out: BOOLEAN);
+	VAR
+		bank, ofs: LONGINT;
+	BEGIN
+		GetBankOfs(gpio, bank, ofs);
+		IF (bank < 0) & (ofs < 0) THEN RETURN END;
+
+		IF out THEN
+			INCL(bankCtrlRegs[bank].DIRM, ofs)
+		ELSE
+			EXCL(bankCtrlRegs[bank].DIRM, ofs)
+		END
+	END SetDirection;
+
+	(** If 'on' enable (if 'off' disable) output for 'gpio'. 'gpio' direction must be set to output prior to this call. *)
+	PROCEDURE EnableOutput * (gpio: LONGINT; on: BOOLEAN);
+	VAR
+		bank, ofs: LONGINT;
+	BEGIN
+		GetBankOfs(gpio, bank, ofs);
+		IF (bank < 0) & (ofs < 0) THEN RETURN END;
+
+		IF on THEN
+			INCL(bankCtrlRegs[bank].OEN, ofs)
+		ELSE
+			EXCL(bankCtrlRegs[bank].OEN, ofs)
+		END
+	END EnableOutput;
+
+	(** Set the data of output GPIO 'gpio' to 'data' (TRUE for high, FALSE for low). *)
+	PROCEDURE SetData * (gpio: LONGINT; data: BOOLEAN);
+	VAR
+		bank, ofs: LONGINT;
+	BEGIN
+		GetBankOfs(gpio, bank, ofs);
+		IF (bank < 0) & (ofs < 0) THEN RETURN END;
+
+		IF data THEN
+			INCL(dataRegs.DATA[bank], ofs)
+		ELSE
+			EXCL(dataRegs.DATA[bank], ofs)
+		END
+	END SetData;
+
+	(** Get data of input GPIO 'gpio': TRUE for high, FALSE for low. *)
+	PROCEDURE GetData * (gpio: LONGINT): BOOLEAN;
+	VAR
+		bank, ofs: LONGINT;
+	BEGIN
+		GetBankOfs(gpio, bank, ofs);
+		IF (bank < 0) & (ofs < 0) THEN HALT(100) END;
+
+		RETURN ofs IN dataRegs.DATA_RO[bank]
+	END GetData;
+
+	PROCEDURE GetBankOfs (gpio: LONGINT; VAR bank, ofs: LONGINT);
+	BEGIN
+		IF gpio < 54 THEN
+			bank := gpio DIV 32;
+			ofs := gpio MOD 32
+		ELSE
+			(*! TODO: implement *)
+		END;
+		bank := -1;
+		ofs := -1
+	END GetBankOfs;
+
+BEGIN
+	dataRegs := Platform.GpioData;
+	FOR i := 0 TO Platform.GpioBankNb - 1 DO
+		bankCtrlRegs[i] := Platform.GpioBank[i]
+	END
+END Gpio.

source/Zynq.PrecisionTimer.Mod

@@ -0,0 +1,30 @@
+MODULE PrecisionTimer; (** AUTHOR ""; PURPOSE ""; *)
+
+IMPORT
+	Machine, BootConfig;
+
+TYPE
+	Counter* = HUGEINT;
+
+	(**
+		Query timer counter in ticks
+	*)
+	PROCEDURE GetCounter*(): Counter;
+	BEGIN
+		RETURN Machine.GetTimer();
+	END GetCounter;
+
+	(**
+		Query timer tick frequency in Hz
+	*)
+	PROCEDURE GetFrequency*(): Counter;
+	BEGIN
+		RETURN frequencyInHz;
+	END GetFrequency;
+
+VAR
+	frequencyInHz: Counter;
+
+BEGIN
+	frequencyInHz := BootConfig.GetIntValue("CpuClockHz");
+END PrecisionTimer.

source/Zynq.PrivateWatchdog.Mod

@@ -0,0 +1,62 @@
+MODULE PrivateWatchdog; (** AUTHOR "Timothée Martiel, 11/2017"; PURPOSE "Zynq private watchdog driver"; *)
+IMPORT SYSTEM, Platform;
+
+CONST
+	(** Modes *)
+	Reset * = TRUE; (** Resets the system *)
+	Interrupt * = FALSE; (** Triggers an interrupt *)
+
+	ControlWatchdogEnable = 0;
+	ControlAutoReload = 1;
+	ControlItEnable = 2;
+	ControlWdMode = 3;
+	ControlPrescalerOfs = 8;
+	ControlPrescalerMask = {8 .. 15};
+
+VAR
+	frequency: HUGEINT;
+
+	(** Start the private watchdog with the given mode and delay *)
+	PROCEDURE Start * (mode: BOOLEAN; delay: LONGINT);
+	VAR
+		val: SET;
+	BEGIN
+		ASSERT(frequency > 0);
+		Platform.mpcore.Watchdog_Reset_Status_Register := 1; (* Clear the reset status *)
+		Feed(delay);
+		val := {ControlWatchdogEnable};
+		IF mode THEN
+			(* Reset *)
+			INCL(val, ControlWdMode)
+		ELSE
+			INCL(val, ControlItEnable)
+		END;
+		Platform.mpcore.Watchdog_Control_Register := SYSTEM.VAL(LONGINT, val)
+	END Start;
+
+	(** Stop private watchdog *)
+	PROCEDURE Stop *;
+	BEGIN
+		Platform.mpcore.Watchdog_Disable_Register := Platform.PrivateWatchdogDisableKey0;
+		Platform.mpcore.Watchdog_Disable_Register := LONGINT(Platform.PrivateWatchdogDisableKey1);
+		Platform.mpcore.Watchdog_Control_Register := 0
+	END Stop;
+
+	(** Feed the watchdog: overwrites its count with the given delay *)
+	PROCEDURE Feed * (delay: LONGINT);
+	BEGIN
+		Platform.mpcore.Watchdog_Load_Register := LONGINT(HUGEINT(delay) * frequency);
+	END Feed;
+
+	(** Check if the watchdog has been triggered *)
+	PROCEDURE Triggered * (): BOOLEAN;
+	BEGIN
+		RETURN Platform.mpcore.Watchdog_Reset_Status_Register = 1
+	END Triggered;
+
+	(** Initialise watchdog reference frequency (CPU freq / 2) *)
+	PROCEDURE Init * (timerFrequency: HUGEINT);
+	BEGIN
+		frequency := timerFrequency
+	END Init;
+END PrivateWatchdog.

source/Zynq.PsConfig.Mod

@@ -0,0 +1,376 @@
+(**
+	AUTHOR "Alexey Morozov, HighDim GmbH, 2015";
+	PURPOSE "Interface for system level configuration/control of Zynq Processing System (PS)";
+*)
+MODULE PsConfig;
+
+IMPORT
+	SYSTEM, Platform, BootConfig, Trace;
+
+CONST
+	(** error codes *)
+	Ok* = 0;
+	InvalidChannel* = 1; (** invalid channel (e.g. PL clock channel) specified *)
+	InvalidClockSource* = 2; (** invalid clock source specified *)
+	InvalidDivisor* = 3; (** invalid clock divisor value specified *)
+	InvalidModule* = 4; (** invalid I/O module specified *)
+	InvalidDevice* = 5; (** invalid I/O device for specified module *)
+
+	UnknownError* = 256;
+
+	(** PLL clock source types *)
+	IoPll* = 0; (** IO PLL used as a clock source for IO peripherals *)
+	ArmPll* = 1; (** ARM PLL used as a clock source for the CPU *)
+	DdrPll* = 3; (** DDR PLL used as a clock source for DDR memory *)
+
+	(** I/O Clock Modules *)
+	IoUsb* = 0;
+	IoGem* = 1;
+	IoSdio* = 2;
+	IoSmc* = 3;
+	IoSpi* = 4;
+	IoQuadSpi* = 5;
+	IoUart* = 6;
+	IoCan* = 7;
+	IoGpio* = 8;
+	IoI2c* = 9;
+
+VAR
+	psRefClockHz: HUGEINT;
+
+	(**
+		Get clock frequency of a given PLL clock source
+
+		srcSel: source selector (either of IoPll, ArmPll, DdrPll)
+		res: result code; zero in case of success
+
+		Returns the frequency in Hz
+	*)
+	PROCEDURE GetPllClockFrequency*(srcSel: LONGINT; VAR res: LONGINT): HUGEINT;
+	BEGIN
+		CASE srcSel OF
+			|IoPll: RETURN HUGEINT(LSH(SYSTEM.MSK(Platform.slcr.IO_PLL_CTRL,0x7F000),-12)) * psRefClockHz;
+			|ArmPll: RETURN HUGEINT(LSH(SYSTEM.MSK(Platform.slcr.ARM_PLL_CTRL,0x7F000),-12)) * psRefClockHz;
+			|DdrPll: RETURN HUGEINT(LSH(SYSTEM.MSK(Platform.slcr.DDR_PLL_CTRL,0x7F000),-12)) * psRefClockHz;
+		ELSE
+			res := InvalidClockSource; RETURN 0;
+		END;
+	END GetPllClockFrequency;
+
+	(**
+		Setup reset signals to programming logic
+
+		assertedChannels: specifies the set of PL channels with asserted reset; can include 0, 1, 2, 3
+		res: result code; zero in case of success
+
+		Returns TRUE in case of success
+	*)
+	PROCEDURE SetPlResets*(assertedChannels: SET; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+		IF assertedChannels * {4..31} # {} THEN res := InvalidChannel; RETURN FALSE; END;
+
+		Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey; (* enable writing to SLCR registers *)
+		Platform.slcr.FPGA_RST_CTRL := assertedChannels;
+		Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+
+		res := Ok;
+		RETURN TRUE;
+	END SetPlResets;
+
+	(**
+		Setup a given channel of Programming Logic (PL) clock
+
+		channel: selected channel (either of 0, 1, 2, 3)
+		srcSel: source selector (either of IoPll, ArmPll, DdrPll)
+		divisor0: provides the divisor used to divide the source clock to generate the required generated clock frequency. First cascade divider.
+		divisor1: provides the divisor used to divide the source clock to generate the required generated clock frequency. Second cascade divider.
+		res: result code; zero in case of success
+
+		Returns TRUE in case of success
+	*)
+	PROCEDURE SetPlClock*(channel: LONGINT; srcSel: LONGINT; divisor0, divisor1: LONGINT; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+		IF (srcSel > 0) OR (srcSel > 3) THEN res := InvalidClockSource; RETURN FALSE; END;
+		IF (divisor0 < 1) OR (divisor0 > 63) OR (divisor1 < 0) OR (divisor1 > 63) THEN res := InvalidDivisor; RETURN FALSE; END;
+
+		Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey; (* enable writing to SLCR registers *)
+
+		CASE channel OF
+			|0: Platform.slcr.FPGA0_CLK_CTRL := srcSel + LSH(divisor0,8) + LSH(divisor1,20);
+			|1: Platform.slcr.FPGA1_CLK_CTRL := srcSel + LSH(divisor0,8) + LSH(divisor1,20);
+			|2: Platform.slcr.FPGA2_CLK_CTRL := srcSel + LSH(divisor0,8) + LSH(divisor1,20);
+			|3: Platform.slcr.FPGA3_CLK_CTRL := srcSel + LSH(divisor0,8) + LSH(divisor1,20);
+		ELSE
+			Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+			res := InvalidChannel;
+			RETURN FALSE;
+		END;
+
+		Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+
+		res := Ok;
+		RETURN TRUE;
+	END SetPlClock;
+
+	(**
+		Get clock frequency of a given PL clock channel
+
+		res: result code; zero in case of success
+
+		Returns the frequency in Hz
+	*)
+	PROCEDURE GetPlClockFrequency*(channel: LONGINT; VAR res: LONGINT): HUGEINT;
+	VAR
+		d, srcSel, divisor0, divisor1: LONGINT;
+	BEGIN
+		CASE channel OF
+			|0: d := Platform.slcr.FPGA0_CLK_CTRL;
+			|1: d := Platform.slcr.FPGA1_CLK_CTRL;
+			|2: d := Platform.slcr.FPGA2_CLK_CTRL;
+			|3: d := Platform.slcr.FPGA3_CLK_CTRL;
+		ELSE
+			res := InvalidChannel;
+			RETURN 0;
+		END;
+
+		srcSel := LSH(SYSTEM.MSK(d,0x30),-4);
+		divisor0 := LSH(SYSTEM.MSK(d,0x3F00),-8);
+		divisor1 := LSH(SYSTEM.MSK(d,0x3F00000),-20);
+
+		RETURN GetPllClockFrequency(srcSel,res) DIV (divisor0*divisor1);
+	END GetPlClockFrequency;
+
+	(**
+		Stop a given PL clock
+
+		channel: clock channel number
+		res: result code; zero in case of success
+
+		Returns TRUE in case of success
+	*)
+	PROCEDURE StopPlClock*(channel: LONGINT; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+
+		Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey; (* enable writing to SLCR registers *)
+
+		CASE channel OF
+			|0: Platform.slcr.FPGA0_THR_CNT := 1;
+			|1: Platform.slcr.FPGA1_THR_CNT := 1;
+			|2: Platform.slcr.FPGA2_THR_CNT := 1;
+			|3: Platform.slcr.FPGA3_THR_CNT := 1;
+		ELSE
+			Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+			res := InvalidChannel;
+			RETURN FALSE;
+		END;
+
+		Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+
+		res := Ok;
+		RETURN TRUE;
+	END StopPlClock;
+
+	(**
+		Start a given PL clock
+
+		channel: clock channel number
+		res: result code; zero in case of success
+
+		Returns TRUE in case of success
+	*)
+	PROCEDURE StartPlClock*(channel: LONGINT; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+		Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey; (* enable writing to SLCR registers *)
+
+		CASE channel OF
+			|0: Platform.slcr.FPGA0_THR_CNT := 0;
+			|1: Platform.slcr.FPGA1_THR_CNT := 0;
+			|2: Platform.slcr.FPGA2_THR_CNT := 0;
+			|3: Platform.slcr.FPGA3_THR_CNT := 0;
+		ELSE
+			Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+			res := InvalidChannel;
+			RETURN FALSE;
+		END;
+
+		Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+
+		res := Ok;
+		RETURN TRUE;
+	END StartPlClock;
+
+	(**
+		Stop given PL clocks
+
+		channels: a set of clock channels to stop
+		res: result code; zero in case of success
+
+		Returns TRUE in case of success
+	*)
+	PROCEDURE StopPlClocks*(channels: SET; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+		IF channels * {0,1,2,3} = {} THEN res := InvalidChannel; RETURN FALSE; END;
+
+		Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey; (* enable writing to SLCR registers *)
+
+		IF 0 IN channels THEN Platform.slcr.FPGA0_THR_CNT := 1; END;
+		IF 1 IN channels THEN Platform.slcr.FPGA1_THR_CNT := 1; END;
+		IF 2 IN channels THEN Platform.slcr.FPGA2_THR_CNT := 1; END;
+		IF 3 IN channels THEN Platform.slcr.FPGA3_THR_CNT := 1; END;
+
+		Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+
+		res := Ok;
+		RETURN TRUE;
+	END StopPlClocks;
+
+	(**
+		Start given PL clocks
+
+		channels: a set of clock channels to start
+		res: result code; zero in case of success
+
+		Returns TRUE in case of success
+	*)
+	PROCEDURE StartPlClocks*(channels: SET; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+		IF channels * {0,1,2,3} = {} THEN res := InvalidChannel; RETURN FALSE; END;
+
+		Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey; (* enable writing to SLCR registers *)
+
+		IF 0 IN channels THEN Platform.slcr.FPGA0_THR_CNT := 0; END;
+		IF 1 IN channels THEN Platform.slcr.FPGA1_THR_CNT := 0; END;
+		IF 2 IN channels THEN Platform.slcr.FPGA2_THR_CNT := 0; END;
+		IF 3 IN channels THEN Platform.slcr.FPGA3_THR_CNT := 0; END;
+
+		Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+
+		res := Ok;
+		RETURN TRUE;
+	END StartPlClocks;
+
+	PROCEDURE GetIoClockFrequency*(module: LONGINT; VAR res: LONGINT): HUGEINT;
+	VAR
+		baseFreq: HUGEINT;
+		val: LONGINT;
+	BEGIN
+		CASE module OF
+			 IoUsb: (*!TODO*)
+			|IoGem: (*!TODO*)
+			|IoSdio: val := Platform.slcr.SDIO_CLK_CTRL;
+			|IoSmc: val := Platform.slcr.SMC_CLK_CTRL;
+			|IoSpi: val := Platform.slcr.SPI_CLK_CTRL;
+			|IoQuadSpi: val := Platform.slcr.LQSPI_CLK_CTRL;
+			|IoUart: val := Platform.slcr.UART_CLK_CTRL;
+			|IoCan: (*!TODO*)
+			|IoGpio: (*!TODO*)
+			|IoI2c: (*!TODO*)
+		ELSE
+			res := InvalidModule;
+			RETURN 0;
+		END;
+		CASE module OF
+			 IoUsb: (*!TODO*)
+			|IoGem: (*!TODO*)
+			|IoSdio, IoSmc, IoSpi, IoQuadSpi, IoUart:
+				val := LSH(SYSTEM.MSK(val, 0x3f00), -8)
+			|IoCan:(*!TODO*)
+			|IoGpio:(*!TODO*)
+			|IoI2c:(*!TODO*)
+		END;
+		baseFreq := GetPllClockFrequency(GetIoClockSource(module, res), res);
+		IF res # Ok THEN RETURN 0 END;
+		RETURN baseFreq DIV val
+	END GetIoClockFrequency;
+
+	PROCEDURE GetIoClockSource*(module: LONGINT; VAR res: LONGINT): LONGINT;
+	VAR
+		pll, val: LONGINT;
+	BEGIN
+		res := Ok;
+		CASE module OF
+			 IoUsb: (*!TODO*)
+			|IoGem: (*!TODO*)
+			|IoSdio: val := Platform.slcr.SDIO_CLK_CTRL;
+			|IoSmc: val := Platform.slcr.SMC_CLK_CTRL;
+			|IoSpi: val := Platform.slcr.SPI_CLK_CTRL;
+			|IoQuadSpi: val := Platform.slcr.LQSPI_CLK_CTRL;
+			|IoUart: val := Platform.slcr.UART_CLK_CTRL;
+			|IoCan: (*!TODO*)
+			|IoGpio: (*!TODO*)
+			|IoI2c: (*!TODO*)
+		ELSE
+			res := InvalidModule;
+			RETURN -1;
+		END;
+		CASE module OF
+			 IoUsb: (*!TODO*)
+			|IoGem: (*!TODO*)
+			|IoSdio, IoSmc, IoSpi, IoQuadSpi, IoUart:
+				pll := LSH(SYSTEM.MSK(val, 0x30), -4);
+				IF pll = 2 THEN pll := ArmPll END;
+			|IoCan:(*!TODO*)
+			|IoGpio:(*!TODO*)
+			|IoI2c:(*!TODO*)
+		END;
+		RETURN pll
+	END GetIoClockSource;
+
+	PROCEDURE SetIoClockFrequency*(module: LONGINT; freq: HUGEINT; VAR res: LONGINT): BOOLEAN;
+	VAR
+		baseFreq: HUGEINT;
+		val, div: LONGINT;
+	BEGIN
+		res := Ok;
+		CASE module OF
+			 IoUsb: (*!TODO*)
+			|IoGem: (*!TODO*)
+			|IoSdio: val := Platform.slcr.SDIO_CLK_CTRL;
+			|IoSmc: val := Platform.slcr.SMC_CLK_CTRL;
+			|IoSpi: val := Platform.slcr.SPI_CLK_CTRL;
+			|IoQuadSpi: val := Platform.slcr.LQSPI_CLK_CTRL;
+			|IoUart: val := Platform.slcr.UART_CLK_CTRL;
+			|IoCan: (*!TODO*)
+			|IoGpio: (*!TODO*)
+			|IoI2c: (*!TODO*)
+		ELSE
+			res := InvalidModule;
+			RETURN FALSE;
+		END;
+		baseFreq := GetPllClockFrequency(GetIoClockSource(module, res), res);
+		IF res # Ok THEN RETURN FALSE END;
+
+		Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey; (* enable writing to SLCR registers *)
+		CASE module OF
+			 IoUsb: (*!TODO*)
+			|IoGem: (*!TODO*)
+			|IoSdio, IoSmc, IoSpi, IoQuadSpi, IoUart:
+				div := LONGINT(baseFreq DIV freq);
+				val := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, val) - {8 .. 13} + SYSTEM.VAL(SET, LSH(div, 8)) * {8 .. 13});
+				Platform.slcr.SDIO_CLK_CTRL := val
+			|IoCan:(*!TODO*)
+			|IoGpio:(*!TODO*)
+			|IoI2c:(*!TODO*)
+		END;
+		Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey; (* disable writing to SLCR registers *)
+		RETURN TRUE
+	END SetIoClockFrequency;
+
+	PROCEDURE SetIoClockSource*(module, source: LONGINT; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+		
+	END SetIoClockSource;
+
+	PROCEDURE StartIoClock*(module, device: LONGINT; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+		
+	END StartIoClock;
+
+	PROCEDURE StopIoClock*(module, device: LONGINT; VAR res: LONGINT): BOOLEAN;
+	BEGIN
+		
+	END StopIoClock;
+
+BEGIN
+	psRefClockHz := BootConfig.GetIntValue("PsRefClockHz");
+END PsConfig.

source/Zynq.PsSerials.Mod

@@ -0,0 +1,136 @@
+MODULE PsSerials; (** AUTHOR "Timothée Martiel, 11/2017"; PURPOSE "Serial interface for Zynq PS UARTs"; *)
+
+IMPORT Platform, BootConfig, Modules, Strings, PsUartMin, PsUart, Serials, Objects, Machine;
+
+TYPE
+	Port = OBJECT (Serials.Port)
+	VAR
+		uart: PsUart.UartController;
+		
+		sendCharLock := FALSE : BOOLEAN;
+
+		PROCEDURE & Init (id: LONGINT);
+		BEGIN
+			uart := PsUart.GetUart(id);
+			ASSERT(uart # NIL);
+		END Init;
+
+		PROCEDURE Open (bps, data, parity, stop : LONGINT; VAR res: LONGINT);
+		BEGIN{EXCLUSIVE}
+			PsUart.Open(uart, bps, data, parity, stop, res);
+		END Open;
+
+		PROCEDURE Close;
+		BEGIN{EXCLUSIVE}
+			PsUart.Close(uart);
+		END Close;
+
+		PROCEDURE Yield(VAR res: LONGINT): BOOLEAN;
+		BEGIN
+			IF uart.open THEN
+				Objects.Yield;
+				RETURN TRUE;
+			ELSE
+				res := Serials.Closed;
+				RETURN FALSE;
+			END;
+		END Yield;
+
+		PROCEDURE Yield0(VAR res: LONGINT): BOOLEAN;
+		BEGIN
+			IF uart.open THEN
+				RETURN TRUE;
+			ELSE
+				res := Serials.Closed;
+				RETURN FALSE;
+			END;
+		END Yield0;
+
+		PROCEDURE SendChar(char: CHAR; VAR res: LONGINT);
+		BEGIN
+			Machine.AcquireObject(sendCharLock);
+			(*! use Yield0 method to make sure no low-level lock is acquired here - required when used as trace output *)
+			PsUart.SendChar(uart, char, TRUE, Yield0, res);
+		FINALLY
+			Machine.ReleaseObject(sendCharLock);
+		END SendChar;
+
+		PROCEDURE Send(CONST buf: ARRAY OF CHAR; ofs, len: LONGINT; propagate: BOOLEAN; VAR res: LONGINT);
+		BEGIN
+			PsUart.Send(uart, buf, ofs, len, propagate, Yield, res);
+		END Send;
+
+		PROCEDURE ReceiveChar(VAR char: CHAR; VAR res: LONGINT);
+		BEGIN
+			char := PsUart.ReceiveChar(uart, Yield, res);
+		END ReceiveChar;
+
+		PROCEDURE Receive(VAR buf: ARRAY OF CHAR; ofs, size, min: LONGINT; VAR len, res: LONGINT);
+		BEGIN
+			PsUart.Receive(uart, buf, ofs, size, min, len, Yield, res);
+		END Receive;
+
+		PROCEDURE Available(): LONGINT;
+		BEGIN
+			RETURN PsUart.Available(uart);
+		END Available;
+
+		PROCEDURE GetPortState(VAR openstat : BOOLEAN; VAR bps, data, parity, stop : LONGINT);
+		BEGIN{EXCLUSIVE}
+			openstat := uart.open;
+			bps := uart.bps;
+			data := uart.data;
+			parity := uart.parity;
+			stop := uart.stop;
+		END GetPortState;
+
+	END Port;
+
+VAR
+	ports: ARRAY 2 OF Port;
+
+	PROCEDURE Init;
+	VAR
+		i, clk, res: LONGINT;
+		name, desc: ARRAY 32 OF CHAR;
+		tracePort: LONGINT;
+	BEGIN
+		clk := BootConfig.GetIntValue("UartInputClockHz");
+		FOR i := 0 TO LEN(Platform.UartBase,0)-1 DO
+			PsUart.Install(i, Platform.UartBase[i], clk, res);
+		END;
+
+		(* if one of PS UART's is used for tracing, make the corresponding port open to allow trace output via PsUartMin *)
+		tracePort := BootConfig.GetIntValue("TracePort");
+		IF (tracePort >= 1) & (tracePort <= LEN(Platform.UartBase,0)) THEN
+			PsUart.Open(PsUart.GetUart(tracePort-1), BootConfig.GetIntValue("TraceBPS"),PsUartMin.DefaultDataBits,PsUartMin.DefaultParity,PsUartMin.DefaultStop, res);
+		ELSE tracePort := -1;
+		END;
+
+		FOR i := 0 TO LEN(ports) - 1 DO
+			NEW(ports[i], i);
+			name := "UART";
+			Strings.AppendInt(name, i+1);
+			desc := "Zynq PS UART";
+			Strings.AppendInt(desc, i);
+			Serials.RegisterOnboardPort(i+1, ports[i], name, desc);
+		END;
+
+		IF tracePort >= 0 THEN
+			(* switch from PsUartMin to port-based trace output *)
+			Serials.SetTracePort(tracePort, BootConfig.GetIntValue("TraceBPS"),PsUartMin.DefaultDataBits,PsUartMin.DefaultParity,PsUartMin.DefaultStop, res);
+		END;
+		Modules.InstallTermHandler(Cleanup)
+	END Init;
+
+	PROCEDURE Cleanup;
+	VAR i: LONGINT;
+	BEGIN
+		FOR i := 0 TO LEN(ports) - 1 DO
+			IF ports[i] # NIL THEN Serials.UnRegisterPort(ports[i]) END
+		END
+	END Cleanup;
+
+BEGIN
+	Init
+END PsSerials.

source/Zynq.PsTraceDevice.Mod

@@ -0,0 +1,37 @@
+MODULE TraceDevice;
+(**
+	AUTHOR: Timothee Martiel, Alexey Morozov, HighDim GmbH, 2013-2017
+	PURPOSE: simple abtraction for installing kernel tracing over Zynq PS UART
+*)
+
+IMPORT SYSTEM, Platform, BootConfig, Trace, PsUartMin;
+
+VAR
+	uart: PsUartMin.UartRegisters;
+
+	PROCEDURE TraceChar(ch: CHAR);
+	VAR ignore: LONGINT;
+	BEGIN
+		PsUartMin.SendChar(uart, ch, TRUE, NIL, ignore);
+	END TraceChar;
+
+	PROCEDURE Install *;
+	VAR
+		uartId: LONGINT;
+		res: LONGINT;
+	BEGIN
+		Trace.Init;
+		uartId := BootConfig.GetIntValue("TracePort")-1;
+
+		PsUartMin.Install(uartId, Platform.UartBase[uartId], BootConfig.GetIntValue("UartInputClockHz"), res);
+		IF res # 0 THEN RETURN; END;
+
+		uart := PsUartMin.GetUart(uartId);
+
+		IF ~PsUartMin.SetBps(uart, BootConfig.GetIntValue("TraceBPS"), res) THEN RETURN; END;
+		PsUartMin.Enable(uart,TRUE);
+
+		Trace.Char := TraceChar;
+	END Install;
+
+END TraceDevice.

source/Zynq.PsUart.Mod

@@ -0,0 +1,541 @@
+MODULE PsUart;
+(**
+	AUTHOR: Alexey Morozov, Timothee Martiel, HighDim GmbH, 2013-2018
+	PURPOSE: driver implementation for Xilinx Zynq UART PS controller
+*)
+
+IMPORT SYSTEM, UartMin := PsUartMin, PsUartInterrupts, Trace;
+
+CONST
+	(** Receive errors - compatible with A2 Serials *)
+	OverrunError* = 10;
+	ParityError* = 11;
+	FramingError* = 12;
+	BreakInterrupt* = 13;
+
+	DefaultRxBufSize* = 4096;
+	DefaultTxBufSize* = 4096;
+
+	ReceiveTimeoutInUs* = 500; (** Receive timeout in microseconds *)
+
+	(* RX data interrupts *)
+	RxDataInterrupts = {UartMin.XUARTPS_IXR_TOUT , UartMin.XUARTPS_IXR_RXFULL , UartMin.XUARTPS_IXR_RXOVR};
+
+	(* RX error interrupts *)
+	RxErrorInterrupts = {UartMin.XUARTPS_IXR_PARITY , UartMin.XUARTPS_IXR_FRAMING , UartMin.XUARTPS_IXR_OVER};
+
+	(* TX data interrupts *)
+	TxDataInterrupts = {UartMin.XUARTPS_IXR_TXEMPTY, UartMin.XUARTPS_IXR_TTRIG};
+
+	(* TX error interrupts *)
+	TxErrorInterrupts = {UartMin.XUARTPS_IXR_TOVR};
+
+TYPE
+
+	UartController* = POINTER TO RECORD
+		id-: LONGINT; (** UART controller ID *)
+		regs-: UartMin.UartRegisters; (** controller registers *)
+		inputClock-: LONGINT; (** controller input clock in Hz *)
+		bps-, data-, parity-, stop-: LONGINT; (** current parameter values *)
+
+		open-: BOOLEAN; (** TRUE if the controller is open *)
+
+		rxBuf: POINTER TO ARRAY OF CHAR; (* receive (RX) circular buffer *)
+		rxBufRdPos, rxBufWrPos: LONGINT; (* RX buffer read and write positions *)
+
+		txBuf: POINTER TO ARRAY OF CHAR; (* transmit (TX) circular buffer *)
+		txBufRdPos, txBufWrPos: LONGINT; (* TX buffer read and write positions *)
+
+		errors: SET;
+	END;
+
+VAR
+	uarts: ARRAY 2 OF UartController;
+
+	(* Disable all UART interrupts *)
+	PROCEDURE DisableInterrupts(regs: UartMin.UartRegisters);
+	BEGIN
+		regs.idr := UartMin.XUARTPS_IXR_MASK;
+	END DisableInterrupts;
+
+	PROCEDURE IntrHandler(param: ANY);
+	VAR
+		uart: UartController;
+		intrStatus: SET;
+	BEGIN
+		uart := param(UartController);
+		(*Trace.String("imr="); Trace.Set(uart.regs.imr); Trace.Ln;
+		Trace.String("isr="); Trace.Set(uart.regs.isr); Trace.Ln;*)
+
+		intrStatus := uart.regs.imr * uart.regs.isr;
+		uart.regs.isr := intrStatus; (* clear the interrupt *)
+
+		(*Trace.String("intrStatus="); Trace.Set(intrStatus); Trace.Ln;*)
+
+		IF intrStatus * (RxDataInterrupts+RxErrorInterrupts) # {} THEN
+			IntrHandlerRx(uart,intrStatus);
+		END;
+
+		IF intrStatus * TxDataInterrupts # {} THEN
+			IntrHandlerTx(uart,intrStatus);
+		END;
+	END IntrHandler;
+
+	PROCEDURE IntrHandlerRx(uart: UartController; intrStatus: SET);
+	VAR
+		bufWrPos: LONGINT;
+	BEGIN
+
+		IF intrStatus * RxErrorInterrupts # {} THEN
+			IF UartMin.XUARTPS_IXR_OVER IN intrStatus THEN
+				INCL(uart.errors,OverrunError);
+				Trace.String("---rx overrun(1)---: intrStatus="); Trace.Set(intrStatus); Trace.Ln;
+				RETURN;
+			END;
+		END;
+
+		bufWrPos := uart.rxBufWrPos;
+
+		WHILE ~(UartMin.XUARTPS_SR_RXEMPTY IN uart.regs.sr) DO
+
+			uart.rxBuf[bufWrPos] := CHR(uart.regs.fifo);
+			INC(bufWrPos);
+			IF bufWrPos = LEN(uart.rxBuf) THEN
+				bufWrPos := 0;
+			END;
+
+			IF bufWrPos = uart.rxBufRdPos THEN
+				INCL(uart.errors,OverrunError);
+				Trace.String("---rx overrun(2)---: intrStatus="); Trace.Set(intrStatus); Trace.Ln;
+				RETURN;
+			END;
+		END;
+
+		uart.rxBufWrPos := bufWrPos;
+	END IntrHandlerRx;
+
+	PROCEDURE IntrHandlerTx(uart: UartController; intrStatus: SET);
+	VAR bufRdPos: LONGINT;
+	BEGIN
+		IF intrStatus * TxErrorInterrupts # {} THEN
+			IF UartMin.XUARTPS_IXR_TOVR IN intrStatus THEN
+				INCL(uart.errors,OverrunError);
+				Trace.String("---tx overrun---: intrStatus="); Trace.Set(intrStatus); Trace.Ln;
+				RETURN;
+			END;
+		END;
+
+		bufRdPos := uart.txBufRdPos;
+
+		WHILE (bufRdPos # uart.txBufWrPos) & ~(UartMin.XUARTPS_SR_TXFULL IN uart.regs.sr) DO
+			uart.regs.fifo := ORD(uart.txBuf[bufRdPos]);
+			INC(bufRdPos);
+			IF bufRdPos = LEN(uart.txBuf) THEN
+				bufRdPos := 0;
+			END;
+		END;
+
+		(* disable TX data interrupts if the buffer is empty *)
+		IF bufRdPos = uart.txBufWrPos THEN
+			uart.regs.idr := uart.regs.idr + TxDataInterrupts;
+		END;
+
+		uart.txBufRdPos := bufRdPos;
+	END IntrHandlerTx;
+
+	(*
+		Returns TRUE if a cyclic buffer is full
+	*)
+	PROCEDURE BufIsFull(bufWrPos, bufRdPos, bufSize: LONGINT): BOOLEAN;
+	BEGIN
+		IF bufWrPos # (bufSize-1) THEN
+			RETURN bufRdPos = (bufWrPos+1);
+		ELSE
+			RETURN bufRdPos = 0;
+		END;
+	END BufIsFull;
+
+	(**
+		Install a UART controller present in the system
+
+		uart: ID (0-based index) of the UART controller to install
+		base: controller base address
+		inputClock: controller input clock in Hz
+		res: returned error code, 0 in case of success
+	*)
+	PROCEDURE Install* (uart: LONGINT; base: ADDRESS; inputClock: LONGINT; VAR res: LONGINT);
+	VAR ctl: UartController;
+	BEGIN
+		UartMin.Install(uart, base, inputClock, res);
+		IF res # 0 THEN RETURN; END;
+
+		NEW(ctl);
+		uarts[uart] := ctl;
+
+		ctl.id := uart;
+		ctl.regs := UartMin.GetUart(uart);
+		ctl.inputClock := inputClock;
+		ctl.open := FALSE;
+		ctl.bps := UartMin.DefaultBPS;
+		ctl.data := UartMin.DefaultDataBits;
+		ctl.parity := UartMin.DefaultParity;
+		ctl.stop := UartMin.DefaultStop;
+
+		NEW(ctl.rxBuf,DefaultRxBufSize);
+		NEW(ctl.txBuf,DefaultTxBufSize);
+
+		ASSERT(PsUartInterrupts.InstallInterruptHandler(uart,IntrHandler,ctl));
+	END Install;
+
+	(**
+		Get UART controller with a given ID
+
+		uart: UART controller ID
+
+		Returns NIL in case if no controller with given ID has been installed
+	*)
+	PROCEDURE GetUart*(uart: LONGINT): UartController;
+	BEGIN
+		IF (uart >= 0) & (uart < LEN(uarts)) THEN
+			RETURN uarts[uart];
+		ELSE RETURN NIL;
+		END;
+	END GetUart;
+
+	(**
+		Open a UART controller
+
+		uart: UART controller
+		bps: baudrate
+		data: number of data bits
+		parity: parity control
+		stop: number of stop bits
+		res: returned error code, 0 in case of success
+	*)
+	PROCEDURE Open*(uart: UartController; bps, data, parity, stop: LONGINT; VAR res: LONGINT);
+	VAR n: LONGINT;
+	BEGIN
+		IF uart.open THEN res := UartMin.PortInUse; RETURN; END;
+
+		UartMin.Reset(uart.regs);
+
+		IF ~UartMin.SetBps(uart.regs, bps, res) OR
+			~UartMin.SetDataBits(uart.regs, data, res) OR
+			~UartMin.SetParity(uart.regs, parity, res) OR
+			~UartMin.SetStopBits(uart.regs, stop, res) THEN  RETURN;
+		END;
+
+		uart.bps := bps;
+		uart.data := data;
+		uart.parity := parity;
+		uart.stop := stop;
+
+		uart.rxBufWrPos := 0;
+		uart.rxBufRdPos := 0;
+
+		uart.txBufWrPos := 0;
+		uart.txBufRdPos := 0;
+
+		(* configure receive timeout to be as close as possible to ReceiveTimeoutInUs *)
+		n := ENTIER((ReceiveTimeoutInUs*REAL(bps)+1000000) / 4000000 + 0.5);
+		n := MAX(1,MIN(255,n-1));
+		TRACE(n);
+		uart.regs.rxtout := n;
+
+		uart.regs.cr := uart.regs.cr + {UartMin.XUARTPS_CR_TORST}; (* restart receive timeout counter *)
+
+		uart.regs.rxwm := 32; (* RX FIFO triggering threshold *)
+
+		uart.regs.txwm := 32; (* TX FIFO triggering threshold *)
+
+		uart.regs.ier := (RxDataInterrupts+RxErrorInterrupts+TxErrorInterrupts);
+
+		UartMin.Enable(uart.regs,TRUE);
+		
+		res := 0;
+		uart.open := TRUE;
+	END Open;
+
+	(**
+		Close a UART controller
+
+		uart: UART controller
+	*)
+	PROCEDURE Close*(uart: UartController);
+	BEGIN
+		uart.open := FALSE;
+		DisableInterrupts(uart.regs);
+		UartMin.Enable(uart.regs,FALSE);
+	END Close;
+
+	PROCEDURE OccupiedBufSpace(bufWrPos, bufRdPos, bufSize: LONGINT): LONGINT;
+	VAR n: LONGINT;
+	BEGIN
+		n := bufWrPos - bufRdPos;
+		IF n >= 0 THEN
+			RETURN n;
+		ELSE
+			RETURN n+bufSize;
+		END;
+	END OccupiedBufSpace;
+
+	(* Returns the amount of available free space in a cyclic buffer *)
+	PROCEDURE AvailableBufSpace(bufWrPos, bufRdPos, bufSize: LONGINT): LONGINT;
+	VAR n: LONGINT;
+	BEGIN
+		n := bufWrPos - bufRdPos;
+		IF n >= 0 THEN
+			RETURN bufSize-1-n;
+		ELSE
+			RETURN -n-1;
+		END;
+	END AvailableBufSpace;
+
+	(**
+		Returns number of bytes available in the receive buffer of a UART controller
+
+		uart: UART controller
+		res: error code, 0 in case of success
+	*)
+	PROCEDURE Available*(uart: UartController): LONGINT;
+	BEGIN
+		RETURN OccupiedBufSpace(uart.rxBufWrPos,uart.rxBufRdPos,LEN(uart.rxBuf));
+	END Available;
+
+	(**
+		Send a single character to the UART
+
+		ch: character to send
+		propagate: TRUE for flushing the TX FIFO buffer
+		res: error code, 0 in case of success
+	*)
+	PROCEDURE SendChar*(uart: UartController; ch: CHAR; propagate: BOOLEAN; onBusy: UartMin.BusyLoopCallback; VAR res: LONGINT);
+	BEGIN
+		(*! for the moment just write directly to the FIFO *)
+		res := 0;
+		WHILE uart.open DO
+			IF ~(UartMin.XUARTPS_SR_TNFUL IN uart.regs.sr) THEN
+				uart.regs.fifo := ORD(ch); RETURN;
+			END;
+		END;
+		
+		res := UartMin.Closed;
+	(*TYPE ArrayOfChar1 = ARRAY 1 OF CHAR;
+	BEGIN
+		(*!TODO: do not use interrupts here to avoid problems when SendChar is used for trace output *)
+		Send(uart, SYSTEM.VAL(ArrayOfChar1,ch), 0, 1, propagate, onBusy, res);*)
+	END SendChar;
+
+	(**
+		Send data to the UART
+	*)
+	PROCEDURE Send*(uart: UartController; CONST buf: ARRAY OF CHAR; offs, len: LONGINT; propagate: BOOLEAN; onBusy: UartMin.BusyLoopCallback; VAR res: LONGINT);
+	VAR
+		bufWrPos, n: LONGINT;
+	BEGIN
+
+		IF ~uart.open THEN res := UartMin.Closed; RETURN; END;
+
+		WHILE uart.open & (len > 0) DO
+
+			bufWrPos := uart.txBufWrPos;
+			n := AvailableBufSpace(bufWrPos,uart.txBufWrPos,LEN(uart.txBuf));
+
+			IF n # 0 THEN
+
+				n := MIN(n,len);
+				DEC(len,n);
+
+				WHILE n > 0 DO
+					uart.txBuf[bufWrPos] := buf[offs];
+					INC(bufWrPos);
+					IF bufWrPos = LEN(uart.txBuf) THEN
+						bufWrPos := 0;
+					END;
+					INC(offs); DEC(n);
+				END;
+
+				uart.txBufWrPos := bufWrPos;
+
+				(* enable TX interrupts *)
+				uart.regs.ier := uart.regs.ier + TxDataInterrupts;
+			ELSE
+				(* enable TX interrupts *)
+				uart.regs.ier := uart.regs.ier + TxDataInterrupts;
+				IF (onBusy # NIL) & ~onBusy(res) THEN RETURN; END;
+			END;
+		END;
+
+		IF propagate THEN
+			(* flush the buffer *)
+			WHILE uart.open & (uart.txBufRdPos # uart.txBufWrPos) DO
+				IF (onBusy # NIL) & ~onBusy(res) THEN RETURN; END;
+			END;
+
+			(* flush the FIFO *)
+			WHILE uart.open & ~(UartMin.XUARTPS_SR_TXEMPTY IN uart.regs.sr) DO
+				IF (onBusy # NIL) & ~onBusy(res) THEN RETURN; END;
+			END;
+		END;
+
+		IF uart.open THEN
+			res := 0;
+		ELSE
+			IF OverrunError IN uart.errors THEN res := OverrunError;
+			ELSE res := UartMin.Closed;
+			END;
+		END;
+(*	BEGIN
+
+		WHILE uart.open & (len > 0) DO
+			IF ~(UartMin.XUARTPS_SR_TNFUL IN uart.regs.sr) THEN
+				uart.regs.fifo := ORD(buf[offs]);
+				INC(offs); DEC(len);
+			ELSIF (onBusy # NIL) & ~onBusy(res) THEN
+				RETURN;
+			END;
+		END;
+
+		IF propagate THEN (* flush the FIFO *)
+			WHILE uart.open & ~(UartMin.XUARTPS_SR_TXEMPTY IN uart.regs.sr) DO
+				IF (onBusy # NIL) & ~onBusy(res) THEN RETURN; END;
+			END;
+		END;
+
+		IF uart.open THEN
+			res := UartMin.Ok;
+		ELSE
+			res := UartMin.Closed;
+		END;*)
+	END Send;
+
+	(**
+		Receive a single character from UART
+
+		res: error code, 0 in case of success
+
+		Remarks: blocks until a character is available
+	*)
+	PROCEDURE ReceiveChar*(uart: UartController; onBusy: UartMin.BusyLoopCallback; VAR res: LONGINT): CHAR;
+	VAR
+		buf: ARRAY 1 OF CHAR;
+		len: LONGINT;
+	BEGIN
+		Receive(uart,buf,0,1,1,len,onBusy,res);
+		RETURN buf[0];
+	END ReceiveChar;
+
+	(**
+		Receive data from the UART
+	*)
+	PROCEDURE Receive*(uart: UartController; VAR buf: ARRAY OF CHAR; offs, size, min: LONGINT; VAR len: LONGINT; onBusy: UartMin.BusyLoopCallback; VAR res: LONGINT);
+	VAR
+		bufRdPos, n: LONGINT;
+	BEGIN
+		IF ~uart.open THEN res := UartMin.Closed; RETURN; END;
+
+		res := 0;
+		len := 0;
+
+		IF size = 0 THEN RETURN; END;
+
+		min := MIN(size,min);
+
+		WHILE uart.open & (size > 0) DO
+
+			bufRdPos := uart.rxBufRdPos;
+
+			n := OccupiedBufSpace(uart.rxBufWrPos,bufRdPos,LEN(uart.rxBuf));
+
+			IF n # 0 THEN
+
+				n := MIN(n,size);
+				DEC(size,n); INC(len,n);
+				IF min > 0 THEN DEC(min,n); END;
+
+				WHILE n > 0 DO
+					buf[offs] := uart.rxBuf[bufRdPos];
+					INC(bufRdPos);
+					IF bufRdPos = LEN(uart.rxBuf) THEN
+						bufRdPos := 0;
+					END;
+					INC(offs); DEC(n);
+				END;
+
+				uart.rxBufRdPos := bufRdPos;
+
+			ELSIF min > 0 THEN
+				IF (onBusy # NIL) & ~onBusy(res) THEN RETURN; END;
+			ELSE
+				RETURN;
+			END;
+		END;
+
+	(*BEGIN
+		res := Ok;
+		len := 0;
+
+		IF size = 0 THEN RETURN; END;
+
+		min := MIN(size,min);
+
+		WHILE uart.open & (~(UartMin.XUARTPS_SR_RXEMPTY IN uart.regs.sr) OR (min > 0)) DO
+			IF ~(UartMin.XUARTPS_SR_RXEMPTY IN uart.regs.sr) THEN
+				WHILE (size > 0) & ~(UartMin.XUARTPS_SR_RXEMPTY IN uart.regs.sr) DO
+					buf[offs] := uart.regs.fifo;
+					DEC(min); DEC(size); INC(offs); INC(len);
+				END;
+			ELSIF (onBusy # NIL) & ~onBusy(res) THEN
+				RETURN;
+			END;
+		END;
+
+		IF ~uart.open THEN
+			res := Closed;
+		END;*)
+	END Receive;
+
+	PROCEDURE PrintRegisters(regs: UartMin.UartRegisters);
+	BEGIN
+		Trace.String("cr("); Trace.Hex(ADDRESSOF(regs.cr),-8); Trace.String("): "); Trace.Set(regs.cr); Trace.Ln;
+		Trace.String("mr("); Trace.Hex(ADDRESSOF(regs.mr),-8); Trace.String("): "); Trace.Set(regs.mr); Trace.Ln;
+		Trace.String("ier(");  Trace.Hex(ADDRESSOF(regs.ier),-8); Trace.String("): write only"); Trace.Ln;
+		Trace.String("idr("); Trace.Hex(ADDRESSOF(regs.idr),-8); Trace.String("): write only"); Trace.Ln;
+		Trace.String("imr("); Trace.Hex(ADDRESSOF(regs.imr),-8); Trace.String("): "); Trace.Set(regs.imr); Trace.Ln;
+		Trace.String("isr("); Trace.Hex(ADDRESSOF(regs.isr),-8); Trace.String("): "); Trace.Set(regs.isr); Trace.Ln;
+		Trace.String("baudgen("); Trace.Hex(ADDRESSOF(regs.baudgen),-8); Trace.String("): "); Trace.Int(regs.baudgen,0); Trace.Ln;
+		Trace.String("rxtout("); Trace.Hex(ADDRESSOF(regs.rxtout),-8); Trace.String("): "); Trace.Int(regs.rxtout,0); Trace.Ln;
+		Trace.String("rxwm("); Trace.Hex(ADDRESSOF(regs.rxwm),-8); Trace.String("): "); Trace.Int(regs.rxwm,0); Trace.Ln;
+		Trace.String("modemcr("); Trace.Hex(ADDRESSOF(regs.modemcr),-8); Trace.String("): "); Trace.Set(regs.modemcr); Trace.Ln;
+		Trace.String("modemsr("); Trace.Hex(ADDRESSOF(regs.modemsr),-8); Trace.String("): "); Trace.Set(regs.modemsr); Trace.Ln;
+		Trace.String("sr("); Trace.Hex(ADDRESSOF(regs.sr),-8); Trace.String("): "); Trace.Set(regs.sr); Trace.Ln;
+		Trace.String("fifo(");  Trace.Hex(ADDRESSOF(regs.fifo),-8); Trace.String("): --- "); Trace.Ln;
+		Trace.String("bauddiv("); Trace.Hex(ADDRESSOF(regs.bauddiv),-8); Trace.String("): "); Trace.Int(regs.bauddiv,0); Trace.Ln;
+		Trace.String("flowdel("); Trace.Hex(ADDRESSOF(regs.flowdel),-8); Trace.String("): "); Trace.Int(regs.flowdel,0); Trace.Ln;
+		Trace.String("txwm("); Trace.Hex(ADDRESSOF(regs.txwm),-8); Trace.String("): "); Trace.Int(regs.txwm,0); Trace.Ln;
+	END PrintRegisters;
+
+	PROCEDURE Show*;
+	BEGIN
+		IF uarts[0] # NIL THEN
+			Trace.StringLn("PS UART0:");
+			PrintRegisters(uarts[0].regs);
+			Trace.String("rxBufRdPos="); Trace.Int(uarts[0].rxBufRdPos,0); Trace.Ln;
+			Trace.String("rxBufWrPos="); Trace.Int(uarts[0].rxBufWrPos,0); Trace.Ln;
+			Trace.String("txBufRdPos="); Trace.Int(uarts[0].txBufRdPos,0); Trace.Ln;
+			Trace.String("txBufWrPos="); Trace.Int(uarts[0].txBufWrPos,0); Trace.Ln;
+			Trace.Ln;
+		END;
+		IF uarts[1] # NIL THEN
+			Trace.StringLn("PS UART1:");
+			PrintRegisters(uarts[1].regs);
+			Trace.String("rxBufRdPos="); Trace.Int(uarts[1].rxBufRdPos,0); Trace.Ln;
+			Trace.String("rxBufWrPos="); Trace.Int(uarts[1].rxBufWrPos,0); Trace.Ln;
+			Trace.String("txBufRdPos="); Trace.Int(uarts[1].txBufRdPos,0); Trace.Ln;
+			Trace.String("txBufWrPos="); Trace.Int(uarts[1].txBufWrPos,0); Trace.Ln;
+			Trace.Ln;
+		END;
+
+	END Show;
+
+END PsUart.

source/Zynq.PsUartInterrupts.Mod

@@ -0,0 +1,59 @@
+(**
+	AUTHOR: Alexey Morozov, Timothee Martiel, HighDim GmbH, 2013-2018
+	PURPOSE: interrupt handling infrastracture for Zynq PS UART driver used in A2
+*)
+MODULE PsUartInterrupts;
+
+IMPORT
+	Platform, Machine;
+
+TYPE
+	(** UART interrupt handler *)
+	UartInterruptHandler* = PROCEDURE(param: ANY);
+
+VAR
+	intrHandler0, intrHandler1: UartInterruptHandler;
+	intrHandlerParam0, intrHandlerParam1: ANY;
+
+	PROCEDURE IntrHandlerUart0(VAR state: Machine.State);
+	BEGIN
+		intrHandler0(intrHandlerParam0);
+	END IntrHandlerUart0;
+
+	PROCEDURE IntrHandlerUart1(VAR state: Machine.State);
+	BEGIN
+		intrHandler1(intrHandlerParam1);
+	END IntrHandlerUart1;
+
+	(**
+		Install a UART interrupt handler
+
+		uart: PS UART controller ID
+		interruptHandler: interrupt handler
+		interruptHandlerParam: parameter to be passed to interrupt handler
+
+		Return: FALSE in case of a wrong value of UART ID or if interruptHandler is NIL
+	*)
+	PROCEDURE InstallInterruptHandler*(uart: LONGINT; interruptHandler: UartInterruptHandler; interruptHandlerParam: ANY): BOOLEAN;
+	BEGIN
+		IF interruptHandler = NIL THEN
+			RETURN FALSE;
+		END;
+
+		CASE uart OF
+			0:
+				intrHandler0 := interruptHandler;
+				intrHandlerParam0 := interruptHandlerParam;
+				Machine.InstallHandler(IntrHandlerUart0,Platform.UartIrq[0]);
+			|1:
+				intrHandler1 := interruptHandler;
+				intrHandlerParam1 := interruptHandlerParam;
+				Machine.InstallHandler(IntrHandlerUart1,Platform.UartIrq[1]);
+		ELSE
+			RETURN FALSE;
+		END;
+
+		RETURN TRUE;
+	END InstallInterruptHandler;
+
+END PsUartInterrupts.

source/Zynq.PsUartMin.Mod

@@ -0,0 +1,455 @@
+MODULE PsUartMin;
+(**
+	AUTHOR: Alexey Morozov, Timothee Martiel, HighDim GmbH, 2013-2017
+	PURPOSE: minimal driver implementation for Xilinx Zynq UART PS controller
+*)
+
+IMPORT SYSTEM;
+
+CONST
+	(** Parity types - compatible with A2 Serials *)
+	ParNo* = 0;  ParOdd* = 1;  ParEven* = 2;  ParMark* = 3;  ParSpace* = 4;
+
+	(** Stop bits - compatible with A2 Serials *)
+	Stop1* = 1;  Stop2* = 2;  Stop1dot5* = 3;
+
+	(** Error codes - compatible with A2 Serials *)
+	Ok* =  0;
+	Closed* = -1;
+	PortInUse* =  1; NoSuchPort* =  2; WrongBPS* =  3; WrongData* =  4; WrongParity* =  5; WrongStop* =  6;
+
+	(* Default settings *)
+	DefaultBPS* = 115200; DefaultDataBits* = 8; DefaultParity* = ParNo; DefaultStop* = Stop1;
+
+	NumUarts* = 2; (** number of supported UART controllers *)
+
+	 (* Control Register Bit Definition
+	 	The Control register (CR) controls the major functions of the device.
+	 *)
+	XUARTPS_CR_STOPBRK* = 8; (* Stop transmission of break *)
+	XUARTPS_CR_STARTBRK* = 7; (* Set break *)
+	XUARTPS_CR_TORST* = 6; (* RX timeout counter restart *)
+	XUARTPS_CR_TX_DIS* = 5; (* TX disabled. *)
+	XUARTPS_CR_TX_EN* = 4; (* TX enabled *)
+	XUARTPS_CR_RX_DIS* = 3; (* RX disabled. *)
+	XUARTPS_CR_RX_EN* = 2; (* RX enabled *)
+	XUARTPS_CR_EN_DIS_MASK* = {2..5}; (* Enable/disable Mask *)
+	XUARTPS_CR_TXRST* = 1; (* TX logic reset *)
+	XUARTPS_CR_RXRST* = 0; (* RX logic reset *)
+
+	(* Mode Register Bit Definition
+
+		The mode register (MR) defines the mode of transfer as well as the data
+		format. If this register is modified during transmission or reception,
+		data validity cannot be guaranteed.
+	*)
+	XUARTPS_MR_CHMODE_R_LOOP = {8,9}; (* Remote loopback mode *)
+	XUARTPS_MR_CHMODE_L_LOOP = {9}; (* Local loopback mode *)
+	XUARTPS_MR_CHMODE_ECHO = {8}; (* Auto echo mode *)
+	XUARTPS_MR_CHMODE_NORM = {}; (* Normal mode *)
+	XUARTPS_MR_CHMODE_SHIFT = 8; (* Mode shift *)
+	XUARTPS_MR_CHMODE_MASK = {8..9}; (* Mode mask *)
+	XUARTPS_MR_STOPMODE_2 = {7}; (* 2 stop bits *)
+	XUARTPS_MR_STOPMODE_1_5 = {6}; (* 1.5 stop bits *)
+	XUARTPS_MR_STOPMODE_1 = {}; (* 1 stop bit *)
+	XUARTPS_MR_STOPMODE_SHIFT = 6; (* Stop bits shift *)
+	XUARTPS_MR_STOPMODE_MASK = {6..7}; (* Stop bits mask *)
+	XUARTPS_MR_PARITY_NONE = {5}; (* No parity mode *)
+	XUARTPS_MR_PARITY_MARK = {3,4}; (* Mark parity mode *)
+	XUARTPS_MR_PARITY_SPACE = {4}; (* Space parity mode *)
+	XUARTPS_MR_PARITY_ODD = {3}; (* Odd parity mode *)
+	XUARTPS_MR_PARITY_EVEN = {}; (* Even parity mode *)
+	XUARTPS_MR_PARITY_SHIFT = 3; (* Parity setting shift *)
+	XUARTPS_MR_PARITY_MASK = {3..5}; (* Parity mask *)
+	XUARTPS_MR_CHARLEN_6 = {1,2}; (* 6 bits data *)
+	XUARTPS_MR_CHARLEN_7 = {2}; (* 7 bits data *)
+	XUARTPS_MR_CHARLEN_8 = {}; (* 8 bits data *)
+	XUARTPS_MR_CHARLEN_SHIFT = 1; (* Data Length shift *)
+	XUARTPS_MR_CHARLEN_MASK = {1..2}; (* Data length mask *)
+	XUARTPS_MR_CLKSEL_BIT = 0; (* Input clock selection *)
+
+	(** Interrupt Registers
+
+		Interrupt control logic uses the interrupt enable register (IER) and the
+		interrupt disable register (IDR) to set the value of the bits in the
+		interrupt mask register (IMR). The IMR determines whether to pass an
+		interrupt to the interrupt status register (ISR).
+		Writing a 1 to IER Enbables an interrupt, writing a 1 to IDR disables an
+		interrupt. IMR and ISR are read only, and IER and IDR are write only.
+		Reading either IER or IDR returns 0x00.
+
+		All four registers have the same bit definitions.
+	*)
+	XUARTPS_IXR_RBRK	= 13; (** Rx FIFO break detect interrupt *)
+	XUARTPS_IXR_TOVR*	= 12; (** Tx FIFO Overflow interrupt *)
+	XUARTPS_IXR_TNFUL*	= 11; (** Tx FIFO Nearly Full interrupt *)
+	XUARTPS_IXR_TTRIG*	= 10; (** Tx Trig interrupt *)
+	XUARTPS_IXR_DMS*		= 9; (** Modem status change interrupt *)
+	XUARTPS_IXR_TOUT*	= 8; (** Timeout error interrupt *)
+	XUARTPS_IXR_PARITY* 	= 7; (** Parity error interrupt *)
+	XUARTPS_IXR_FRAMING*	= 6; (** Framing error interrupt *)
+	XUARTPS_IXR_OVER*	= 5; (** Overrun error interrupt *)
+	XUARTPS_IXR_TXFULL* 	= 4; (** TX FIFO full interrupt. *)
+	XUARTPS_IXR_TXEMPTY*	= 3; (** TX FIFO empty interrupt. *)
+	XUARTPS_IXR_RXFULL* 	= 2; (** RX FIFO full interrupt. *)
+	XUARTPS_IXR_RXEMPTY*	= 1; (** RX FIFO empty interrupt. *)
+	XUARTPS_IXR_RXOVR*  	= 0; (** RX FIFO trigger interrupt. *)
+	XUARTPS_IXR_MASK*	= {0..13}; (** Valid bit mask *)
+
+	(** Channel Status Register
+
+		The channel status register (CSR) is provided to enable the control logic
+		to monitor the status of bits in the channel interrupt status register,
+		even if these are masked out by the interrupt mask register.
+	*)
+	XUARTPS_SR_TNFUL* =	14; (** TX FIFO Nearly Full Status *)
+	XUARTPS_SR_TTRIG*	 =	13; (** TX FIFO Trigger Status *)
+	XUARTPS_SR_FLOWDEL* =	12; (** RX FIFO fill over flow delay *)
+	XUARTPS_SR_TACTIVE* =	11; (** TX active *)
+	XUARTPS_SR_RACTIVE* =	10; (** RX active *)
+	XUARTPS_SR_DMS*	 =	9; (** Delta modem status change *)
+	XUARTPS_SR_TOUT*	 =	8; (** RX timeout *)
+	XUARTPS_SR_PARITY* =	7; (** RX parity error *)
+	XUARTPS_SR_FRAME* =	6; (** RX frame error *)
+	XUARTPS_SR_OVER*	 =	5; (** RX overflow error *)
+	XUARTPS_SR_TXFULL* =	4; (** TX FIFO full *)
+	XUARTPS_SR_TXEMPTY* =	3; (** TX FIFO empty *)
+	XUARTPS_SR_RXFULL* =	2; (** RX FIFO full *)
+	XUARTPS_SR_RXEMPTY* =	1; (** RX FIFO empty *)
+	XUARTPS_SR_RXOVR* =	0; (** RX FIFO fill over trigger *)
+
+	(*
+		Modem Control register
+	*)
+	XUARTPS_MODEMCR_FCM = 5;
+	XUARTPS_MODEMCR_RTS = 1;
+	XUARTPS_MODEMCR_DTR = 0;
+
+	(*
+		Modem Status register
+	*)
+	XUARTPS_MODEMSR_FCMS = 8;
+	XUARTPS_MODEMSR_DCD = 7;
+	XUARTPS_MODEMSR_RI = 6;
+	XUARTPS_MODEMSR_DSR = 5;
+	XUARTPS_MODEMSR_CTS = 4;
+	XUARTPS_MODEMSR_DCDX = 3;
+	XUARTPS_MODEMSR_RIX = 2;
+	XUARTPS_MODEMSR_DSRX = 1;
+	XUARTPS_MODEMSR_CTSX = 0;
+
+	(* The following constant defines the amount of error that is allowed for
+		a specified baud rate. This error is the difference between the actual
+		baud rate that will be generated using the specified clock and the
+		desired baud rate.
+	*)
+	XUARTPS_MAX_BAUD_ERROR_RATE = 3;	(* max % error allowed *)
+
+TYPE
+ 	(*
+ 		(* Register offsets for the UART controller *)
+		XUARTPS_CR_OFFSET = 0x0000;  (* Control Register [8:0] *)
+		XUARTPS_MR_OFFSET = 0x0004;  (* Mode Register [9:0] *)
+		XUARTPS_IER_OFFSET = 0x0008;  (* Interrupt Enable [12:0] *)
+		XUARTPS_IDR_OFFSET = 0x000C;  (* Interrupt Disable [12:0] *)
+		XUARTPS_IMR_OFFSET = 0x0010;  (* Interrupt Mask [12:0] *)
+		XUARTPS_ISR_OFFSET = 0x0014;  (* Interrupt Status [12:0]*)
+		XUARTPS_BAUDGEN_OFFSET = 0x0018;  (* Baud Rate Generator [15:0] *)
+		XUARTPS_RXTOUT_OFFSET = 0x001C;  (* RX Timeout [7:0] *)
+		XUARTPS_RXWM_OFFSET = 0x0020;  (* RX FIFO Trigger Level [5:0] *)
+		XUARTPS_MODEMCR_OFFSET = 0x0024;  (* Modem Control [5:0] *)
+		XUARTPS_MODEMSR_OFFSET = 0x0028;  (* Modem Status [8:0] *)
+		XUARTPS_SR_OFFSET = 0x002C;  (* Channel Status [14:0] *)
+		XUARTPS_FIFO_OFFSET = 0x0030;  (* FIFO [7:0] *)
+		XUARTPS_BAUDDIV_OFFSET = 0x0034;  (* Baud Rate Divider [7:0] *)
+		XUARTPS_FLOWDEL_OFFSET = 0x0038;  (* Flow Delay [5:0] *)
+		XUARTPS_TXWM_OFFSET = 0x0044;  (* TX FIFO Trigger Level [5:0] *)
+		XUARTPS_RXBS_OFFSET = 0x0048;  (* RX FIFO Byte Status [11:0] *)
+	*)
+	UartRegisters* = POINTER {UNSAFE, UNTRACED}  TO RECORD
+		cr*, mr*, ier*, idr*, imr*, isr*: SET;
+		baudgen*, rxtout*, rxwm*: LONGINT;
+		modemcr*, modemsr*: SET;
+		sr*: SET;
+		fifo*: LONGINT;
+		bauddiv*, flowdel*: LONGINT;
+		padding2: ARRAY 2 OF LONGINT;
+		txwm*: LONGINT;
+	END;
+
+	BusyLoopCallback* = PROCEDURE{DELEGATE}(VAR res: LONGINT): BOOLEAN;
+
+VAR
+	uarts: ARRAY NumUarts OF UartRegisters;
+	inputClocks: ARRAY NumUarts OF LONGINT;
+
+	(**
+		Install a UART controller
+
+		uart: ID (0-based index) of the UART controller to install
+		base: controller base address
+		inputClock: controller input clock in Hz
+		res: returned error code, 0 in case of success
+	*)
+	PROCEDURE Install* (uart: LONGINT; base: ADDRESS; inputClock: LONGINT; VAR res: LONGINT);
+	BEGIN
+		IF (uart < 0) OR (uart >= LEN(uarts)) THEN
+			res := NoSuchPort; RETURN;
+		END;
+
+		res := Ok;
+
+		uarts[uart] := base;
+		inputClocks[uart] := inputClock;
+
+		Reset(uarts[uart]);
+		ASSERT(SetBps(uarts[uart],DefaultBPS,res));
+		ASSERT(SetDataBits(uarts[uart],DefaultDataBits,res));
+		ASSERT(SetParity(uarts[uart],DefaultParity,res));
+		ASSERT(SetStopBits(uarts[uart],DefaultStop,res));
+	END Install;
+
+	(**
+		Get UART controller with a given ID
+	*)
+	PROCEDURE GetUart*(uart: LONGINT): UartRegisters;
+	BEGIN
+		IF (uart >= 0) & (uart < LEN(uarts)) THEN
+			RETURN uarts[uart];
+		ELSE RETURN NIL;
+		END;
+	END GetUart;
+
+	(**
+		Enable/Disable the UART controller
+
+		enable: TRUE for enabling
+	*)
+	PROCEDURE Enable*(uart: UartRegisters; enable: BOOLEAN);
+	BEGIN
+		IF enable THEN
+			uart.cr := uart.cr * SET(-XUARTPS_CR_EN_DIS_MASK) + {XUARTPS_CR_RX_EN, XUARTPS_CR_TX_EN};
+		ELSE
+			uart.cr := uart.cr * SET(-XUARTPS_CR_EN_DIS_MASK) + {XUARTPS_CR_RX_DIS, XUARTPS_CR_TX_DIS};
+		END;
+	END Enable;
+
+	(**
+		Reset the controller
+	*)
+	PROCEDURE Reset*(uart: UartRegisters);
+	BEGIN
+		(* disable all UART interrupts *)
+		uart.idr := XUARTPS_IXR_MASK;
+
+		(* disable RX/TX *)
+		uart.cr := {XUARTPS_CR_RX_DIS, XUARTPS_CR_TX_DIS};
+
+		(* software reset of RX and TX - this clears the FIFO. *)
+		uart.cr := {XUARTPS_CR_RXRST, XUARTPS_CR_TXRST};
+
+(*! this makes the controller crazy when executed within PsUart.Open
+		(* clear status flags - SW reset will not clear sticky flags *)
+		uart.isr := XUARTPS_IXR_MASK;*)
+
+		(* set CR to its default value *)
+		uart.cr := {XUARTPS_CR_RX_DIS, XUARTPS_CR_TX_DIS, XUARTPS_CR_STOPBRK};
+	END Reset;
+
+	(*
+		Setup baudrate in bps
+	*)
+	PROCEDURE SetBps*(uart: UartRegisters; baudrate: LONGINT; VAR res: LONGINT): BOOLEAN;
+	VAR
+		i, clock: LONGINT;
+		valBAUDDIV, valBRGR, calcBaudrate, baudError: LONGINT;
+		bestError, bestBRGR, bestBAUDDIV: LONGINT;
+	BEGIN
+		i := 0;
+		WHILE (i < LEN(uarts)) & (uarts[i] # uart) DO INC(i); END;
+		ASSERT(i < LEN(uarts));
+		clock := inputClocks[i];
+
+		(*
+			Make sure the baud rate is not impossilby large.
+	 		Fastest possible baud rate is Input Clock / 2
+		*)
+		IF (baudrate <= 0) OR (baudrate*2 > clock) THEN
+			res := NoSuchPort; RETURN FALSE;
+		END;
+
+		(* Check whether the input clock is divided by 8 *)
+		IF XUARTPS_MR_CLKSEL_BIT IN uart.mr THEN
+			clock := clock DIV 8;
+		END;
+
+		(* Determine the Baud divider. It can be 4 to 254.
+			Loop through all possible combinations *)
+		bestError := MAX(LONGINT);
+		FOR valBAUDDIV := 4 TO 255 DO
+
+			(* Calculate the value for BRGR register *)
+			valBRGR := clock DIV (baudrate * (valBAUDDIV + 1));
+
+			IF valBRGR > 0 THEN
+				(* Calculate the baud rate from the BRGR value *)
+				calcBaudrate := clock DIV (valBRGR * (valBAUDDIV + 1));
+
+				(* Avoid unsigned integer underflow *)
+				IF baudrate > calcBaudrate THEN
+					baudError := baudrate - calcBaudrate;
+				ELSE
+					baudError := calcBaudrate - baudrate;
+				END;
+
+				(*
+					Find the calculated baud rate closest to requested baud rate.
+				*)
+				IF baudError < bestError THEN
+					bestBRGR := valBRGR;
+					bestBAUDDIV := valBAUDDIV;
+					bestError := baudError;
+				END;
+			END;
+
+			INC(valBAUDDIV);
+		END;
+
+		(*
+			Make sure the best error is not too large.
+		*)
+		IF (bestError * 100) DIV baudrate > XUARTPS_MAX_BAUD_ERROR_RATE THEN (* baudrate error *)
+			res := WrongBPS; RETURN FALSE;
+		END;
+
+		(* write baudrate settings *)
+		uart.baudgen := bestBRGR;
+		uart.bauddiv := bestBAUDDIV;
+
+		res := Ok;
+		RETURN TRUE
+	END SetBps;
+
+	(**
+		Set number of data bits
+	*)
+	PROCEDURE SetDataBits*(uart: UartRegisters; dataBits: LONGINT; VAR res: LONGINT): BOOLEAN;
+	VAR reg: SET;
+	BEGIN
+		CASE dataBits OF
+			 6: reg := XUARTPS_MR_CHARLEN_6;
+			|7: reg := XUARTPS_MR_CHARLEN_7;
+			|8: reg := XUARTPS_MR_CHARLEN_8;
+		ELSE
+			res := WrongData; RETURN FALSE;
+		END;
+
+		uart.mr := uart.mr * SET(-XUARTPS_MR_CHARLEN_MASK) + reg;
+
+		res := Ok;
+		RETURN TRUE
+	END SetDataBits;
+
+	(**
+		Setup parity check type
+	*)
+	PROCEDURE SetParity*(uart: UartRegisters; parityType: LONGINT; VAR res: LONGINT): BOOLEAN;
+	VAR reg: SET;
+	BEGIN
+		CASE parityType OF
+			 ParNo: reg := XUARTPS_MR_PARITY_NONE;
+			|ParEven: reg := XUARTPS_MR_PARITY_EVEN;
+			|ParOdd: reg := XUARTPS_MR_PARITY_ODD;
+			|ParMark: reg := XUARTPS_MR_PARITY_MARK;
+			|ParSpace: reg := XUARTPS_MR_PARITY_SPACE;
+		ELSE
+			res := WrongParity; RETURN FALSE;
+		END;
+
+		uart.mr := uart.mr * SET(-XUARTPS_MR_PARITY_MASK) + reg;
+
+		res := Ok;
+		RETURN TRUE
+	END SetParity;
+
+	(**
+		Setup number of stop bits
+	*)
+	PROCEDURE SetStopBits*(uart: UartRegisters; stopBits: LONGINT; VAR res: LONGINT): BOOLEAN;
+	VAR reg: SET;
+	BEGIN
+		CASE stopBits OF
+			 Stop1: reg := XUARTPS_MR_STOPMODE_1;
+			|Stop2: reg := XUARTPS_MR_STOPMODE_2;
+			|Stop1dot5: reg := XUARTPS_MR_STOPMODE_1_5;
+		ELSE
+			res := WrongStop; RETURN FALSE;
+		END;
+
+		uart.mr := uart.mr * SET(-XUARTPS_MR_STOPMODE_MASK) + reg;
+
+		res := Ok;
+		RETURN TRUE
+	END SetStopBits;
+
+	(**
+		Send a single character to the UART
+
+		ch: character to send
+		propagate: TRUE for flushing the TX FIFO buffer
+		res: error code, 0 in case of success
+	*)
+	PROCEDURE SendChar*(uart: UartRegisters; ch: CHAR; propagate: BOOLEAN; onBusy: BusyLoopCallback; VAR res: LONGINT);
+	BEGIN
+		(* Wait until TX FIFO is not full *)
+		WHILE (XUARTPS_CR_TX_EN IN uart.cr) & (XUARTPS_SR_TXFULL IN uart.sr) DO
+			IF (onBusy # NIL) & ~onBusy(res) THEN RETURN; END;
+		END;
+
+		IF (XUARTPS_CR_TX_EN IN uart.cr) THEN
+			uart.fifo := ORD(ch);
+			IF propagate THEN (* flush the FIFO *)
+				WHILE (XUARTPS_CR_TX_EN IN uart.cr) & ~(XUARTPS_SR_TXEMPTY IN uart.sr) DO
+					IF (onBusy # NIL) & ~onBusy(res) THEN RETURN; END;
+				END;
+			END;
+			IF (XUARTPS_CR_TX_EN IN uart.cr) THEN res := Ok; ELSE res := Closed; END;
+		ELSE res := Closed;
+		END;
+	END SendChar;
+
+	(**
+		Receive a single character from UART
+
+		res: error code, 0 in case of success
+
+		Remarks: blocks until a character is available
+	*)
+	PROCEDURE ReceiveChar*(uart: UartRegisters; onBusy: BusyLoopCallback; VAR res: LONGINT): CHAR;
+	BEGIN
+		(* wait until data is available *)
+		WHILE (XUARTPS_CR_RX_EN IN uart.cr) & (XUARTPS_SR_RXEMPTY IN uart.sr) DO
+			IF (onBusy # NIL) & ~onBusy(res) THEN RETURN 0X; END;
+		END;
+
+		IF (XUARTPS_CR_RX_EN IN uart.cr) THEN
+			res := Ok; RETURN CHR(uart.fifo);
+		ELSE
+			res := Closed; RETURN 0X;
+		END;
+	END ReceiveChar;
+
+	(**
+		Returns number of bytes available in the receive FIFO of the UART controller
+
+		res: error code, 0 in case of success
+	*)
+	PROCEDURE Available*(uart: UartRegisters): LONGINT;
+	BEGIN
+		IF ~(XUARTPS_SR_RXEMPTY IN uart.sr) THEN
+			RETURN 1;
+		END;
+		RETURN 0;
+	END Available;
+
+END PsUartMin.

source/Zynq.SdControllers.Mod

@@ -0,0 +1,214 @@
+MODULE SdControllers;
+(**
+	AUTHOR Timothée Martiel, 12/2015
+	PURPOSE SD Host Controller Initialization for Zynq SoC.
+*)
+
+IMPORT
+	Platform, Modules, Objects, Commands,
+	Sd, SdDisks, SdEnvironment, Log := SdEnvironment;
+
+CONST
+	Ready = 0;
+	Running = 1;
+	Stopped = 2;
+	Error = 3;
+
+TYPE
+	HostController * = OBJECT
+	VAR
+		hc: Sd.HostController;
+		handler: InterruptHandler;
+		state, event: LONGINT;
+		card: Sd.Card;
+		halted: BOOLEAN;
+
+		PROCEDURE & Init (base: ADDRESS; int: LONGINT; clock: HUGEINT);
+		VAR
+			result: LONGINT;
+		BEGIN
+			NEW(hc);
+			Sd.InitHostController(hc, base);
+			IF ~Sd.SetExternalClock(hc, clock, clock, result) THEN
+				Log.String("[SD] Failed to initialize host controller: error code ");
+				Log.Int(result, 0);
+				Log.Ln;
+				state := Error;
+				RETURN
+			END;
+			state := Ready; (* Do not put this later, as events might be triggered in the constructor *)
+			NEW(handler, SELF);
+			SdEnvironment.InstallHandler(handler.Handle, int);
+		END Init;
+
+		PROCEDURE HandleEvent (card: Sd.Card; event: LONGINT; param: ANY);
+		BEGIN {EXCLUSIVE}
+			AWAIT(state # Running);
+			SELF.event := event;
+			SELF.card := card;
+			IF state = Ready THEN state := Running END
+		END HandleEvent;
+
+		PROCEDURE Stop;
+		BEGIN {EXCLUSIVE}
+			IF state < Stopped THEN state := Stopped END;
+			AWAIT(halted)
+		END Stop;
+
+		PROCEDURE WaitForEventCompletion;
+		BEGIN {EXCLUSIVE}
+			AWAIT(state # Running)
+		END WaitForEventCompletion;
+
+	BEGIN {ACTIVE}
+		LOOP
+			BEGIN {EXCLUSIVE}
+				AWAIT(state # Ready);
+				IF state >= Stopped THEN EXIT END;
+			END;
+			SdDisks.HandleSdEvent(card, event);
+			BEGIN {EXCLUSIVE}
+				state := Ready
+			END
+		END
+	FINALLY
+		BEGIN {EXCLUSIVE} halted := TRUE END
+	END HostController;
+
+	InterruptHandler * = OBJECT
+	VAR
+		hc: Sd.HostController;
+		timer: Objects.Timer;
+		mask: SET;
+		blocked: BOOLEAN;
+
+		PROCEDURE & Init (hc: HostController);
+		BEGIN
+			SELF.hc := hc.hc;
+			NEW(timer);
+			blocked := FALSE
+		END Init;
+
+		PROCEDURE Block (hc: Sd.HostController; mask: SET; timeout: LONGINT): BOOLEAN;
+		VAR
+			irqs: SET;
+		BEGIN {EXCLUSIVE}
+			ASSERT(hc = SELF.hc);
+			blocked := TRUE;
+			irqs := hc.regs.InterruptSignalEnable;
+			hc.regs.InterruptSignalEnable := irqs + mask;
+			SELF.mask := mask;
+			Objects.SetTimeout(timer, Unblock, timeout);
+			AWAIT(~blocked);
+			hc.regs.InterruptSignalEnable := irqs;
+			RETURN mask * hc.regs.InterruptStatus # {}
+		END Block;
+
+		PROCEDURE Unblock;
+		BEGIN {EXCLUSIVE}
+			blocked := FALSE
+		END Unblock;
+
+		PROCEDURE Handle;
+		BEGIN
+			IF hc.regs.InterruptStatus * mask # {} THEN
+				Unblock;
+				Objects.CancelTimeout(timer);
+			END;
+			Sd.HandleInterrupt(hc);
+		END Handle;
+	END InterruptHandler;
+
+VAR
+	hc: ARRAY 2 OF HostController;
+
+	PROCEDURE Init;
+	VAR
+		i: LONGINT;
+	BEGIN
+		Modules.InstallTermHandler(Cleanup);
+		FOR i := 0 TO Platform.SdNb - 1 DO
+			IF SdEnvironment.Enable(i) THEN
+				IF Sd.EnableTrace THEN
+					Log.String("[SD] Enabling controller "); Log.Int(i, 0); Log.Ln;
+					Log.String("[SD]	register base = "); Log.Address(Platform.SdBase[i]); Log.Ln;
+					Log.String("[SD]	base clock = "); Log.Int(SdEnvironment.HcClock(i), 0); Log.String(" Hz"); Log.Ln;
+					Log.String("[SD]	irq = "); Log.Int(Platform.SdIrq[i], 0); Log.Ln;
+				END;
+				NEW(hc[i], Platform.SdBase[i], Platform.SdIrq[i], SdEnvironment.HcClock(i));
+				Sd.SetEventHandler(hc[i].hc, hc[i].HandleEvent, NIL);
+				hc[i].WaitForEventCompletion;
+(*				Sd.SetBlocker(hc[i].hc, hc[i].handler.Block)*)
+			ELSE
+				IF Sd.EnableTrace THEN
+					Log.String("[SD] Not Enabling controller "); Log.Int(i, 0); Log.Ln
+				END;
+			END
+		END
+	END Init;
+
+	PROCEDURE Cleanup;
+	VAR
+		i: LONGINT;
+	BEGIN
+		FOR i := 0 TO LEN(hc) - 1 DO
+			IF hc[i] # NIL THEN hc[i].Stop END
+		END
+	END Cleanup;
+
+	PROCEDURE Statistics * (c: Commands.Context);
+	VAR
+		accesses: LONGINT;
+		byteRead, byteWritten, read, write: HUGEINT;
+		tread, twrite: HUGEINT;
+		speedR, speedW: LONGREAL;
+	BEGIN
+		byteRead := Sd.NbyteRead;
+		byteWritten := Sd.NbyteWritten;
+		read := Sd.Nread;
+		write := Sd.Nwrite;
+		tread := SdEnvironment.ToMicro(Sd.Tread);
+		twrite := SdEnvironment.ToMicro(Sd.Twrite);
+
+		IF read > 0 THEN
+			speedR := LONGREAL(byteRead) / LONGREAL(tread);
+			c.out.String("SD Statistics:"); c.out.Ln;
+			c.out.String("  Bytes read: "); c.out.Int(byteRead, 0); c.out.Ln;
+			c.out.String("  Number of reads: "); c.out.Int(read, 0); c.out.Ln;
+			c.out.String("  Read time: "); c.out.Int(tread, 0); c.out.String(" us"); c.out.Ln;
+			c.out.String("  Average read size: "); c.out.Int(byteRead DIV read, 0); c.out.String(" bytes"); c.out.Ln;
+			c.out.String("  Read speed: "); c.out.FloatFix(speedR, 0, 3, 0); c.out.String(" Mb/s"); c.out.Ln;
+			c.out.String("  Average read time: "); c.out.FloatFix(LONGREAL(tread) / LONGREAL(read), 0, 3, 0); c.out.String(" us"); c.out.Ln;
+		ELSE
+			c.out.String("No read statistics");
+			c.out.Ln
+		END;
+
+		IF write > 0 THEN
+			speedW := LONGREAL(byteWritten) / LONGREAL(twrite);
+			c.out.String("  Bytes written: "); c.out.Int(byteWritten, 0); c.out.Ln;
+			c.out.String("  Number of writes: "); c.out.Int(write, 0); c.out.Ln;
+			c.out.String("  Write time: "); c.out.Int(twrite, 0); c.out.String(" us"); c.out.Ln;
+			c.out.String("  Average write size: "); c.out.Int(byteWritten DIV write, 0); c.out.String(" bytes"); c.out.Ln;
+			c.out.String("  Write speed: "); c.out.FloatFix(speedW, 0, 3, 0); c.out.String(" Mb/s"); c.out.Ln;
+			c.out.String("  Average write time: "); c.out.FloatFix(LONGREAL(twrite) / LONGREAL(write), 0, 3, 0); c.out.String(" us"); c.out.Ln;
+		ELSE
+			c.out.String("No write statistics");
+			c.out.Ln
+		END;
+
+		accesses := SdDisks.NcacheHits + SdDisks.NcacheMiss;
+		c.out.String("SD Disks Cache Statistics"); c.out.Ln;
+		c.out.String("  Number of accesses: "); c.out.Int(accesses, 0); c.out.Ln;
+		c.out.String("  Number of hits: "); c.out.Int(SdDisks.NcacheHits, 0); c.out.String(" ("); c.out.FloatFix(SdDisks.NcacheHits / accesses * 100.0, 0, 2, 0); c.out.String(" %)"); c.out.Ln;
+		c.out.String("  Number of misses: "); c.out.Int(SdDisks.NcacheMiss, 0); c.out.String(" ("); c.out.FloatFix(SdDisks.NcacheMiss / accesses * 100.0, 0, 2, 0); c.out.String(" %)"); c.out.Ln;
+		c.out.String("  Number of evictions: "); c.out.Int(SdDisks.NcacheEvict, 0); c.out.String(" ("); c.out.FloatFix(SdDisks.NcacheEvict / SdDisks.NcacheMiss * 100.0, 0, 2, 0); c.out.String(" %)"); c.out.Ln;
+		c.out.String("SD Disks Write Buffer Statistics"); c.out.Ln;
+		c.out.String("  Average write size: "); c.out.FloatFix(SdDisks.NbufferSize / SdDisks.NbufferWrites, 0, 2, 0); c.out.String(" bytes"); c.out.Ln;
+		c.out.String("  Average queue length: "); c.out.FloatFix(SdDisks.NbufferQueueSize / SdDisks.NbufferQueueSamples, 0, 2, 0); c.out.Ln;
+		c.out.Update;
+	END Statistics;
+
+BEGIN
+	Init
+END SdControllers.

source/Zynq.SystemWatchdog.Mod

@@ -0,0 +1,68 @@
+MODULE SystemWatchdog; (** AUTHOR "Timothée Martiel"; PURPOSE "Zynq system watchdog driver"; *)
+
+IMPORT SYSTEM, Platform;
+
+CONST
+	(** Modes *)
+	Reset * = TRUE; (** In this mode, the watchdog resets the whole system when triggered *)
+	Irq * = FALSE; (** In this mode, the watchdog triggers IRQ 30 *)
+
+	ZmrWdEn = 0;
+	ZmrRstEn = 1;
+	ZmrIrqEn = 2;
+	ZmrIrqLen4 = {};
+	ZmrIrqLen8 = {7};
+	ZmrIrqLen16 = {8};
+	ZmrIrqLen32 = {7, 8};
+	ZmrIrqLenMask = {7, 8};
+	ZmrKey = LSH(LONGINT(0ABCH), 12); (*0AB'C0'00H;*)
+
+	CcrDelayMask = {2 .. 13};
+	CcrPrescalerMask = 3H;
+	CcrKey = 92'00'00H;
+
+	RestartKey = 1999H;
+
+	(** Start the watchdog for delay ms *)
+	PROCEDURE Start * (mode: BOOLEAN; delay: LONGINT);
+	VAR val: SET;
+	BEGIN
+		Stop;
+		(* Set clock input *)
+		Platform.slcr.SLCR_UNLOCK := Platform.SlcrUnlockKey;
+		Platform.slcr.WDT_CLK_SEL := 0;
+		Platform.slcr.SLCR_LOCK := Platform.SlcrLockKey;
+
+		(* Set delay *)
+		val := SYSTEM.VAL(SET, LSH(delay * 150'000', -12) + 1) * CcrDelayMask;
+		Platform.swdt.XWDTPS_CCR_OFFSET := CcrKey + SYSTEM.VAL(LONGINT, val);
+
+		(* Enable Watchdog *)
+		IF mode THEN
+			INCL(val, ZmrRstEn)
+		ELSE
+			INCL(val, ZmrIrqEn)
+		END;
+		INCL(val, ZmrWdEn);
+		val := val + ZmrIrqLen4;
+		Platform.swdt.XWDTPS_ZMR_OFFSET := ZmrKey + SYSTEM.VAL(LONGINT, val);
+		Feed
+	END Start;
+
+	(** Stop the watchdog *)
+	PROCEDURE Stop *;
+	BEGIN
+		Platform.swdt.XWDTPS_ZMR_OFFSET := ZmrKey
+	END Stop;
+
+	(** Feed the watchdog: set its count to delay ms *)
+	PROCEDURE Feed *;
+	BEGIN
+		Platform.swdt.XWDTPS_RESTART_OFFSET := RestartKey;
+	END Feed;
+
+	PROCEDURE Test *;
+	BEGIN
+		Start(Reset, 100)
+	END Test;
+END SystemWatchdog.

source/Zynq.UsbEhciPhy.Mod

@@ -0,0 +1,118 @@
+MODULE UsbEhciPhy; (** AUTHOR "Timothée Martiel"; PURPOSE "TUSB1210 USB EHCI PHY Control"; *)
+
+IMPORT SYSTEM, Kernel, KernelLog, Gpio, UsbDebug;
+
+CONST
+	(* Ulpi Viewport bits *)
+	UlpiWakeup = 31;
+	UlpiRun = 30;
+	UlpiWrite = 29;
+	UlpiAddressMask = {16 .. 23};
+	UlpiAddressOfs = 16;
+	UlpiWriteDataMask = {0 .. 7};
+
+	(* ULPI addresses *)
+	FuncCtrl = 4H;
+	FuncCtrlSet = 5H;
+	FuncCtrlClr = 6H;
+	IfcCtrlSet = 8H;
+	IfcCtrlClr = 9H;
+	OtgCtrlSet = 0BH;
+	OtgCtrlClr = 0CH;
+
+	(* Register bits in FuncCtrl *)
+	Reset = 5;
+	SuspendM = 6;
+	Opmode = {3, 4};
+	OpmodeNorm = {};
+	XcvrSelect = {0, 1};
+	XcvrHS = {};
+	XcvrFS = {0};
+	XcvrLS = {1};
+	
+
+	(* Register bits in OtgCtrl *)
+	IdPullUp = 0;
+	DmPullDown = 2;
+	DpPullDown = 1;
+	DrvVbus = 5;
+	DrvVbusExt = 6;
+
+	(* Default Timeout. Value comes from Linux implementation *)
+	Timeout = 2000;
+
+	(** Wakeup ULPI *)
+	PROCEDURE Wakeup (viewport: ADDRESS): BOOLEAN;
+	VAR
+		timer: Kernel.MilliTimer;
+		reg: SET;
+	BEGIN
+		Kernel.SetTimer(timer, Timeout);
+		SYSTEM.PUT32(viewport, {UlpiWakeup});
+		REPEAT
+			reg := SYSTEM.VAL(SET, SYSTEM.GET32(viewport))
+		UNTIL ~(31 IN reg) OR Kernel.Expired(timer);
+		IF (31 IN reg) & (UsbDebug.Level >= UsbDebug.Errors) THEN
+			KernelLog.String("TUSB1210 UsbEhciPhy: could not wakeup PHY");
+			KernelLog.Ln
+		END;
+		RETURN ~(31 IN reg)
+	END Wakeup;
+
+	(** Write to ULPI register *)
+	PROCEDURE Write(viewport, address: ADDRESS; value: SET): BOOLEAN;
+	VAR
+		timer: Kernel.MilliTimer;
+		reg: SET;
+	BEGIN
+		IF ~Wakeup(viewport) THEN RETURN FALSE END;
+		Kernel.SetTimer(timer, Timeout);
+		SYSTEM.PUT32(viewport, SYSTEM.VAL(SET, value) * UlpiWriteDataMask + SYSTEM.VAL(SET, LSH(address, UlpiAddressOfs)) * UlpiAddressMask + {UlpiWrite, UlpiRun});
+		REPEAT
+			SYSTEM.GET(viewport, reg);
+		UNTIL ~(30 IN reg) OR Kernel.Expired(timer);
+		IF (30 IN reg) & (UsbDebug.Level >= UsbDebug.Errors) THEN
+			KernelLog.String("TUSB1210 UsbEhcuPhy: could not write to PHY");
+			KernelLog.Ln
+		END;
+		RETURN ~(30 IN reg)
+	END Write;
+
+	(**
+	 * Inits the ULPI via the Viewport register of the EHCI controller.
+	 * Has to be done when the controller is configured and running.
+	 *
+	 * 'viewport' is the address of the viewport register. 'reset' is the GPIO
+	 * pin to which the full ULPI reset is wired (negative if not available).
+	 *)
+	PROCEDURE Init * (viewport: ADDRESS; reset: LONGINT): BOOLEAN;
+	VAR
+		i: LONGINT;
+	BEGIN
+		IF reset >= 0 THEN
+			Gpio.SetDirection(reset, Gpio.Output);
+			Gpio.EnableOutput(reset, TRUE);
+			Gpio.SetData(reset, TRUE);
+			Gpio.SetData(reset, FALSE);
+			(*! TODO: Wait 2 us *)
+			FOR i := 0 TO 1000000 DO END;
+			Gpio.SetData(reset, TRUE)
+		ELSE
+			KernelLog.Enter; KernelLog.String("Skipping GPIO USB reset"); KernelLog.Exit
+		END;
+
+		(* Reset *)
+		IF viewport # 0 THEN
+			IF ~Write(viewport, FuncCtrlSet, {Reset}) THEN RETURN FALSE END;
+			IF ~Write(viewport, OtgCtrlSet, {DmPullDown, DpPullDown, IdPullUp}) THEN RETURN FALSE END;
+			IF ~Write(viewport, FuncCtrlSet, {2, SuspendM} + OpmodeNorm + XcvrLS) THEN RETURN FALSE END;
+			IF ~Write(viewport, FuncCtrlClr, {0 .. 6} - OpmodeNorm  - XcvrLS - {2, SuspendM}) THEN RETURN FALSE END;
+			IF ~Write(viewport, OtgCtrlSet, {DrvVbus, DrvVbusExt}) THEN RETURN FALSE END;
+			IF ~Write(viewport, FuncCtrlClr, {2}) THEN RETURN FALSE END
+		ELSE
+			KernelLog.Enter; KernelLog.String("Skipping USB Viewport reset"); KernelLog.Exit
+		END;
+		KernelLog.Enter; KernelLog.String("USB PHY Initialized sucessfully"); KernelLog.Exit;
+		RETURN TRUE
+	END Init;
+END UsbEhciPhy.

source/Zynq.UsbEhciZynq.Mod

@@ -0,0 +1,200 @@
+MODULE UsbEhciZynq; (** AUTHOR "Timothée Martiel"; PURPOSE "Instal EHCI driver on Zynq systems."; *)
+(**
+ * The main purpose of this module is to install the EHCI driver on Zynq SoCs.
+ * Call
+ * 		UsbEhciZynq.Install ~
+ * to install the driver and unload this module ro remove it.
+ *
+ * This module also patches the behavior of the EHCI driver to the Zynq host controller. The host controller is
+ * not fully compliant with the EHCI specifications and has On-The-Go features that require additional steps in
+ * the initialization. These differences are accounted for in the EnhancedHostController object provided here.
+ *)
+IMPORT SYSTEM, Platform, BootConfig, Machine, Objects, Kernel, KernelLog, Debug := UsbDebug, Usbdi, Usb, UsbHcdi, UsbEhci, UsbEhciPhy;
+
+CONST
+	HcUlpiViewport = 30H;
+	HcOtgSc = 64H;
+	HcUsbMode = 68H;
+	TTCtrl * = 1CH;
+
+	(* HcUsbMode bits *)
+	ModeHost = {0, 1};
+	ModeDevice = {1};
+	ModeIdle = {};
+	
+	(* HcOtgSc bits *)
+	OtgScId = 8;
+	OtgScIdPu = 5;
+
+	(* HcPortSc bits *)
+	PortScPortSpeed = {26, 27};
+	PortScPortSpeedLow = {26};
+	PortScPortSpeedFull = {};
+	PortScPortSpeedHigh = {27};
+
+TYPE
+	(** Zynq-specific EHCI driver. Uses the main implementation and adds Zynq-specific initialization code where needed. *)
+	EnhancedHostController = OBJECT (UsbEhci.EnhancedHostController)
+	VAR
+		phyResetGpio: LONGINT;
+
+		PROCEDURE GetPortStatus * (port : LONGINT; ack : BOOLEAN) : SET;
+		VAR
+			status, dword: SET;
+		BEGIN
+			status := GetPortStatus^(port, ack);
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(ports[port]));
+			IF UsbHcdi.PortStatusEnabled * status # {} THEN
+				IF dword * PortScPortSpeed = PortScPortSpeedHigh THEN
+					status := status + UsbHcdi.PortStatusHighSpeed
+				ELSIF dword * PortScPortSpeed = PortScPortSpeedLow THEN
+					status := status + UsbHcdi.PortStatusLowSpeed
+				ELSIF dword * PortScPortSpeed = PortScPortSpeedFull THEN
+					status := status + UsbHcdi.PortStatusFullSpeed
+				END
+			END;
+			RETURN status
+		END GetPortStatus;
+
+		PROCEDURE HasCompanion * (): BOOLEAN;
+		BEGIN
+			RETURN FALSE
+		END HasCompanion;
+
+		PROCEDURE ResetAndEnablePort (port: LONGINT): BOOLEAN;
+		VAR
+			dword: SET;
+			res: BOOLEAN;
+		BEGIN
+			res := ResetAndEnablePort^(port);
+			IF res THEN
+				dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + HcUsbMode));
+				dword := dword + ModeHost;
+				SYSTEM.PUT32(iobase + HcUsbMode, dword);
+			END;
+			RETURN res
+		END ResetAndEnablePort;
+
+		(* Reset the host controller. Note: This will NOT assert reset on the USB downstream ports. *)
+		PROCEDURE HardwareReset() : BOOLEAN;
+		VAR
+			viewportInit: ARRAY 32 OF CHAR;
+			viewport: LONGINT;
+			dword: SET;
+			res: BOOLEAN;
+		BEGIN
+			(* Set mode to host *)
+			SYSTEM.GET(iobase + HcUsbMode, dword);
+			SYSTEM.PUT(iobase + HcUsbMode, dword + ModeHost);
+
+			res := HardwareReset^();
+
+			IF res THEN
+				(* Set mode to host *)
+				SYSTEM.GET(iobase + HcUsbMode, dword);
+				SYSTEM.PUT(iobase + HcUsbMode, dword + ModeHost);
+				SYSTEM.GET(iobase + HcOtgSc, dword);
+				INCL(dword, 7);
+				INCL(dword, 5);
+				SYSTEM.PUT(iobase + HcOtgSc, dword);
+
+				(* Try putting port in full-speed mode *)
+				SYSTEM.PUT(iobase + UsbEhci.HcPortSc, {8});
+			END;
+			IF ~res THEN
+				RETURN FALSE
+			END;
+			Machine.GetConfig('UsbViewportInit', viewportInit);
+			IF viewportInit = '0' THEN
+				viewport := 0
+			ELSE
+				viewport := iobase + HcUlpiViewport
+			END;
+			RETURN UsbEhciPhy.Init(viewport, phyResetGpio)
+		END HardwareReset;
+
+		(*
+		 * Start the host controller.
+		 * This will:
+		 * - enable interrupts for the host controller and install a interrupt handler
+		 * - set the addresses for the periodic and asynchronous lists
+		 * - turn the host controller on
+		 * - route all ports to the EHCI controller
+		 * - power on all ports of the root hub
+	  	 *)
+		PROCEDURE Start():BOOLEAN;
+		VAR dword : SET;
+			res: BOOLEAN;
+		BEGIN
+			res := Start^();	
+			IF ~res THEN RETURN FALSE END;
+
+			(* Clear interrupts *)
+			SYSTEM.PUT32(iobase + UsbEhci.HcUsbSts, 0);
+			
+			(* Enable all interrupts except the frame list rollover interrupt *)
+			dword := SYSTEM.VAL(SET, SYSTEM.GET32(iobase + UsbEhci.HcUsbIntr));
+			interruptsEnabled := dword + {0 .. 5} - UsbEhci.StsFrameListRollover + {19} (* UPI: triggered by iTD IOC *);
+			SYSTEM.PUT32(iobase + UsbEhci.HcUsbIntr, interruptsEnabled);
+
+			(* Set the TT HubAddress to 7FH *)
+			(*SYSTEM.PUT32(iobase + 1CH, SYSTEM.VAL(SET, SYSTEM.GET32(iobase + (* TTCTRL *)1CH)) + {24 .. 30});*)
+
+			RETURN TRUE
+		END Start;
+
+		PROCEDURE Schedule * (transfer: UsbHcdi.TransferToken);
+		VAR
+			address: LONGINT;
+		BEGIN
+			IF (transfer.pipe.speed = UsbHcdi.LowSpeed) OR (transfer.pipe.speed = UsbHcdi.FullSpeed) THEN
+				address := transfer.pipe.device(Usb.UsbDevice).ttAddress;
+				IF address = 0 THEN
+					address := transfer.pipe.address
+				END;
+			END;
+			SYSTEM.PUT32(iobase + TTCtrl, LSH(address, 24));
+			Schedule^(transfer);
+		END Schedule;
+	END EnhancedHostController;
+
+VAR
+	i: LONGINT;
+
+	(**
+	 * Initializes and install the EHCI host controller driver on the host controller mapped at address iobase
+	 * and using interrupt number irq.
+	 *)
+	PROCEDURE Init(irq, phyResetGpio: LONGINT; iobase: ADDRESS);
+	CONST
+		(* Some part of the EHCI driver is PCI-related. These are dummy values for it. *)
+		bus = 0;
+		device = 0;
+		function = 0;
+	VAR
+		hostController: EnhancedHostController;
+	BEGIN
+		NEW(hostController, bus, device, function);
+		hostController.phyResetGpio := phyResetGpio;
+		IF hostController.Init(iobase, irq) THEN
+			KernelLog.String("UsbEhci: Initialised USB Enhanced Host Controller."); KernelLog.Ln;
+			UsbHcdi.RegisterHostController(hostController, UsbEhci.Description);
+		ELSE
+			KernelLog.String("UsbEhci: Cannot init USB Enhanced Host Controller."); KernelLog.Ln;
+		END;
+	END Init;
+
+	PROCEDURE Install*;
+	(* Load module *)
+	END Install;
+
+BEGIN
+	IF BootConfig.GetBoolValue("UsbEnable0") THEN
+		KernelLog.Enter; KernelLog.String("Initializing USB"); KernelLog.Int(0, 0); KernelLog.String(", Address = "); KernelLog.Address(Platform.UsbBase[0]); KernelLog.String(", IRQ = "); KernelLog.Int(Platform.UsbIrq[0], 0); KernelLog.String(", PHY Reset = "); KernelLog.Int(BootConfig.GetIntValue("UsbPhyRstGpio0"), 0); KernelLog.Exit;
+		Init(Platform.UsbIrq[0], BootConfig.GetIntValue("UsbPhyRstGpio0"), Platform.UsbBase[0])
+	END;
+	IF BootConfig.GetBoolValue("UsbEnable1") THEN
+		KernelLog.Enter; KernelLog.String("Initializing USB"); KernelLog.Int(1, 0); KernelLog.String(", Address = "); KernelLog.Address(Platform.UsbBase[1]); KernelLog.String(", IRQ = "); KernelLog.Int(Platform.UsbIrq[1], 0); KernelLog.String(", PHY Reset = "); KernelLog.Int(BootConfig.GetIntValue("UsbPhyRstGpio1"), 0); KernelLog.Exit;
+		Init(Platform.UsbIrq[1], BootConfig.GetIntValue("UsbPhyRstGpio1"), Platform.UsbBase[1])
+	END
+END UsbEhciZynq.

source/Zynq.XEmac.Mod

@@ -0,0 +1,1602 @@
+MODULE XEmac; (** AUTHOR "Timothee Martiel"; PURPOSE "Zynq7000 Ethernet Controller Driver"; *)
+(** 2014.08.26		Adapted the driver for Minos to A2 *)
+
+IMPORT SYSTEM, Machine, Objects, Kernel, KernelLog, Network;
+
+CONST
+	Trace = FALSE;
+
+	IRQ = 54;
+
+	(* Directions in ErrorHandler *)
+	Send = 1X;
+	Recv = 2X;
+
+	(* Link speed detect modes *)
+	LinkSpeedAutodetect = 0;
+	LinkSpeed10 = 1;
+	LinkSpeed100 = 2;
+	LinkSpeed1000 = 3;
+	LinkSpeedMode = LinkSpeedAutodetect;
+
+	(* state *)
+	IsReady = 11111111H;
+	IsStarted = 22222222H;
+
+	(* Buffer descriptors *)
+   	BdSize = 8;
+	RxBdCount = 512;
+	TxBdCount = 512;
+	BdAdrOffset = 0H;
+	BdStatOffset = 4H;
+
+	BufSize = 1536;
+
+	ZeroMAC = SYSTEM.VAL(Network.LinkAdr, [0X, 0X, 0X, 0X, 0X, 0X, 0X, 0X]);
+
+	MaxTypeId = 4;
+
+	(* Options *)
+	(**
+	 * Accept all incoming packets.
+	 * This option defaults to disabled (cleared)
+	 *)
+	Promisc = 0;
+	(**
+	 * Frame larger than 1516 support for Tx & Rx.
+	 * This option defaults to disabled (cleared)
+	 *)
+	Frame1536 = 1;(* 00000002H; *)
+	(**
+	 * VLAN Rx & Tx frame support.
+	 * This option defaults to disabled (cleared)
+	 *)
+	Vlan = 2;(* 00000004H; *)
+	(**
+	 * Enable recognition of flow control frames on Rx
+	 * This option defaults to enabled (set)
+	 *)
+	FlowControl = 4;(* 00000010H; *)
+	(**
+	 * Strip FCS and PAD from incoming frames. Note: PAD from VLAN frames is not
+	 * stripped.
+	 * This option defaults to enabled (set)
+	 *)
+	FcsStrip = 5;(* 00000020H; *)
+	(**
+	 * Generate FCS field and add PAD automatically for outgoing frames.
+	 * This option defaults to disabled (cleared)
+	 *)
+	FcsInsert = 6;(* 00000040H; *)
+	(**
+	 * Enable Length/TYPE error checking for incoming frames. When this option is
+	 * set, the MAC will filter frames that have a mismatched TYPE/length field
+	 * and if REPORT_RXERR is set, the user is notified when these
+	 * TYPEs of frames are encountered. When this option is cleared, the MAC will
+	 * allow these TYPEs of frames to be received.
+	 *
+	 * This option defaults to disabled (cleared)
+	 *)
+	LenTypeErr = 7;(* 00000080H; *)
+	(**
+	 * Enable the transmitter.
+	 * This option defaults to enabled (set)
+	 *)
+	TransmitterEnable = 8;(* 00000100H; *)
+	(**
+	 * Enable the receiver
+	 * This option defaults to enabled (set)
+	 *)
+	ReceiverEnable = 9;(*00000200H *);
+	(**
+	 * Allow reception of the broadcast address
+	 * This option defaults to enabled (set)
+	 *)
+	Broadcast = 10;(* 00000400H; *)
+	(**
+	 * Allows reception of multicast addresses programmed into hash
+	 * This option defaults to disabled (clear)
+	 *)
+	Multicast = 11;(* 00000800H; *)
+	(**
+	 * Enable the RX checksum offload
+	 * This option defaults to enabled (set)
+	 *)
+	RxChksumEnable = 12;(* 00001000H; *)
+	(**
+	 * Enable the TX checksum offload
+	 * This option defaults to enabled (set)
+	 *)
+	TxChksumEnable = 13;(* 00002000H; *)
+	Defaults = {Promisc, FlowControl, FcsInsert, FcsStrip, Broadcast, LenTypeErr, TransmitterEnable, ReceiverEnable, RxChksumEnable, TxChksumEnable};
+
+	(* Register relative addresses *)
+	Nwctrl = 0H;
+	Nwcfg = 4H;
+	Nwsr = 8H;
+	Dmacr = 10H;
+	Txsr = 14H;
+	Rxqbase = 18H;
+	Txqbase = 1CH;
+	Rxsr = 20H;
+	Isr = 24H;
+	Ier = 28H;
+	Idr = 2CH;
+	Imr = 30H;
+	PhyMntnc = 34H;
+	Rxpause = 38H;
+	Txpause = 3CH;
+	HashL = 80H;
+	HashH = 84H;
+	Last = 1B4H;
+	Laddr1l = 88H;
+	Laddr1h = 8CH;
+	Laddr2l = 90H;
+	Laddr2h = 94H;
+	Laddr3l = 98H;
+	Laddr3h = 9CH;
+	Laddr4l = 0A0H;
+	Laddr4h = 0A4H;
+	Match1 = 0A8H;
+	Match2 = 0ACH;
+	Match3 = 0B0H;
+	Match4 = 0B4H;
+	Stretch = 0BCH;
+	OctTxL = 100H;
+	OctTxH = 104H;
+	Txcnt = 108H;
+	Txbccnt = 10CH;
+	Txmccnt = 110H;
+	Txpausecnt = 114H;
+	Tx64cnt = 118H;
+	Tx65cnt = 11CH;
+	Tx128cnt = 120H;
+	Tx256cnt = 124H;
+	Tx512cnt = 128H;
+	Tx1024cnt = 12CH;
+	Tx1519cnt = 130H;
+	Txuruncnt = 134H;
+	Snglcollcnt = 138H;
+	Multicollcnt = 13CH;
+	Excesscollcnt = 140H;
+	Latecollcnt = 144H;
+	Txdefercnt = 148H;
+	Txcsensecnt = 14CH;
+	Octrxl = 150H;
+	Octrxh = 154H;
+	Rxcnt = 158H;
+	Rxbroadcnt = 15CH;
+	Rxmulticnt = 160H;
+	Rxpausecnt = 164H;
+	Rx64cnt = 168H;
+	Rx65cnt = 16CH;
+	Rx128cnt = 170H;
+	Rx256cnt = 174H;
+	Rx512cnt = 178H;
+	Rx1024cnt = 17CH;
+	Rx1519cnt = 180H;
+	Rxundrcnt = 184H;
+	Rxovrcnt = 188H;
+	Rxjabcnt = 18CH;
+	Rxfcscnt = 190H;
+	Rxlengthcnt = 194H;
+	Rxsymbcnt = 198H;
+	Rxaligncnt = 19CH;
+	Rxreserrcnt = 1A0H;
+	Rxorcnt = 1A4H;
+	Rxipccnt = 1A8H;
+	Rxtcpccnt = 1ACH;
+	Rxudpccnt = 1B0H;
+	C1588_sec = 1D0H;
+	C1588_nanosec = 1D4H;
+	C1588_adj = 1D8H;
+	C1588_inc = 1DCH;
+	Ptp_txsec = 1E0H;
+	Ptp_txnanosec = 1E4H;
+	Ptp_rxsec = 1E8H;
+	Ptp_rxnanosec = 1ECH;
+	Ptpp_txsec = 1F0H;
+	Ptpp_txnanosec = 1F4H;
+	Ptpp_rxsec = 1F8H;
+	Ptpp_rxnanosec = 1FCH;
+
+	(** Masks and bits *)
+	DmacrRxBuf = {16 .. 23};
+	DmacrRxBufShift = 16;
+	DmacrTcpChksum = 11;
+	DmacrTxSize = 10;
+	DmacrRxSize = {8, 9};
+	DmacrEndian = 7;
+	DmacrBlength = {0 .. 4};
+	DmacrSingleAhbBurst = 0;
+	DmacrIncr4AhbBurst = 2;
+	DmacrIncr8AhbBurst = 3;
+	DmacrIncr16AhbBurst = 4;
+
+	IxrPtppstx = 25;
+	IxrPtppdrtx = 24;
+	IxrPtpstx = 23;
+	IxrPtpdrtx = 22;
+
+	IxrPtppsrx = 21;
+	IxrPtppdrrx = 20;
+	IxrPtpsrx = 19;
+	IxrPtpdrrx = 18;
+
+	IxrPauseTx = 14;
+	IxrPauseZero = 13;
+	IxrPauseNzero = 12;
+	IxrHrespnOk = 11;
+	IxrRxOvr = 10;
+
+	IxrTxCompl = 7;
+	IxrTxExh = 6;
+	IxrRetry = 5;
+	IxrUrun = 4;
+
+	IxrTxUsed = 3;
+	IxrRxUsed = 2;
+	IxrFrameRx = 1;
+	IxrMgmnt = 0;
+
+	IxrAll = {0 .. 14};
+	IxrTxErr = {IxrTxExh, IxrRetry, IxrUrun, IxrTxUsed};
+	IxrRxErr = {IxrHrespnOk, IxrRxUsed, IxrRxOvr};
+
+	LaddrMach = {0 .. 31};
+
+	NwcfgBadpreambEn = 29;
+	NwcfgIpdStretch = 28;
+	NwcfgFcsIgnore = 26;
+	NwcfgHdRxEn = 25;
+	NwcfgRxChkSumEn = 24;
+	NwcfgPauseCopyDi = 23;
+	NwcfgMdcShift = 18;
+	NwcfgMdcClkDiv = {18 .. 20};
+	NwcfgFcsRem = 17;
+	NwcfgLengthErrDscrd = 16;
+	Nwcfg1000 = 10;
+	Nwcfg100 = 0;
+	NwcfgUcastHashEn = 7;
+	NwcfgFdEn = 1;
+	Nwcfg1536RxEn = 8;
+	NwcfgNvlanDisc = 2;
+	NwcfgPauseEn = 13;
+	NwcfgCopyAllEn = 4;
+	NwcfgBcastDi = 5;
+	NwcfgMcastHashEn = 6;
+
+	NwctrlZeroPauseTx = {11};
+	NwctrlPauseTx = {11};
+	NwctrlHaltTx = {10};
+	NwctrlStartTx = {9};
+	NwctrlStatwen = {7};
+	NwctrlStatinc = {6};
+	NwctrlStatclr = {5};
+	NwctrlMdEn = {4};
+	NwctrlTxEn = {3};
+	NwctrlRxEn = {2};
+	NwctrlLoopEn = {1};
+
+	NwsrMdio = 1;
+	NwsrMdioIdle = 2;
+
+	PhyMntncOp = {17, 30};
+	PhyMntncOpR = {29};
+	PhyMntncOpW = {28};
+	PhyMntncAddR = {23 .. 27};
+	PhyMntncReg = {18 .. 22};
+	PhyMntncData = {0 .. 15};
+	PhyMntncPhyAdShift = 23;
+	PhyMntncPhRegShift = 18;
+
+	RxsrHrespnok = 3;
+	RxsrRxOvr = 2;
+	RxsrFrameRx = 1;
+	RxsrBuffna = 0;
+	RxsrError = 13;
+
+	RxBufBcast = 31;
+	RxBufMultiHash = 30;
+	RxBufUniHash = 29;
+	RxBufExh = 28;
+	RxBufAMatch = {25,26};
+	RxBufIDfound = 24;
+	RxBufIDmatch = {22,23};
+	RxBufVlan = 21;
+	RxBufPri = 20;
+	RxBufVpri = {17..19};
+	RxBufCfi = 16;
+	RxBufEof = 15;
+	RxBufSof = 14;
+	RxBufLen = {0 .. 13};
+	RxBufWrap = 1;
+	RxBufNew = 0;
+	RxBufAdd = {2 .. 31};
+	RxBufSize = 1536;
+	RxBufUnit = 64;
+
+	TxBufUsed = 31;
+	TxBufWrap = 30;
+	TxBufRetry = 29;
+	TxBufUrun = 28;
+	TxBufExh = 27;
+	TxBufTcp = 26;
+	TxBufNocrc = 16;
+	TxBufLast = 15;
+	TxBufLen = {0 .. 13};
+
+	TxsrHrespnok = 8;
+	TxsrUrun = 6;
+	TxsrTxCompl = 5;
+	TxsrBufExh = 4;
+	TxsrTxGo = 3;
+	TxsrRxOvr = 2;
+	TxsrFrameRx = 1;
+	TxsrUsedRead = 0;
+	TxsrError = {TxsrHrespnok, TxsrUrun, TxsrBufExh, TxsrRxOvr, TxsrFrameRx, TxsrUsedRead};
+
+	(** Clock divisors *)
+	MdcDiv8 = 0;
+	MdcDiv16 = 1;
+	MdcDiv32 = 2;
+	MdcDiv48 = 3;
+	MdcDiv64 = 4;
+	MdcDiv96 = 5;
+	MdcDiv128 = 6;
+	MdcDiv224 = 7;
+
+	(* Phy return state *)
+	EmacMiiReadError = 1003;
+	EmacMiiBusy = 1004;
+	Success = 0;
+	Failure = 1;
+	DeviceIsStarted = 5;
+	DeviceIsStopped = 6;
+
+	(* Phy registers *)
+	IeeeControlReg = 0;
+	IeeeStatusReg = 1;
+	IeeeAutonegoAdvertiseReg = 4;
+	IeeePartnerAbilities1Reg = 5;
+	Ieee1000AdvertiseReg = 9;
+	IeeePartnerAbilities3Reg = 10;
+	IeeeCopperSpecificControlReg = 16;
+	IeeeSpecificStatusReg = 17;
+	IeeeCopperSpecificStatusReg2 = 19;
+	IeeeControlRegMac = 21;
+	IeeePageAddressRegister = 22;
+
+	(* Phy bits and masks *)
+	Advertise10Half = 5;
+	Advertise10Full = 6;
+	Advertise100Half = 7;
+	Advertise100Full = 8;
+	Advertise100 = {Advertise100Full, Advertise100Half};
+	Advertise10 = {Advertise10Full, Advertise10Half};
+	Advertise1000 = {8, 9};
+
+	IeeeCtrl1GbpsLinkspeed = 2040H;
+	IeeeCtrlLinkSpeed = {6};
+	IeeeCtrlLinkSpeed1000M = {6};
+	IeeeCtrlLinkSpeed100M = {13};
+	IeeeCtrlLinkSpeed10M = {};
+	IeeeCtrlReset = {15};
+	IeeeCtrlAutonegotiateEnable = {12};
+	IeeeStatAutonegotiateCapable = {3};
+	IeeeStatAutonegotiateComplete = {5};
+	IeeeStatAutonegotiateRestart = {9};
+	IeeeStat1gbpsExtensions = {8};
+	IeeeAn1Ability = {5 .. 12};
+	IeeeAn3Ability1Gbps = {10 .. 11};
+	IeeeAn1Ability100Mbps = {7 .. 9};
+	IeeeAn1Ability10Mbps = {5 .. 6};
+	IeeeRgmiiTxRxClockDelayed = {4 .. 5};
+
+	IeeeAsymmetricPause = {11};
+	IeeePause = {10};
+	IeeeAutonegError = {15};
+
+	PhyDetectReg = 1;
+	PhyDetect = {3, 11, 12};
+
+TYPE
+	Buffer = ARRAY BufSize OF CHAR;
+
+	(** Implements the Network.LinkDevice abstract class. *)
+	LinkDevice * = OBJECT (Network.LinkDevice)
+		VAR
+			ctrl: Controller;
+
+		PROCEDURE & Constr * (type, mtu, adrSize: LONGINT);
+		VAR
+			i: LONGINT;
+		BEGIN
+			Constr^(type, mtu, adrSize);
+			FOR i := 0 TO LEN(broadcast) - 1 DO
+				broadcast[i] := 0FFX
+			END;
+		END Constr;
+
+		PROCEDURE Linked * (): LONGINT;
+		BEGIN
+			RETURN Network.LinkLinked
+		END Linked;
+
+		PROCEDURE DoSend * (dst: Network.LinkAdr; type: LONGINT; CONST l3hdr, l4hdr, data: ARRAY OF CHAR; h3len, h4len, dofs, dlen: LONGINT);
+		BEGIN
+			ctrl.SendFrame(dst, type, l3hdr, l4hdr, data, h3len, h4len, dofs, dlen);
+		END DoSend;
+
+		PROCEDURE Finalize * (on: BOOLEAN);
+		BEGIN
+			Finalize^(on)
+		END Finalize;
+	END LinkDevice;
+
+	(** Ethernet controller driver. *)
+	Controller = OBJECT
+		VAR
+			(** Base of memory-mapped registers *)
+			iobase: ADDRESS;
+			dev: LinkDevice;
+			phy: Phy;
+			state, isStarted: LONGINT;
+			speed: LONGINT;
+			timer: Kernel.Timer;
+			rxFrames: POINTER TO ARRAY OF Buffer;
+			txFrame: Buffer;
+			lastRxQBar, lastTxQBar: ADDRESS;
+			rxBdBase, txBdBase: ADDRESS;
+			options: SET;
+			txBdStartAdr, txBdEndAdr, rxBdStartAdr, rxBdEndAdr: ADDRESS;
+			kill, start: BOOLEAN;
+
+			(* For DMA interaction *)
+			buffer: POINTER TO ARRAY OF CHAR;
+
+			(** Statistics *)
+			nbrRxFrames, nbrtxFrames, nbrRxErrors, nbrTxErrors: LONGINT;
+
+		(** Initializes the controller.
+			iobase: base address of the registers
+		*)
+		PROCEDURE & Init * (iobase: ADDRESS; ld: LinkDevice);
+		VAR
+			i: LONGINT;
+			reg: SET;
+			(* TEST *)arp: ARRAY 28 OF CHAR;
+			buf: ARRAY 256 OF Network.Buffer;
+		BEGIN
+			NEW(timer);
+			NEW(rxFrames, RxBdCount);
+			options := Defaults;
+			SELF.iobase := iobase;
+			dev := ld;
+			dev.calcChecksum := {Network.ChecksumIP, Network.ChecksumTCP, Network.ChecksumUDP};
+			kill := FALSE;
+			start := FALSE;
+			state := IsReady;
+			Reset;
+
+			(* Initialize HW *)
+			IF Trace THEN KernelLog.String("Zynq.XEmac: Initializing hardware"); KernelLog.Ln END;
+			SetMacAddress(1, SYSTEM.VAL(Network.LinkAdr, [0X, 0AX, 35X, 0X, 1X, 2X, 0X, 0X]));
+			SetMdioDivisor(MdcDiv224);
+			NEW(phy, iobase);
+			speed := phy.linkSpeed;
+			IF speed # 1000 THEN
+				IF Trace THEN KernelLog.String("XEmac: error in speed initialization."); KernelLog.Ln END;
+				RETURN
+			END;
+
+			SetOperatingSpeed(speed);
+
+			SYSTEM.GET(iobase + Dmacr, reg);
+			INCL(reg, 4);
+			SYSTEM.PUT(iobase + Dmacr, reg);
+
+			SetMdioDivisor(MdcDiv224);
+			IF Trace THEN KernelLog.String("XEmac: Initializing DMA"); KernelLog.Ln END;
+			FOR i := 0 TO 6000000 DO END;
+
+			InitDMA;
+			
+			(* preallocate network stack buffers *)
+			FOR i := 0 TO LEN(buf,0)-1 DO
+				buf[i] := Network.GetNewBuffer();
+			END;
+			FOR i := 0 TO LEN(buf,0)-1 DO
+				Network.ReturnBuffer(buf[i]);
+			END;
+
+			IF Trace THEN KernelLog.String("XEmac: Starting controller"); KernelLog.Ln END;
+			Start
+		END Init;
+
+		(* Terminates the process *)
+		PROCEDURE Kill;
+		BEGIN {EXCLUSIVE}
+			kill := TRUE
+		END Kill;
+
+		PROCEDURE Start;
+		VAR
+			reg: SET;
+		BEGIN
+			IF Trace THEN KernelLog.String("Starting XEmac"); KernelLog.Ln END;
+			IF isStarted = IsStarted THEN RETURN END;
+			(* Start DMA *)
+			SYSTEM.PUT(iobase + Rxqbase, rxBdBase);
+			SYSTEM.PUT(iobase + Txqbase, txBdBase);
+
+			(* Clear any existing interrupt status *)
+			SYSTEM.PUT(iobase + Isr, IxrAll);
+
+			(* Optionally enable TX and RX *)
+			IF TransmitterEnable IN options THEN
+				SYSTEM.GET(iobase + Nwctrl, reg);
+				reg := reg + NwctrlTxEn;
+				SYSTEM.PUT(iobase + Nwctrl, reg)
+			END;
+			IF ReceiverEnable IN options THEN
+				SYSTEM.GET(iobase + Nwctrl, reg);
+				reg := reg + NwctrlRxEn;
+				SYSTEM.PUT(iobase + Nwctrl, reg)
+			END;
+
+			(* Enable TX and RX interrupts *)
+			SYSTEM.PUT(iobase + Ier, (IxrTxErr + IxrRxErr + {IxrTxCompl, IxrFrameRx}) * IxrAll);
+
+			(*BEGIN {EXCLUSIVE}
+				start := TRUE
+			END;*)
+		(*VAR
+			reg: SET;
+		BEGIN
+			SYSTEM.GET(iobase + Nwctrl, reg);
+			reg := reg + NwctrlStartTx;
+			SYSTEM.PUT(iobase + Nwctrl, reg)*)
+		END Start;
+
+		PROCEDURE Stop;
+		VAR
+			reg: SET;
+		BEGIN
+			ASSERT(state = IsReady);
+			(* Disable all interrupts *)
+			SYSTEM.PUT32(iobase + Idr, IxrAll);
+			(* Disable TX and RX *)
+			SYSTEM.GET(iobase + Nwctrl, reg);
+			reg := reg - (NwctrlRxEn + NwctrlTxEn);
+			SYSTEM.PUT(iobase + Nwctrl, reg);
+			isStarted := 0
+		END Stop;
+
+		PROCEDURE Transmit;
+		VAR
+			reg: SET;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac: transmitting"); KernelLog.Ln END;
+			SYSTEM.GET(iobase + Nwctrl, reg);
+			reg := reg + NwctrlStartTx;
+			SYSTEM.PUT(iobase + Nwctrl, reg)
+		END Transmit;
+
+		(** Stop and restart device *)
+		PROCEDURE Reset;
+		VAR
+			i, tmp: LONGINT;
+			reg: SET;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac resetting controller"); KernelLog.Ln END;
+			ASSERT(state = IsReady);
+
+			Stop;
+			options := Defaults;
+			reg := (NwctrlStatclr + NwctrlMdEn) * (-NwctrlLoopEn);
+			SYSTEM.PUT(iobase + Nwctrl, reg);
+
+			reg := {};
+			INCL(reg, Nwcfg100);
+			INCL(reg, NwcfgFdEn);
+			INCL(reg, NwcfgUcastHashEn);
+			SYSTEM.PUT(iobase + Nwcfg, reg);
+
+			i := RxBufSize DIV RxBufUnit;
+			IF RxBufSize MOD RxBufUnit # 0 THEN INC(i) END;
+			reg := SYSTEM.VAL(SET, LSH(i, DmacrRxBufShift)) * DmacrRxBuf + DmacrRxSize;
+			INCL(reg, DmacrTxSize);
+			SYSTEM.PUT(iobase + Dmacr, reg);
+
+			SYSTEM.PUT32(iobase + Txsr, 0);
+			SYSTEM.PUT32(iobase + Rxqbase, 0);
+			SYSTEM.PUT32(iobase + Txqbase, 0);
+			SYSTEM.PUT32(iobase + Rxsr, 0);
+			SYSTEM.PUT32(iobase + Idr, IxrAll);
+
+			SYSTEM.GET(iobase + Isr, reg);
+			SYSTEM.PUT(iobase + Isr, reg);
+
+			SYSTEM.PUT32(iobase + PhyMntnc, 0);
+
+			ClearHash;
+
+			(* Clear all MAC addresses *)
+			FOR i := 1 TO 4 DO
+				SetMacAddress(i, ZeroMAC);
+				tmp := SetTypeIdCheck(i, 0)
+			END;
+
+			(* Clear all counters. *)
+			FOR i := 0 TO (Last- OctTxL) DIV 4 - 1 DO
+				SYSTEM.GET(iobase + OctTxL + 4 * i, reg)
+			END;
+
+			(* Disable receiver. *)
+			SYSTEM.GET(iobase + Nwctrl, reg);
+			SYSTEM.PUT(iobase + Nwctrl, reg - NwctrlRxEn);
+
+			SetOptions(options - {TransmitterEnable, ReceiverEnable});
+			ClearOptions(-options);
+		END Reset;
+
+		PROCEDURE ResetDevice;
+		VAR
+			options: SET;
+		BEGIN
+			Stop;
+			options := SELF.options;
+			Reset;
+			SetOptions(options);
+			ClearOptions(-options)
+		END ResetDevice;
+
+		PROCEDURE SendFrame (CONST dst: Network.LinkAdr; type: LONGINT; CONST l3hdr, l4hdr, data: ARRAY OF CHAR; h3len, h4len, dofs, dlen: LONGINT);
+		VAR
+			txLen, offset: LONGINT;
+			bufferAdr, txbd: ADDRESS;
+			intsEnabled: BOOLEAN;
+			reg: SET;
+		BEGIN (*{EXCLUSIVE}*)
+			IF Trace THEN KernelLog.String("XEmac: Sending Frame"); KernelLog.Ln END;
+			(* Prepare buffer *) (* This part is taken from RTL8169.Mod *)
+			txLen := 14 + h3len + h4len + dlen;
+			bufferAdr := ADDRESSOF(txFrame[0]);
+
+			(* set destination mac address (first 6 bytes of eth frame) *)
+			SYSTEM.MOVE(ADDRESSOF(dst[0]), bufferAdr, 6);
+			(*txFrame[0] := dst[0]; txFrame[1] := dst[1]; txFrame[2] := dst[2]; txFrame[3] := dst[3]; txFrame[4] := dst[4]; txFrame[5] := dst[5];*)
+			(* set source mac address (6 bytes @ offset 6 of eth frame) *)
+			SYSTEM.MOVE(ADDRESSOF(dev.local[0]), bufferAdr + 6, 6);
+			(*txFrame[6]:=000X;txFrame[7]:=00AX;txFrame[8]:=035X;txFrame[9]:=000X;txFrame[10]:=001X;txFrame[11]:=002X;*)
+
+			(* set upper layer type, bring type from host to network byte order *)
+			SYSTEM.PUT16(bufferAdr + 12, ROT(SYSTEM.VAL(INTEGER, SHORT(type)), 8));
+
+			(*IF Trace THEN
+				KernelLog.Buffer(txFrame, 0, txLen);
+				KernelLog.Ln
+			END;*)
+
+			offset := 14;
+			(* move layer 3 and layer 4 headers, data *)
+			IF h3len > 0 THEN
+				SYSTEM.MOVE(ADDRESSOF(l3hdr[0]), bufferAdr + offset, h3len);
+				INC(offset, h3len);
+			END;
+			IF h4len > 0 THEN
+				SYSTEM.MOVE(ADDRESSOF(l4hdr[0]), bufferAdr + offset, h4len);
+				INC(offset, h4len);
+			END;
+			IF offset + dlen < BufSize THEN
+				SYSTEM.MOVE(ADDRESSOF(data[0]) + dofs, bufferAdr + offset, dlen);
+				INC(offset, dlen);
+			END;
+
+			(* make the frame at least 64 bytes long *)
+			WHILE offset < 60 DO
+				txFrame[offset] := CHR(0);
+				INC(offset);
+				INC(txLen)
+			END;
+
+			(*IF Trace THEN
+				KernelLog.String("Sending frame of length ");
+				KernelLog.Int(txLen, 0);
+				KernelLog.Ln;
+				KernelLog.Memory(bufferAdr, txLen)
+			END;*)
+
+			(* Send it with DMA *)
+			intsEnabled := Machine.AreInterruptsEnabled();
+			Machine.DisableInterrupts;
+
+			SYSTEM.GET(iobase + Txqbase, txbd);
+			IF txbd = 0 THEN txbd := txBdStartAdr END;
+
+			Machine.FlushDCacheRange(ADDRESSOF(txFrame[0]), txLen);
+
+			SYSTEM.PUT(txbd + BdAdrOffset, ADDRESSOF(txFrame));
+			IF txbd # txBdEndAdr THEN
+				SYSTEM.PUT(txbd + BdStatOffset, {TxBufLast} + SYSTEM.VAL(SET, txLen));
+			ELSE
+				IF Trace THEN KernelLog.String("END OF TXBUFFERS"); KernelLog.Ln END;
+				SYSTEM.PUT(txbd + BdStatOffset, {TxBufLast, TxBufWrap} + SYSTEM.VAL(SET, txLen));
+			END;
+			
+			(* no need to flush/invalidate cache for BD memory space
+			Machine.FlushDCacheRange(txbd, BdSize);
+			*)
+			
+			CODE
+				DSB
+			END;
+
+			(* Transmit *)
+			Transmit;
+
+			IF intsEnabled THEN Machine.EnableInterrupts END;
+		END SendFrame;
+
+		(** Called by the polling body. Signals the new frames to the link device *)
+		PROCEDURE ReceiveFrame;
+		VAR
+			currentBd, adr: ADDRESS;
+			len, i: LONGINT;
+			reg: SET;
+			buf: Network.Buffer;
+			type: INTEGER;
+		BEGIN
+			(*IF Trace THEN KernelLog.String("Receiving frame"); KernelLog.Ln END;*)
+			currentBd := lastRxQBar;
+			SYSTEM.GET(currentBd + BdAdrOffset, adr);
+			adr := adr - (adr MOD 4);
+
+			SYSTEM.GET(currentBd, reg);
+			IF RxBufNew IN reg THEN
+				(*IF Trace THEN
+					KernelLog.String("Current BD: "); KernelLog.Address(currentBd); KernelLog.Ln;
+					KernelLog.String("QBar: "); KernelLog.Address(SYSTEM.GET32(iobase + Rxqbase)); KernelLog.Ln
+				END;*)
+
+				IF SYSTEM.GET32(currentBd + 4) = 0 THEN
+					SYSTEM.GET(iobase + Nwctrl, reg);
+					reg := reg * SET(-NwctrlRxEn);
+					SYSTEM.PUT(iobase + Nwctrl, reg);
+
+					SYSTEM.GET(iobase + Nwctrl, reg);
+					reg := reg + NwctrlRxEn + {18};
+					SYSTEM.PUT(iobase + Nwctrl, reg)
+				ELSE
+					IF Trace THEN KernelLog.String("Receiving frame"); KernelLog.Ln END;
+					SYSTEM.GET(currentBd + BdStatOffset, reg);
+					len := SYSTEM.VAL(LONGINT, reg * RxBufLen);
+					(* Invalidate DCache *)
+					(* no need to flush/invalidate cache for BD memory space
+					Machine.InvalidateDCacheRange(currentBd,BdSize);
+					*)
+					Machine.InvalidateDCacheRange(adr,len);					
+
+					(* Construct Frame *)
+					buf := Network.GetNewBuffer(); ASSERT(buf # NIL);
+					buf.ofs := 14;
+					buf.len := len - 14;
+					buf.next := NIL;
+					buf.prev := NIL;
+					buf.calcChecksum := dev.calcChecksum;
+					
+					SYSTEM.MOVE(adr+6,ADDRESSOF(buf.src[0]),6);
+					
+					type := INTEGER(SYSTEM.GET8(adr + 12)) * 100H + INTEGER(SYSTEM.GET8(adr + 13));
+					SYSTEM.MOVE(adr,ADDRESSOF(buf.data[0]),len);
+					dev.QueueBuffer(buf, type)
+				END
+			END;
+
+			IF currentBd = rxBdEndAdr THEN
+				SYSTEM.PUT32(currentBd + BdAdrOffset, adr + 2);
+				lastRxQBar := rxBdStartAdr
+			ELSE
+				SYSTEM.PUT32(currentBd + BdAdrOffset, adr);
+				INC(lastRxQBar, BdSize)
+			END;
+			SYSTEM.PUT32(currentBd + BdStatOffset, 0);
+			
+			(* no need to flush/invalidate cache for BD memory space
+			Machine.FlushDCacheRange(currentBd, BdSize);
+			*)
+		END ReceiveFrame;
+
+		PROCEDURE SetOptions (opts: SET);
+		VAR
+			reg, regNetCfg, regNewNetCfg: SET;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac.SetOptions: "); KernelLog.Hex(SYSTEM.VAL(ADDRESS, opts), -8); KernelLog.Ln END;
+
+			ASSERT(state = IsReady);
+
+
+			(* Many of these options will change the NET_CONFIG registers.
+			 * To reduce the amount of IO to the device, group these options here
+			 * and change them all at once.
+			 *)
+
+			(* Grab current register contents *)
+			SYSTEM.GET(iobase + Nwcfg, regNetCfg);
+			regNewNetCfg := regNetCfg;
+			IF Trace THEN KernelLog.String("regNetCfg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, regNetCfg), -8); KernelLog.Ln END;
+			(*
+			 * It is configured to max 1536.
+			 *)
+			IF Frame1536 IN opts  THEN
+				regNewNetCfg := regNewNetCfg + {Nwcfg1536RxEn};
+			END;
+
+			(* Turn on VLAN packet only, only VLAN tagged will be accepted *)
+			IF Vlan IN opts  THEN
+				regNewNetCfg := regNewNetCfg + {NwcfgNvlanDisc};
+			END;
+
+			(* Turn on FCS stripping on receive packets *)
+			IF FcsStrip IN opts  THEN
+				regNewNetCfg := regNewNetCfg + {NwcfgFcsRem};
+			END;
+
+			(* Turn on length/type field checking on receive packets *)
+			IF LenTypeErr IN opts  THEN
+				regNewNetCfg := regNewNetCfg + {NwcfgLengthErrDscrd};
+			END;
+
+			(* Turn on flow control *)
+			IF FlowControl IN opts  THEN
+				regNewNetCfg := regNewNetCfg + {NwcfgPauseEn};
+			END;
+
+			(* Turn on promiscuous frame filtering (all frames are received) *)
+			IF Promisc IN opts  THEN
+				regNewNetCfg := regNewNetCfg + {NwcfgCopyAllEn};
+			END;
+
+			(* Allow broadcast address reception *)
+			IF Broadcast IN opts  THEN
+				regNewNetCfg := regNewNetCfg - {NwcfgBcastDi};
+			END;
+
+			(* Allow multicast address filtering *)
+			IF Multicast IN opts  THEN
+				regNewNetCfg := regNewNetCfg + {NwcfgMcastHashEn};
+			END;
+
+			(* enable RX checksum offload *)
+			IF RxChksumEnable IN opts  THEN
+				regNewNetCfg := regNewNetCfg + {NwcfgRxChkSumEn};
+			END;
+
+			(* Officially change the NET_CONFIG registers if it needs to be
+			 * modified.
+			 *)
+			IF regNetCfg # regNewNetCfg THEN
+				SYSTEM.PUT(iobase + Nwcfg, regNewNetCfg);
+			END;
+			IF Trace THEN KernelLog.String("regNewNetCfg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, regNewNetCfg), -8); KernelLog.Ln END;
+
+			(* Enable TX checksum offload *)
+			IF TxChksumEnable IN opts  THEN
+				SYSTEM.GET(iobase + Dmacr, reg);
+				reg := reg + {DmacrTcpChksum};
+				SYSTEM.PUT(iobase + Dmacr, reg);
+				IF Trace THEN KernelLog.String("DMACR_OFFSET reg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, reg), -8); KernelLog.Ln END;
+			END;
+
+			(* Enable transmitter *)
+			IF TransmitterEnable IN opts  THEN
+				SYSTEM.GET(iobase + Nwctrl, reg);
+				reg := reg + NwctrlTxEn;
+				SYSTEM.PUT(iobase + Nwctrl, reg);
+				IF Trace THEN KernelLog.String("NWCTRL_OFFSET reg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, reg), -8); KernelLog.Ln END;
+			END;
+
+			(* Enable receiver *)
+			IF ReceiverEnable IN opts  THEN
+				SYSTEM.GET(iobase + Nwctrl, reg);
+				reg := reg + NwctrlRxEn;
+				SYSTEM.PUT(iobase + Nwctrl, reg);
+				IF Trace THEN KernelLog.String("NWCTRL_OFFSET reg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, reg), -8); KernelLog.Ln END;
+			END;
+
+			(* The remaining options not handled here are managed elsewhere in the
+			 * driver. No register modifications are needed at this time. Reflecting
+			 * the option in InstancePtr->Options is good enough for now.
+			 *)
+
+			(* Set options word to its new value *)
+			SELF.options := SELF.options + opts;
+			IF Trace THEN KernelLog.String("SetOptions END "); KernelLog.Address(SYSTEM.VAL(ADDRESS, SELF.options)); KernelLog.Ln END;
+		END SetOptions;
+
+		PROCEDURE ClearOptions (opts: SET);
+		VAR
+			reg, old: SET;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac: Clear Options"); KernelLog.Ln END;
+
+			SYSTEM.GET(iobase + Nwcfg, reg);
+			old := reg;
+
+			(*
+			 * It is configured to max 1536.
+			 *)
+			IF Frame1536 IN opts  THEN
+				EXCL(reg, Nwcfg1536RxEn);
+			END;
+
+			(* Turn off VLAN packet only, only VLAN tagged will be accepted *)
+			IF Vlan IN opts  THEN
+				EXCL(reg, NwcfgNvlanDisc);
+			END;
+
+			(* Turn off FCS stripping on receive packets *)
+			IF FcsStrip IN opts  THEN
+				EXCL(reg, NwcfgFcsRem);
+			END;
+
+			(* Turn off length/type field checking on receive packets *)
+			IF LenTypeErr IN opts  THEN
+				EXCL(reg, NwcfgLengthErrDscrd);
+			END;
+
+			(* Turn off flow control *)
+			IF FlowControl IN opts  THEN
+				EXCL(reg, NwcfgPauseEn);
+			END;
+
+			(* Turn off promiscuous frame filtering (all frames are received) *)
+			IF Promisc IN opts  THEN
+				EXCL(reg, NwcfgCopyAllEn);
+			END;
+
+			(* Forbid broadcast address reception *)
+			IF Broadcast IN opts  THEN
+				EXCL(reg, NwcfgBcastDi);
+			END;
+
+			(* Forbid multicast address filtering *)
+			IF Multicast IN opts  THEN
+				EXCL(reg, NwcfgMcastHashEn);
+			END;
+
+			(* Disable RX checksum offload *)
+			IF RxChksumEnable IN opts  THEN
+				EXCL(reg, NwcfgRxChkSumEn);
+			END;
+
+			(* Officially change the NET_CONFIG registers if it needs to be
+			 * modified.
+			 *)
+			IF reg # old THEN
+				SYSTEM.PUT(iobase + Nwcfg, reg);
+			END;
+			IF Trace THEN KernelLog.String("regNewNetCfg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, reg), -8); KernelLog.Ln END;
+
+			(* Disable TX checksum offload *)
+			IF TxChksumEnable IN opts  THEN
+				SYSTEM.GET(iobase + Dmacr, reg);
+				EXCL(reg, DmacrTcpChksum);
+				SYSTEM.PUT(iobase + Dmacr, reg);
+				IF Trace THEN KernelLog.String("DMACR_OFFSET reg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, reg), -8); KernelLog.Ln END;
+			END;
+
+			(* Disable transmitter *)
+			IF TransmitterEnable IN opts  THEN
+				SYSTEM.GET(iobase + Nwctrl, reg);
+				reg := reg * SET(-NwctrlTxEn);
+				SYSTEM.PUT(iobase + Nwctrl, reg);
+				IF Trace THEN KernelLog.String("NWCTRL_OFFSET reg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, reg), -8); KernelLog.Ln END;
+			END;
+
+			(* Disable receiver *)
+			IF ReceiverEnable IN opts  THEN
+				SYSTEM.GET(iobase + Nwctrl, reg);
+				reg := reg * SET(-NwctrlRxEn);
+				SYSTEM.PUT(iobase + Nwctrl, reg);
+				IF Trace THEN KernelLog.String("NWCTRL_OFFSET reg="); KernelLog.Hex(SYSTEM.VAL(LONGINT, reg), -8); KernelLog.Ln END;
+			END;
+
+			SELF.options := SELF.options * (-opts);
+		END ClearOptions;
+
+		PROCEDURE ClearHash;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac: Clear Hash"); KernelLog.Ln END;
+			ASSERT(state = IsReady);
+			SYSTEM.PUT32(iobase + HashL, 0);
+			SYSTEM.PUT32(iobase + HashH, 0)
+		END ClearHash;
+
+		PROCEDURE SetMdioDivisor (divisor: LONGINT);
+			VAR
+				reg: SET;
+		BEGIN
+			ASSERT(state = IsReady);
+			(* Only last 3 bits are valid *)
+			ASSERT(divisor < 8);
+
+			SYSTEM.GET(iobase + Nwcfg, reg);
+			reg := reg * SET(-NwcfgMdcClkDiv);
+			reg := reg + SYSTEM.VAL(SET, LSH(divisor, NwcfgMdcShift));
+			SYSTEM.PUT(iobase + Nwcfg, reg);
+		END SetMdioDivisor;
+
+		PROCEDURE SetOperatingSpeed (speed: LONGINT);
+		VAR
+			reg: SET;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac: setting operating speed."); KernelLog.Ln END;
+			ASSERT(state = IsReady);
+			ASSERT((speed = 10) OR (speed = 100) OR (speed = 1000));
+
+			SYSTEM.GET(iobase + Nwcfg, reg);
+
+			reg := reg - {Nwcfg1000, Nwcfg100};
+			CASE speed OF
+				 10:
+				|100:
+					INCL(reg, Nwcfg100)
+				|1000:
+					INCL(reg, Nwcfg1000)
+			END;
+
+			SYSTEM.PUT(iobase + Nwcfg, reg);
+
+			IF Trace THEN KernelLog.String("Waiting for operation speed to stabilize"); KernelLog.Ln END;
+			timer.Sleep(1000);
+			IF Trace THEN KernelLog.String("XEmac: operating speed set."); KernelLog.Ln END;
+		END SetOperatingSpeed;
+
+		(*! TODO: change parameter interface to a more convenient one *)
+		PROCEDURE SetMacAddress(index: LONGINT; mac: Network.LinkAdr);
+		VAR
+			reg: SET;
+		BEGIN
+			ASSERT(index > 0);
+			DEC(index);
+			reg := SYSTEM.VAL(SET, ORD(mac[0]) + LSH(ORD(mac[1]), 8) + LSH(ORD(mac[2]), 16) + LSH(ORD(mac[3]), 24));
+			SYSTEM.PUT(iobase + Laddr1l + (index * 8), reg);
+
+			(* There are reserved bits in TOP so don't affect them *)
+			SYSTEM.GET(iobase + Laddr1h + (index * 8), reg);
+			reg := reg * SET(-LaddrMach) + SYSTEM.VAL(SET, ORD(mac[4]) + LSH(ORD(mac[5]), 8));
+			SYSTEM.PUT(iobase + Laddr1h + index * 8, reg);
+
+			dev.local := mac;
+			IF Trace THEN
+				KernelLog.String("XEmac MAC address set to: ");
+				KernelLog.Hex(ORD(mac[0]), -2); KernelLog.String(".");
+				KernelLog.Hex(ORD(mac[1]), -2); KernelLog.String(".");
+				KernelLog.Hex(ORD(mac[2]), -2); KernelLog.String(".");
+				KernelLog.Hex(ORD(mac[3]), -2); KernelLog.String(".");
+				KernelLog.Hex(ORD(mac[4]), -2); KernelLog.String(".");
+				KernelLog.Hex(ORD(mac[5]), -2); KernelLog.Ln
+			END
+		END SetMacAddress;
+
+		PROCEDURE SetTypeIdCheck(index, idCheck: LONGINT): LONGINT;
+		BEGIN
+			ASSERT(state = IsReady);
+			ASSERT((index > 0) & (index <= MaxTypeId));
+			IF isStarted = IsStarted THEN
+				RETURN IsStarted
+			END;
+
+			SYSTEM.PUT(iobase + Match1 + index * 4, idCheck);
+			RETURN Success
+		END SetTypeIdCheck;
+
+		PROCEDURE InitDMA;
+		CONST
+			MB = 1024*1024;
+		VAR
+			beginAdr, endAdr, adr: ADDRESS;
+			reg: SET;
+			i: LONGINT;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac: Starting DMA"); KernelLog.Ln END;
+			
+			(*
+			NEW(buffer, (RxBdCount + TxBdCount + 20) * BdSize);
+			*)
+
+			(* allocate 2 pages (each page is 1 MBytes) of memory in Heaps for mapping one full page uncached 
+				without affecting any adjacent data *)
+			NEW(buffer,2*MB);
+			ASSERT((RxBdCount + TxBdCount + 20) * BdSize <= MB);
+			beginAdr := ADDRESSOF(buffer[0]);
+			beginAdr := beginAdr + (MB - beginAdr MOD MB);
+			ASSERT(beginAdr MOD MB = 0);
+			Machine.DisableDCacheRange(beginAdr,MB);
+			
+			(* setup DMA RX buffer descriptors *)
+			rxBdStartAdr := beginAdr;
+			endAdr := beginAdr + (RxBdCount - 1) * BdSize;
+			rxBdEndAdr := endAdr;
+			i := 0;
+			adr := beginAdr;
+			WHILE adr < endAdr DO
+				SYSTEM.PUT32(adr + BdAdrOffset, ADDRESSOF(rxFrames[i][0]));
+				INC(adr, BdSize);
+				INC(i)
+			END;
+
+			SYSTEM.PUT32(adr + BdAdrOffset, ADDRESSOF(rxFrames[i][0]) + 2);
+			lastRxQBar := beginAdr;
+			rxBdBase := beginAdr;
+			CODE
+				DSB
+			END;
+			
+			(* no need to flush/invalidate cache for BD memory space
+			(* Flush DCache *)
+			Machine.FlushDCacheRange(beginAdr, RxBdCount * BdSize);
+			*)
+			
+			IF Trace THEN KernelLog.String("XEmac: RX buffers are set up."); KernelLog.Ln END;
+
+			(* setup DMA TX buffer descriptors *)
+			beginAdr := endAdr + BdSize;
+			txBdStartAdr := beginAdr;
+			endAdr := beginAdr + (TxBdCount - 1) * BdSize;
+			txBdEndAdr := endAdr;
+			adr := beginAdr;
+			WHILE adr < endAdr DO
+				SYSTEM.PUT32(adr, 0);
+				SYSTEM.PUT32(adr + 4, {TxBufUsed});
+				INC(adr, BdSize)
+			END;
+			SYSTEM.PUT32(adr + 4, {TxBufUsed, TxBufWrap});
+			lastTxQBar := beginAdr;
+			txBdBase := beginAdr;
+			CODE
+				DSB
+			END;
+			
+			(* no need to flush/invalidate cache for BD memory space
+			(* Flush DCache *)
+			Machine.FlushDCacheRange(txBdBase, TxBdCount * BdSize);
+			*)
+			
+			IF Trace THEN KernelLog.String("XEmac: TX buffers are set up."); KernelLog.Ln END;
+
+			Objects.InstallHandler(SELF.InterruptHandler, IRQ);
+			IF Trace THEN KernelLog.String("DMA Initialized"); KernelLog.Ln END
+		END InitDMA;
+
+		(**
+			Interrupt handler -- tries to handle the maximum number of IRQs
+			at once.
+		*)
+		PROCEDURE InterruptHandler;
+		VAR
+			regISR, regSR: SET;
+		BEGIN
+			ASSERT(state = IsReady);
+
+			(* Get and clear interrupts *)
+			SYSTEM.GET(iobase + Isr, regISR);
+			SYSTEM.PUT(iobase + Isr, regISR);
+
+			(* Transmit complete interrupt *)
+			IF IxrTxCompl IN regISR THEN
+				SYSTEM.PUT(iobase + Txsr, {TxsrTxCompl, TxsrUsedRead});
+				SendHandler
+			END;
+
+			(* Receive complete interrupt *)
+			IF IxrFrameRx IN regISR THEN
+				SYSTEM.PUT(iobase + Rxsr, {RxsrFrameRx, RxsrBuffna});
+				ReceiveHandler
+			END;
+
+			(* Receive error conditions interrupt *)
+			IF IxrRxErr * regISR # {} THEN
+				(* Clear Rx status register *)
+				SYSTEM.GET(iobase + Rxsr, regSR);
+				SYSTEM.PUT(iobase + Rxsr, regSR);
+				IF IxrRxUsed IN regISR THEN
+					(* Flush a packet from Rx SRAM *)
+					SYSTEM.GET(iobase + Nwctrl, regSR);
+					SYSTEM.PUT(iobase + Nwctrl, regSR + {18})
+				END;
+				ErrorHandler(Recv, regSR)
+			END;
+
+			(* Transmit error interrupt
+				TxCompl also activates TxUsed when asserted: we have to check for a real error.
+			*)
+			IF (IxrTxErr * regISR # {}) & (~(IxrTxCompl IN regISR)) THEN
+				SYSTEM.GET(iobase + Txsr, regSR);
+				SYSTEM.PUT(iobase + Txsr, regSR);
+				ErrorHandler(Send, regSR)
+			END;
+		END InterruptHandler;
+
+		PROCEDURE SendHandler;
+		VAR
+			reg: SET;
+			currBdPtr: ADDRESS;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac.SendHandler"); KernelLog.Ln END;
+			SYSTEM.GET(iobase + Txsr, reg);
+			SYSTEM.PUT(iobase + Txsr, reg);
+
+			LOOP
+				currBdPtr := lastTxQBar;
+				IF currBdPtr = SYSTEM.GET32(iobase + Txqbase) THEN EXIT END;
+				SYSTEM.PUT32(currBdPtr + BdAdrOffset, 0);
+				IF currBdPtr = txBdEndAdr THEN
+					SYSTEM.PUT(currBdPtr + BdStatOffset, {TxBufUsed, TxBufWrap});
+					lastTxQBar := txBdStartAdr
+				ELSE
+					SYSTEM.PUT(currBdPtr + BdStatOffset, {TxBufUsed});
+					INC(lastTxQBar, BdSize)
+				END
+			END;
+			IF Trace THEN KernelLog.String("XEmac.SendHandler END"); KernelLog.Ln END
+		END SendHandler;
+
+		PROCEDURE ReceiveHandler;
+		(*VAR
+			buf: Network.Buffer;
+			type: LONGINT;*)
+		VAR
+			reg: SET;
+			tmp: LONGINT;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac.ReceiveHandler"); KernelLog.Ln END;
+			SYSTEM.GET(iobase + Rxsr, reg);
+			SYSTEM.PUT(iobase + Rxsr, reg);
+
+			SYSTEM.GET(iobase + Rxcnt, tmp);
+			IF tmp = 0 THEN
+				SYSTEM.GET(iobase + Nwctrl, reg);
+				reg := reg * SET(-NwctrlRxEn);
+				SYSTEM.PUT(iobase + Nwctrl, reg);
+				SYSTEM.GET(iobase + Nwctrl, reg);
+				reg := reg  + NwctrlRxEn;
+				SYSTEM.PUT(iobase + Nwctrl, reg)
+			END
+		END ReceiveHandler;
+
+		PROCEDURE ErrorHandler (direction: CHAR; errorWord: SET);
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac.ErrorHandler: Error while ") END;
+			CASE direction OF
+				 Send:
+				 	KernelLog.String("sending"); KernelLog.Ln;
+				 	IF TxsrHrespnok IN errorWord THEN
+				 		KernelLog.String("[XEmac ERROR] Transmit DMA error"); KernelLog.Ln
+				 	END;
+				 	IF TxsrUrun IN errorWord THEN
+				 		KernelLog.String("[XEmac ERROR] Transmit underrun"); KernelLog.Ln
+				 	END;
+				 	IF TxsrBufExh IN errorWord THEN
+				 		KernelLog.String("[XEmac ERROR] Transmit buffer exhausted"); KernelLog.Ln
+				 	END;
+				 	IF TxsrRxOvr IN errorWord THEN
+				 		KernelLog.String("[XEmac ERROR] Transmit retry limit exceeded"); KernelLog.Ln
+				 	END;
+				 	IF TxsrFrameRx IN errorWord THEN
+				 		KernelLog.String("[XEmac ERROR] Transmit collision"); KernelLog.Ln
+				 	END;
+				 	IF TxsrUsedRead IN errorWord THEN
+				 		KernelLog.String("[XEmac ERROR] Transmit buffer not available"); KernelLog.Ln;
+				 		KernelLog.String("QBar = "); KernelLog.Address(SYSTEM.GET32(iobase + Txqbase)); KernelLog.Ln
+				 	END;
+				|Recv:
+				 	KernelLog.String("receiving"); KernelLog.Ln;
+					IF RxsrHrespnok IN errorWord THEN
+						KernelLog.String("[XEmac ERROR] Receive DMA error"); KernelLog.Ln
+					END;
+					IF RxsrRxOvr IN errorWord THEN
+						KernelLog.String("[XEmac ERROR] Receive overrun"); KernelLog.Ln
+					END;
+					IF RxsrBuffna IN errorWord THEN
+						KernelLog.String("[XEmac ERROR] Receive buffer not available"); KernelLog.Ln
+					END
+			END;
+			ResetDevice
+		END ErrorHandler;
+
+		PROCEDURE SendTestFrame;
+		VAR
+			txbd: ADDRESS;
+			intsEnabled: BOOLEAN;
+		BEGIN
+			txFrame[0]:=0FFX;txFrame[1]:=0FFX;txFrame[2]:=0FFX;txFrame[3]:=0FFX;txFrame[4]:=0FFX;txFrame[5]:=0FFX;
+			txFrame[6]:=000X;txFrame[7]:=00AX;txFrame[8]:=035X;txFrame[9]:=000X;txFrame[10]:=001X;txFrame[11]:=002X;
+			txFrame[12]:=008X;txFrame[13]:=006X;txFrame[14]:=000X;txFrame[15]:=001X;
+			txFrame[16]:=008X;txFrame[17]:=000X;txFrame[18]:=006X;txFrame[19]:=004X;txFrame[20]:=000X;txFrame[21]:=001X;txFrame[22]:=000X;txFrame[23]:=00AX;
+			txFrame[24]:=035X;txFrame[25]:=000X;txFrame[26]:=001X;txFrame[27]:=002X;txFrame[28]:=0C0X;txFrame[29]:=0A8X;txFrame[30]:=001X;txFrame[31]:=00AX;
+			txFrame[32]:=000X;txFrame[33]:=000X;txFrame[34]:=000X;txFrame[35]:=000X;txFrame[36]:=000X;txFrame[37]:=000X;txFrame[38]:=0C0X;txFrame[39]:=0A8X;
+			txFrame[40]:=001X;txFrame[41]:=00AX;txFrame[42]:=000X;txFrame[43]:=000X;txFrame[44]:=000X;txFrame[45]:=000X;txFrame[46]:=000X;txFrame[47]:=000X;
+			txFrame[48]:=001X;txFrame[49]:=00AX;txFrame[50]:=000X;txFrame[51]:=000X;txFrame[52]:=000X;txFrame[53]:=000X;txFrame[54]:=000X;txFrame[55]:=000X;
+			txFrame[56]:=001X;txFrame[57]:=00AX;txFrame[58]:=000X;txFrame[59]:=000X;
+
+			KernelLog.String("Sending Test Frame"); KernelLog.Ln;
+			KernelLog.Address(ADDRESSOF(txFrame)); KernelLog.Ln;
+			KernelLog.Buffer(txFrame, 0, 60);
+			(* Send it with DMA *)
+			intsEnabled := Machine.AreInterruptsEnabled();
+			Machine.DisableInterrupts;
+
+			SYSTEM.GET(iobase + Txqbase, txbd);
+			KernelLog.String("SendTestFrame: txbd = "); KernelLog.Address(txbd); KernelLog.Ln;
+			IF txbd = 0 THEN txbd := txBdStartAdr END;
+			KernelLog.String("SendTestFrame: txbd = "); KernelLog.Address(txbd); KernelLog.Ln;
+
+			Machine.FlushDCacheRange(ADDRESSOF(txFrame), 60);
+
+			SYSTEM.PUT(txbd + BdAdrOffset, ADDRESSOF(txFrame));
+			IF txbd # txBdEndAdr THEN
+				SYSTEM.PUT(txbd + BdStatOffset, {TxBufLast} + SYSTEM.VAL(SET, 60));
+			ELSE
+				SYSTEM.PUT(txbd + BdStatOffset, {TxBufLast, TxBufWrap} + SYSTEM.VAL(SET, 60));
+			END;
+			
+			(* no need to flush/invalidate cache for BD memory space
+			Machine.FlushDCacheRange(txbd, BdSize);
+			*)
+			
+			CODE
+				DSB
+			END;
+
+			(* Transmit *)
+			Transmit;
+
+			IF intsEnabled THEN Machine.EnableInterrupts END;
+			IF Trace THEN KernelLog.String("Frame sent"); KernelLog.Ln END
+		END SendTestFrame;
+
+		PROCEDURE DumpDBMem(start: ADDRESS; count: LONGINT);
+		VAR
+			i: LONGINT;
+		BEGIN
+			FOR i := 0 TO count DO
+				KernelLog.Address(start); KernelLog.String(":	");
+				KernelLog.Address(SYSTEM.GET32(start)); KernelLog.String(" - ");
+				INC(start, 4);
+				KernelLog.Address(SYSTEM.GET32(start)); KernelLog.Ln;
+				INC(start, 4)
+			END;
+		END DumpDBMem;
+
+	BEGIN {ACTIVE}
+		(* Receive is done by polling *)
+		(*BEGIN {EXCLUSIVE}
+			AWAIT(start)
+		END;*)
+		LOOP
+			(*BEGIN {EXCLUSIVE}
+				IF kill THEN EXIT END
+			END;*)
+			IF lastRxQBar # SYSTEM.GET32(iobase + Rxqbase) THEN
+				ReceiveFrame
+			END
+		END
+	END Controller;
+
+	(** Phy subsystem of the controller. *)
+	Phy = OBJECT
+	VAR
+		iobase, phybase: ADDRESS;
+		linkSpeed: LONGINT;
+		timer: Kernel.Timer;
+
+		PROCEDURE & Setup * (iobase: ADDRESS);
+		BEGIN
+			SELF.iobase := iobase;
+			NEW(timer);
+			IF LinkSpeedMode = LinkSpeedAutodetect THEN
+				linkSpeed := GetIEEEPhySpeed();
+				IF linkSpeed = 1000 THEN
+					IF Trace THEN KernelLog.String("Autonegociated speed: 1000 MB/s"); KernelLog.Ln END;
+					SetUpSLCRDivisors(1000)
+				ELSIF linkSpeed = 100 THEN
+					IF Trace THEN KernelLog.String("Autonegociated speed: 100 MB/s"); KernelLog.Ln END;
+					SetUpSLCRDivisors(100)
+				ELSIF linkSpeed = 10 THEN
+					IF Trace THEN KernelLog.String("Autonegociated speed: 10 MB/s"); KernelLog.Ln END;
+					SetUpSLCRDivisors(10)
+				ELSE
+					IF Trace THEN KernelLog.String("Defaulting autonegociated speed to 10 MB/s"); KernelLog.Ln END;
+					SetUpSLCRDivisors(10)
+				END;
+				RETURN
+			ELSIF LinkSpeedMode = LinkSpeed1000 THEN
+				linkSpeed := 1000;
+			ELSIF LinkSpeedMode = LinkSpeed100 THEN
+				linkSpeed := 100;
+			ELSE
+				linkSpeed := 10;
+			END;
+			SetUpSLCRDivisors(linkSpeed);
+			ConfigureIEEEPhySpeed(linkSpeed);
+			timer.Sleep(1000)
+		END Setup;
+
+		PROCEDURE Detect (): ADDRESS;
+		VAR
+			status, phyReg: LONGINT;
+			phyAdr: ADDRESS;
+		BEGIN
+			IF Trace THEN KernelLog.String("XEmac: Detecting PHY"); KernelLog.Ln END;
+			phyAdr := 31;
+			WHILE phyAdr > 0 DO
+				status := Read(phyAdr, PhyDetectReg, phyReg);
+				IF (phyReg # 0FFFFH) & (SYSTEM.VAL(SET, phyReg) * PhyDetect = PhyDetect) THEN
+					IF Trace THEN KernelLog.String("Found PHY at "); KernelLog.Address(phyAdr); KernelLog.Ln END;
+					RETURN phyAdr
+				END;
+				DEC(phyAdr)
+			END;
+			IF Trace THEN KernelLog.String("PHY not found. Assuming its address is 0"); KernelLog.Ln END;
+			RETURN 0
+		END Detect;
+
+		PROCEDURE GetIEEEPhySpeed (): LONGINT;
+		VAR
+			temp, control, res, status, capabilities: LONGINT;
+			phyAdr: ADDRESS;
+		BEGIN
+			phyAdr := Detect();
+			IF Trace THEN KernelLog.String("Start PHY autonegociation"); KernelLog.Ln END;
+			res := Write(phyAdr, IeeePageAddressRegister, 2);
+			res := Read(phyAdr, IeeeControlRegMac, control);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeRgmiiTxRxClockDelayed);
+			res := Write(phyAdr, IeeeControlRegMac, control);
+
+			res := Write(phyAdr, IeeePageAddressRegister, 0);
+
+			res := Read(phyAdr, IeeeAutonegoAdvertiseReg, control);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeAsymmetricPause + IeeePause + Advertise100 + Advertise10);
+			res := Write(phyAdr, IeeeAutonegoAdvertiseReg, control);
+
+			res := Read(phyAdr, Ieee1000AdvertiseReg, control);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + Advertise1000);
+			res := Write(phyAdr, Ieee1000AdvertiseReg, control);
+
+			res := Write(phyAdr, IeeePageAddressRegister, 0);
+			res := Read(phyAdr, IeeeCopperSpecificControlReg, control);
+			control := control + LSH(7, 12); (* Max number of gigabit attemps *)
+			INCL(SYSTEM.VAL(SET, control), 11); (* Enable dowshift *)
+			res := Write(phyAdr, IeeeCopperSpecificControlReg, control);
+
+			res := Read(phyAdr, IeeeControlReg, control);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeCtrlAutonegotiateEnable);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeStatAutonegotiateRestart);
+			res := Write(phyAdr, IeeeControlReg, control);
+
+			res := Read(phyAdr, IeeeControlReg, control);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeCtrlReset);
+			res := Write(phyAdr, IeeeControlReg, control);
+
+			REPEAT
+				IF Trace THEN KernelLog.String("Waiting for PHY reset"); KernelLog.Ln END;
+				res := Read(phyAdr, IeeeControlReg, control)
+			UNTIL SYSTEM.VAL(SET, control) * IeeeCtrlReset = {};
+
+			IF Trace THEN KernelLog.String("Waiting for PHY to complete autonegotiation"); KernelLog.Ln END;
+
+			res := Read(phyAdr, IeeeStatusReg, status);
+			WHILE SYSTEM.VAL(SET, status) * IeeeStatAutonegotiateComplete = {} DO
+				timer.Sleep(1000);
+				res := Read(phyAdr, IeeeCopperSpecificStatusReg2, temp);
+				IF Trace & (SYSTEM.VAL(SET, temp) * IeeeAutonegError # {}) THEN
+					KernelLog.String("Autonegotiation error");
+					KernelLog.Ln
+				END;
+				res := Read(phyAdr, IeeeStatusReg, status)
+			END;
+			IF Trace THEN KernelLog.String("Autonegotiation complete"); KernelLog.Ln END;
+
+			res := Read(phyAdr, IeeeSpecificStatusReg, capabilities);
+			IF 15 IN SYSTEM.VAL(SET, capabilities) THEN
+				RETURN 1000 (* MB/s *)
+			ELSIF 14 IN SYSTEM.VAL(SET, capabilities) THEN
+				RETURN 100 (* MB/s *)
+			ELSE
+				RETURN 10 (* MB/s *)
+			END
+		END GetIEEEPhySpeed;
+
+		PROCEDURE ConfigureIEEEPhySpeed (speed: LONGINT);
+		VAR
+			control, res, wait: LONGINT;
+			phyAdr: ADDRESS;
+		BEGIN
+			phyAdr := Detect();
+			res := Write(phyAdr, IeeePageAddressRegister, 2);
+			res := Read(phyAdr, IeeeControlRegMac, control);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeRgmiiTxRxClockDelayed);
+			res := Write(phyAdr, IeeeControlRegMac, control);
+
+			res := Write(phyAdr, IeeePageAddressRegister, 0);
+
+			res := Read(phyAdr, IeeeAutonegoAdvertiseReg, control);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeAsymmetricPause);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeePause);
+			res := Write(phyAdr, IeeeAutonegoAdvertiseReg, control);
+
+			res := Read(phyAdr, IeeeControlReg, control);
+			control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) * (-IeeeCtrlLinkSpeed1000M) * (-IeeeCtrlLinkSpeed100M) * (-IeeeCtrlLinkSpeed10M));
+
+			IF linkSpeed = 1000 THEN
+				control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeCtrlLinkSpeed1000M)
+			ELSIF linkSpeed = 100 THEN
+				control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeCtrlLinkSpeed100M)
+				(* Don't advertise PHY speed of 1000 MBPS *)
+			ELSIF linkSpeed = 10 THEN
+				control := SYSTEM.VAL(LONGINT, SYSTEM.VAL(SET, control) + IeeeCtrlLinkSpeed10M)
+			END;
+		END ConfigureIEEEPhySpeed;
+
+		PROCEDURE SetUpSLCRDivisors (div: LONGINT);
+		END SetUpSLCRDivisors;
+
+		PROCEDURE Write (address, register: ADDRESS; data: LONGINT): LONGINT;
+		VAR
+			reg, mgtcr, ipisr: SET;
+		BEGIN
+			IF Trace THEN KernelLog.String("PHY write"); KernelLog.Ln END;
+			SYSTEM.GET(iobase + Nwsr, reg);
+			(* Make sure no other PHY operation is currently in progress *)
+			IF ~(NwsrMdioIdle IN reg) THEN
+				IF Trace THEN KernelLog.String("XEmac Phy write aborted: PHY busy"); KernelLog.Ln END;
+				RETURN EmacMiiBusy
+			END;
+
+			(* Construct Mgtcr mask for the operation *)
+			mgtcr := PhyMntncOp + PhyMntncOpW + SYSTEM.VAL(SET, LSH(phybase, PhyMntncPhyAdShift))
+					+ SYSTEM.VAL(SET, LSH(address, PhyMntncPhyAdShift))
+					+ SYSTEM.VAL(SET, LSH(register, PhyMntncPhRegShift)) + SYSTEM.VAL(SET,data);
+			SYSTEM.PUT(iobase + PhyMntnc, mgtcr);
+
+			REPEAT
+				SYSTEM.GET(iobase + Nwsr, ipisr)
+			UNTIL NwsrMdioIdle IN ipisr;
+			RETURN Success
+		END Write;
+
+		PROCEDURE Read (address, register: ADDRESS; VAR data: LONGINT): LONGINT;
+		VAR
+			reg, ipisr, mgtcr: SET;
+		BEGIN
+			SYSTEM.GET(iobase + Nwsr, reg);
+			IF ~(NwsrMdioIdle IN reg) THEN
+				IF Trace THEN KernelLog.String("XEmac Phy read aborted: PHY busy"); KernelLog.Ln END;
+				RETURN EmacMiiBusy
+			END;
+
+			mgtcr := PhyMntncOp + PhyMntncOpR
+					+ SYSTEM.VAL(SET, LSH(address, PhyMntncPhyAdShift))
+					+ SYSTEM.VAL(SET, LSH(register, PhyMntncPhRegShift));
+			SYSTEM.PUT(iobase + PhyMntnc, mgtcr);
+
+			REPEAT
+				SYSTEM.GET(iobase + Nwsr, ipisr)
+			UNTIL NwsrMdioIdle IN ipisr;
+			data := SYSTEM.GET32(iobase + PhyMntnc) MOD 10000H;
+			RETURN Success
+		END Read;
+	END Phy;
+
+VAR
+	ld: LinkDevice;
+	res: LONGINT;
+BEGIN
+	NEW(ld, Network.TypeEthernet, 1500, 6); (*! What is the correct MTU? *)
+	ld.SetName("XEmac");
+	Network.registry.Add(ld, res);
+	NEW(ld.ctrl, ADDRESS(0E000B000H), ld);
+END XEmac.