MODULE ZynqTools; (** AUTHOR "Timothée Martiel"; PURPOSE "Utility commands for Zynq platforms"; *)

IMPORT SYSTEM, Commands, Streams, Files, Options, Strings;

CONST
	HeaderSize = 8C0H;

	(* Defaults Settings for bootloader *)
	BoardAddress = "10.3.34.8";
	HostAddress = "10.3.34.145";
	Protocol = "TFTP";

	CommandPort = 59999;
	CommandTimeout = 10000;
	DefaultLoadAddress = 100000H;

	CR = 0DX;
	LF = 0AX;

	TCLModuleHeadComment0 = "(** TCL initialization module automatically generated by ZynqTools.ImportTCL for ";
	TCLModuleHeadComment1 = " *)";
	TCLModuleName = "TclInit";
	TCLMaskWrite = 0;
	TCLMaskPoll = 1;
	TCLMaskDelay = 3;
	TCLMwrForce = 4;

	TraceDeployCommands = TRUE;

TYPE
	TclRewriter = OBJECT
	VAR
		token: ARRAY 128 OF CHAR;
		reader: Streams.Reader;
		writer: Streams.Writer;

		PROCEDURE & Init (reader: Streams.Reader; writer: Streams.Writer);
		BEGIN
			SELF.reader := reader;
			SELF.writer := writer
		END Init;

		PROCEDURE Section;
		BEGIN
			ASSERT(token = 'proc');
			ASSERT(reader.GetString(token));
			writer.String("	");
			writer.String(token);
			writer.String(" * = [");

			REPEAT
				ASSERT(reader.GetString(token))
			UNTIL (token # '{') & (token # '}') & (token # '{}');

			LOOP
				IF (token = 'mask_write') OR (token = 'mask_poll') OR (token = 'mask_delay') OR (token = 'mwr') THEN
					IF token = 'mwr' THEN
						ASSERT(reader.GetString(token));
						ASSERT(token = '-force');
						token := "mwr_force";
					END;
					writer.Ln;
					writer.String("		")
				ELSIF (token[0] = '0') & (token[1] = 'X') THEN
					token[1] := 'x'
				END;
				writer.String(token);
				ASSERT(reader.GetString(token));
				IF token = '}' THEN EXIT END;
				writer.String(", ");
			END;

			(*ASSERT(reader.GetString(token));*)
			writer.Ln;
			writer.String("	];");
			writer.Ln
		END Section;

		PROCEDURE Rewrite;
		BEGIN
			WHILE reader.GetString(token) & (token = 'proc') DO Section END
		END Rewrite;
	END TclRewriter;

	(**
		Deploy a software/hardware system on a board using the ZynqBootloader.

		ZynqTools.Deploy [--protocol=<protocol> | -p=<protocol>] [--host=<ipaddress>] [--board=<ipaddress>|-b=<ipaddress>]
			[--software | -s] [--softwareLoadAddress=<address>]
			[--hardware | -h]
			[--bootloader]
			[<software code file>] [<hardware bitstream file>]
		~
		Constraints:
			o if --bootloader is present, --software and --hardware are prohibited
			o protocols: TFTP, UART (for manual control)
			o host: TFTP server computer
			o board: board ip address
	PROCEDURE Deploy * (context: Commands.Context);
	VAR
		command: ARRAY 256 OF CHAR;
		softwareFile, hardwareFile, boardAddress, hostAddress, protocol: ARRAY 128 OF CHAR;
		boardIpAdr: IP.Adr;
		options: Options.Options;
		socket: UDP.Socket;
		loadAddress: LONGINT;
		res: LONGINT;
		loadBootloader, loadSoftware, loadHardware: BOOLEAN;
	BEGIN
		NEW(options);
		options.Add('b', 'board', Options.String);
		options.Add('h', 'hardware', Options.Flag);
		options.Add('p', 'protocol', Options.String);
		options.Add('s', 'software', Options.Flag);
		options.Add(0X, 'bootloader', Options.Flag);
		options.Add(0X, 'host', Options.String);
		options.Add(0X, 'softwareLoadAddress', Options.Integer);

		IF ~options.Parse(context.arg, context.error) THEN context.result := Commands.CommandParseError; RETURN END;

		loadBootloader := options.GetFlag('bootloader');
		loadSoftware := options.GetFlag('software');
		loadHardware := options.GetFlag('hardware');
		IF loadSoftware THEN
			IF ~options.GetInteger('softwareLoadAddress', loadAddress) THEN
				loadAddress := DefaultLoadAddress
			END;
		END;
		IF loadBootloader & (loadSoftware OR loadHardware) THEN
			context.error.String("Cannot program hardware or software when rewriting bootloader");
			context.error.Ln; context.result := Commands.CommandError;
			RETURN
		END;
		IF loadBootloader OR loadSoftware THEN
			IF ~context.arg.GetString(softwareFile) THEN
				context.error.String("Expected software file image");
				context.error.Ln; context.result := Commands.CommandParseError;
				RETURN
			END
		END;
		IF loadHardware THEN
			IF ~context.arg.GetString(hardwareFile) THEN
				context.error.String("Expected hardware file image");
				context.error.Ln; context.result := Commands.CommandParseError;
				RETURN
			END
		END;

		IF ~options.GetString('board', boardAddress) THEN
			boardAddress := BoardAddress
		END;
		IF ~options.GetString('host', hostAddress) THEN
			hostAddress := HostAddress
		END;

		IF ~options.GetString('protocol', protocol) THEN
			protocol := Protocol
		END;
		ASSERT(protocol = "TFTP"); (*! For now only TFTP is supported *)


		(* Print Deployment report *)
		context.out.String('Deploying:'); context.out.Ln;
		IF loadSoftware THEN
			context.out.String("	-> ");
			context.out.String(softwareFile);
			context.out.String(" as software at ");
			context.out.Hex(loadAddress, -8);
			context.out.String("H");
			context.out.Ln
		END;
		IF loadHardware THEN
			context.out.String("	-> ");
			context.out.String(hardwareFile);
			context.out.String(" on FPGA");
			context.out.Ln
		END;
		IF loadBootloader THEN
			context.out.String("	-> ");
			context.out.String(softwareFile);
			context.out.String(" as bootloader");
			context.out.Ln
		END;
		context.out.String("File Transfer Protocol: "); context.out.String(protocol); context.out.Ln;
		IF protocol = 'TFTP' THEN context.out.String("	TFTP server at: "); context.out.String(hostAddress); context.out.Ln END;
		context.out.String("Board network address: "); context.out.String(boardAddress); context.out.Ln;
		context.out.Update;
		TFTPServer.Start;

		(* Send commands *)
		context.out.String("Deploying application...");
		context.out.Update;
		NEW(socket, CommandPort, res);
		IF res # UDP.Ok THEN
			context.error.String("Error opening UDP command socket: ");
			context.error.Int(res, 0);
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;
		boardIpAdr := IP.StrToAdr(boardAddress);
		ASSERT(~IP.IsNilAdr(boardIpAdr));

		(* Source command *)
		command := "setsource ";
		Strings.Append(command, protocol);
		Strings.Append(command, " ");
		Strings.Append(command, hostAddress);
		Strings.AppendChar(command, CR);
		Strings.AppendChar(command, LF);
		SendCommand(context.out, socket, boardIpAdr, CommandPort, command, res);
		IF res # UDP.Ok THEN
			context.error.String("Error sending command: ");
			context.error.Int(res, 0);
			context.error.Ln;
			context.error.Update;
			socket.Close;
			context.result := Commands.CommandError;
			RETURN
		END;

		(* Load bitstream & program fpga *)
		IF loadHardware THEN
			command := 'load ';
			Strings.Append(command, hardwareFile);
			Strings.Append(command, ' fpga');
			Strings.AppendChar(command, CR);
			Strings.AppendChar(command, LF);
			SendCommand(context.out, socket, boardIpAdr, CommandPort, command, res);
			IF res # UDP.Ok THEN
				context.error.String("Error sending command: ");
				context.error.Int(res, 0);
				context.error.Ln;
				context.error.Update;
				socket.Close;
				context.result := Commands.CommandError;
				RETURN
			END;
		END;

		(* Load ARM image to memory and start command *)
		IF loadSoftware THEN
			command := "load ";
			Strings.Append(command, softwareFile);
			Strings.Append(command, " memory ");
			Strings.AppendInt(command, loadAddress);
			Strings.AppendChar(command, CR);
			Strings.AppendChar(command, LF);
			SendCommand(context.out, socket, boardIpAdr, CommandPort, command ,res);
			IF res # UDP.Ok THEN
				context.error.String("Error sending command: ");
				context.error.Int(res, 0);
				context.error.Ln;
				context.error.Update;
				socket.Close;
				context.result := Commands.CommandError;
				RETURN
			END;

			command := "start ";
			Strings.AppendInt(command, loadAddress);
			Strings.AppendChar(command, CR);
			Strings.AppendChar(command, LF);
			SendCommand(context.out, socket, boardIpAdr, CommandPort, command ,res);
			IF res # UDP.Ok THEN
				context.error.String("Error sending command: ");
				context.error.Int(res, 0);
				context.error.Ln;
				context.error.Update;
				socket.Close;
				context.result := Commands.CommandError;
				RETURN
			END;
		END;

		context.out.String('done'); context.out.Ln;
		socket.Close;
		TFTPServer.Stop
	END Deploy;

	PROCEDURE SendCommand (out: Streams.Writer; socket: UDP.Socket; CONST adr: IP.Adr; port: LONGINT; CONST str: ARRAY OF CHAR; VAR res: LONGINT);
	VAR
		ack: ARRAY 64 OF CHAR;
		hostAdr: IP.Adr;
		len, hostPort: LONGINT;
	BEGIN
		IF TraceDeployCommands THEN
			out.String("Deploy Command: ");
			out.String(str);
			out.Update
		END;
		socket.Send(adr, port, str, 0, Strings.Length(str), res);
		socket.Receive(ack, 0, LEN(ack), CommandTimeout, hostAdr, hostPort, len, res);
		IF res # UDP.Ok THEN
			IF TraceDeployCommands THEN
				out.String(" -> unable to send command");
				out.Ln
			END;
			RETURN
		ELSIF ~IP.AdrsEqual(hostAdr, adr) THEN
			res := - 1
		ELSIF (ack[0] # 'O') OR (ack[1] # 'K') OR (ack[2] # ':') THEN
			out.String(ack);
			out.Ln;
			out.Update;
			IF TraceDeployCommands THEN
				out.String(" -> error in command execution");
				out.Ln
			END;
			res := -3
		ELSIF TraceDeployCommands THEN
			out.String(" -> OK");
			out.Ln
		END
	END SendCommand;*)

	(**
		Import a Xilinx TCL board initialization file as an Oberon module suitable for use in the Zynq Bootloader.

		This command simply rewrites first sections of a TCL file, which are expected to be:
			- ps7_pll_init_data_*
			- ps7_clock_init_data_*
			- ps7_ddr_init_data_*
			- ps7_mio_init_data_*
			- ps7_peripherals_init_data_*
		where the * is 1_0, 2_0 or 3_0.
		These sections are placed in a module named FsblInit as matharray constants. These constants are then used
		by the Initializer of the bootloader.

		Syntax:
			ZynqTools.ImportTCL [-d=dir|--directory=dir] [-m=moduleName|--module=moduleName] boardName file ~
		board: name of the board
		file: TCL file name
		-d|--directory gives the output directory.
		-m|--module gives the module name (default TclInit).
		The output file is named Board.Module.Mos
	*)
	PROCEDURE ImportTCL * (context: Commands.Context);
	VAR
		board, outputFile, inputFile, moduleName: ARRAY 128 OF CHAR;
		options: Options.Options;
		inFile, outFile: Files.File;
		input: Files.Reader;
		output: Files.Writer;
		rewriter: TclRewriter;
	BEGIN
		NEW(options);
		options.Add('d', 'directory', Options.String);
		options.Add('m', 'module', Options.String);

		IF ~options.Parse(context.arg, context.error) THEN context.result := Commands.CommandParseError; RETURN END;
		IF ~context.arg.GetString(board) THEN
			context.error.String("Expected board name");
			context.error.Ln;
			context.result := Commands.CommandParseError;
			RETURN
		END;
		IF ~context.arg.GetString(inputFile) THEN
			context.error.String("Expected input TCL file");
			context.error.Ln;
			context.result := Commands.CommandParseError;
			RETURN
		END;
		IF ~options.GetString('module', moduleName) THEN
			COPY(TCLModuleName, moduleName)
		END;
		IF options.GetString('directory', outputFile) THEN
			IF outputFile[Strings.Length(outputFile) - 1] # '/' THEN
				Strings.AppendChar(outputFile, '/')
			END
		ELSE
			outputFile[0] := 0X
		END;
		Strings.Append(outputFile, board);
		Strings.Append(outputFile, '.');
		Strings.Append(outputFile, moduleName);
		Strings.Append(outputFile, '.Mos');

		inFile := Files.Old(inputFile);
		IF inFile = NIL THEN
			context.error.String("Could not open file '");
			context.error.String(inputFile);
			context.error.String("'.");
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;
		Files.OpenReader(input, inFile, 0);
		IF input = NIL THEN
			context.error.String("Cannot read file '");
			context.error.String(inputFile);
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;

		outFile := Files.New(outputFile);
		IF outFile = NIL THEN
			context.error.String("Could not open file '");
			context.error.String(outputFile);
			context.error.String("'.");
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;
		Files.OpenWriter(output, outFile, 0);
		IF output = NIL THEN
			context.error.String("Cannot write to file '");
			context.error.String(outputFile);
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;
		Files.Register(outFile);

		NEW(rewriter, input, output);

		context.out.String("Importing TCL file '");
		context.out.String(inputFile);
		context.out.String("' (");
		context.out.String(board);
		context.out.String(") to '");
		context.out.String(outputFile);
		context.out.String("'.");
		context.out.Ln;
		context.out.Update;

		(* Generate module header *)
		output.String(TCLModuleHeadComment0);
		output.String(board);
		output.String(TCLModuleHeadComment1);
		output.Ln;
		output.String("MODULE ");
		output.String(moduleName);
		output.String(';');
		output.Ln;
		output.String('CONST');
		output.Ln;
		output.String("	mask_write * = ");
		output.Int(TCLMaskWrite, 0);
		output.String(";");
		output.Ln;
		output.String("	mask_poll * = ");
		output.Int(TCLMaskPoll, 0);
		output.String(';');
		output.Ln;
		output.String("	mask_delay * = ");
		output.Int(TCLMaskDelay, 0);
		output.String(';');
		output.Ln;
		output.String("	mwr_force * = ");
		output.Int(TCLMwrForce, 0);
		output.String(';');
		output.Ln;

		rewriter.Rewrite;

		(* Module Footer *)
		output.String("END ");
		output.String(TCLModuleName);
		output.String('.');
		output.Update;

		outFile.Close;
		inFile.Close
	END ImportTCL;

	(**
	 * Generate a Zynq boot ROM.
	 *
	 * Creates a boot header with the specified options and add a binary image after it.
	 *
	 * ZynqTools.GenerateBootROM {-o outfile | -b boardName | -e encryption | --key AESkey | --keyFromEfuse | --keyFromRam | --inPlace | --userWord} input ~
	 *)
	PROCEDURE GenerateBootROM * (context : Commands.Context);
	VAR
		header: ARRAY HeaderSize OF CHAR;
		buffer: ARRAY 1024 OF CHAR;
		input, output, board, key: ARRAY 128 OF CHAR;
		interrupts, registers: ARRAY 32 OF CHAR;
		opt: Options.Options;
		inFile, outFile: Files.File;
		in: Files.Reader;
		out: Files.Writer;
		userWord, read, size, wrote: LONGINT;
		encrypt, keyFromEFuse, keyFromRam, inPlace: BOOLEAN;
	BEGIN
		NEW(opt);
		opt.Add('o', 'outFile', Options.String);
		opt.Add('b', 'board', Options.String);
		opt.Add('e', 'encrypt', Options.Flag);
		opt.Add(0X, 'key', Options.String);
		opt.Add(0X, 'keyFromEfuse', Options.Flag);
		opt.Add(0X, 'keyFromRam', Options.Flag);
		opt.Add(0X, 'inPlace', Options.Flag);
		opt.Add(0X, 'userWord', Options.Integer);

		IF ~opt.Parse(context.arg, context.error) THEN context.result := Commands.CommandParseError; RETURN END;

		context.arg.String(input);
		IF ~opt.GetString("outFile", output) THEN
			output := "BOOT.BIN"
		END;
		IF opt.GetString('board', board) THEN
			board := "None"
		END;
		encrypt := opt.GetFlag('encrypt');
		IF opt.GetString('key', key) THEN
			IF encrypt THEN
				context.error.String("Cannot encrypt image without key");
				context.error.Ln;
				context.result := Commands.CommandError;
				RETURN
			END
		END;
		keyFromEFuse := opt.GetFlag('keyFromEfuse');
		keyFromRam := opt.GetFlag('keyFromRam');
		inPlace := opt.GetFlag('inPlace');
		IF ~opt.GetInteger('userWord', userWord) THEN
			userWord := 0
		END;

		IF encrypt & ~keyFromEFuse & ~keyFromRam THEN
			context.error.String("Cannot encrypt image without key source");
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;

		inFile := Files.Old(input);
		IF inFile = NIL THEN
			context.error.String("Cannot open input file '");
			context.error.String(input);
			context.error.String("'");
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;
		Files.OpenReader(in, inFile, 0);

		outFile := Files.New(output);
		IF outFile = NIL THEN
			context.error.String("Cannot open output file '");
			context.error.String(output);
			context.error.String("'");
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;
		Files.OpenWriter(out, outFile, 0);

		context.out.String("Creating Zynq BootROM image...");
		size := inFile.Length();
		IF size > 30000H THEN
			context.error.String("Image size is too big."); context.error.Ln;
			context.error.String("	Maximum size:	30000H"); context.error.Ln;
			context.error.String("	Actual size:			"); context.error.Hex(size, -5); context.error.String('H'); context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;

		FOR read := 0 TO 31 BY 4 DO
			interrupts[read] := 0FEX;
			interrupts[read + 1] := 0FFX;
			interrupts[read + 2] := 0FFX;
			interrupts[read + 3] := 0EAX;
		END;
		FOR read := 0 TO 31 BY 8 DO
			registers[read] := 0FFX;
			registers[read + 1] := 0FFX;
			registers[read + 2] := 0FFX;
			registers[read + 3] := 0FFX;

			registers[read + 4] := 0X;
			registers[read + 5] := 0X;
			registers[read + 6] := 0X;
			registers[read + 7] := 0X
		END;

		CreateBootHeader(userWord, size, 8C0H, 0, interrupts, registers, encrypt, keyFromEFuse, inPlace, header);
		out.Bytes(header, 0, HeaderSize);
		WHILE in.res = Streams.Ok DO
			in.Bytes(buffer, 0, 1024, read);
			out.Bytes(buffer, 0, read);
			INC(wrote, read)
		END;
		out.Update;
		IF wrote # size THEN
			context.error.String("Error: copied ");
			context.error.Int(wrote, 0);
			context.error.String("B from input file, but length is supposed to be ");
			context.error.Int(size, 0);
			context.error.String("B.");
			context.error.Ln;
			context.error.Update;
			context.result := Commands.CommandError;
			RETURN
		ELSE
			context.out.String("Copied ");
			context.out.Int(wrote, 0);
			context.out.String("/");
			context.out.Int(size, 0);
			context.out.String("B.");
			context.out.Ln
		END;
		context.out.String(" done");
		context.out.Ln;
		inFile.Close;
		Files.Register(outFile);
		outFile.Close;
	END GenerateBootROM;

	PROCEDURE ReplaceImageInBootROM * (context: Commands.Context);
	VAR
		block: ARRAY 1024 OF CHAR;
		imName, romName: ARRAY 128 OF CHAR;
		fIm, fRom, fOut: Files.File;
		im, rom: Files.Reader;
		out: Files.Writer;
		ofs, pos, read: LONGINT;
	BEGIN
		romName := "cfsbl/BOOT.BIN";
		imName := "Fsbl.Bin";

		context.out.String("Replacing image in '");
		context.out.String(romName);
		context.out.String("' by '");
		context.out.String(imName);
		context.out.String("'...");
		context.out.Ln;
		context.out.Update;

		fRom := Files.Old(romName);
		fIm := Files.Old(imName);
		fOut := Files.New("BOOT.BIN");

		IF fRom = NIL THEN
			context.error.String("Could not open ROM file");
			context.error.Ln;
			context.error.Update;
			context.result := Commands.CommandError;
			RETURN
		ELSIF fIm = NIL THEN
			context.error.String("Could not open image file");
			context.error.Ln;
			context.error.Update;
			context.result := Commands.CommandError;
			RETURN
		ELSIF fOut = NIL THEN
			context.error.String("Could not open output file");
			context.error.Ln;
			context.error.Update;
			context.result := Commands.CommandError;
			RETURN
		END;

		Files.OpenReader(rom, fRom, 0);
		Files.OpenReader(im, fIm, 0);
		Files.OpenWriter(out, fOut, 0);

		context.out.String("	Writing in 'BOOT.BIN'");
		context.out.Ln;
		context.out.Update;

		(*
			Read BootROM header:
				1. Copy pre source-offset fields
				2. Copy and remember source offset
				3. Copy until source offset
		 *)
		rom.Bytes(block, 0, 30H, read);
		ASSERT(read = 30H);
		out.Bytes(block, 0, read);

		rom.RawLInt(ofs);
		out.RawLInt(ofs);

		context.out.String("	Found image at location ");
		context.out.Hex(ofs, -8);
		context.out.Ln;
		context.out.Update;

		pos := 34H;
		WHILE pos < ofs DO
			rom.Bytes(block, 0, MIN(LEN(block), LONGINT(ofs) - pos), read);
			out.Bytes(block, 0, read);
			INC(pos, read);
		END;

		(* Copy image *)
		WHILE im.res = Streams.Ok DO
			im.Bytes(block, 0, LEN(block), read);
			out.Bytes(block, 0, read);
		END;

		Files.Register(fOut);
		fIm.Close;
		fRom.Close;
		fOut.Close;

		context.out.String(" done");
		context.out.Ln;
	END ReplaceImageInBootROM;

	(** Extract binary executable from BootROM image. Arguments: input output ~ *)
	PROCEDURE StripHeader*(context : Commands.Context);
	VAR
		block: ARRAY 1024 OF CHAR;
		romName, outName: ARRAY 128 OF CHAR;
		fRom, fOut: Files.File;
		rom: Files.Reader;
		out: Files.Writer;
		ofs, pos, read: LONGINT;
	BEGIN
		IF ~context.arg.GetString(romName) THEN
			context.error.String("Error: expected input and output files as parameters");
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;
		IF ~context.arg.GetString(outName) THEN
			context.error.String("Error: expected output file name as parameter");
			context.error.Ln;
			context.result := Commands.CommandError;
			RETURN
		END;

		context.out.String("Removing BootROM header from '");
		context.out.String(romName);
		context.out.String("'...");
		context.out.Ln;
		context.out.Update;

		fRom := Files.Old(romName);
		fOut := Files.New(outName);

		IF fRom = NIL THEN
			context.error.String("Could not open ROM file");
			context.error.Ln;
			context.error.Update;
			context.result := Commands.CommandError;
			RETURN
		ELSIF fOut = NIL THEN
			context.error.String("Could not open output file");
			context.error.Ln;
			context.error.Update;
			context.result := Commands.CommandError;
			RETURN
		END;

		Files.OpenReader(rom, fRom, 0);
		Files.OpenWriter(out, fOut, 0);

		context.out.String("	Writing in 'out.bin'");
		context.out.Ln;
		context.out.Update;

		(*
			Read BootROM header:
				1. Copy pre source-offset fields
				2. Copy and remember source offset
				3. Copy until source offset
		 *)
		rom.Bytes(block, 0, 30H, read);
		ASSERT(read = 30H);

		rom.RawLInt(ofs);

		context.out.String("	Found image at location ");
		context.out.Hex(ofs, -8);
		context.out.Ln;
		context.out.Update;

		pos := 34H;
		WHILE pos < ofs DO
			rom.Bytes(block, 0, MIN(LEN(block), LONGINT(ofs) - pos), read);
			INC(pos, read)
		END;

		(* Copy image *)
		WHILE rom.res = Streams.Ok DO
			rom.Bytes(block, 0, LEN(block), read);
			out.Bytes(block, 0, read);
		END;

		Files.Register(fOut);
		fRom.Close;
		fOut.Close;

		context.out.String(" done");
		context.out.Ln;
	END StripHeader;

	(** Create a bootROM header from the given parameters. *)
	PROCEDURE CreateBootHeader (userWord, imageSize, imageStartAdr, imageLoadAdr: LONGINT; CONST interrupts, registers: ARRAY OF CHAR;
			encrypt, keyFromEFuse, inPlace: BOOLEAN; VAR header: ARRAY OF CHAR);
	CONST
		InterruptSize		= 32;
		WidthDetection	= LONGINT(0AA995566H);
		ImageId			= LONGINT(0584C4E58H);
		KeyFromEFuse		= LONGINT(0A5C3C5A3H);
		KeyFromRam		= LONGINT(03A5C3C5AH);
		NoEncryption		= 0H;
		ChecksumStart		= 20H;
		ChecksumStop		= 44H;
		MaxRegisterLength	= 800H;
		RegisterEnd		= 8A0H;
	VAR
		chk: LONGINT;
		i, pos: LONGINT;
	BEGIN
		ASSERT(LEN(header) >= 8C0H);

		(* Interrupt Vector *)
		Strings.Move(interrupts, 0, InterruptSize, header, pos);
		INC(pos, InterruptSize);

		(* Width Detection *)
		MoveRawInt(WidthDetection, header, pos);
		INC(pos, 4);

		(* Image Identification *)
		MoveRawInt(ImageId, header, pos);
		INC(pos, 4);

		(* Encryption *)
		IF encrypt & keyFromEFuse THEN
			MoveRawInt(KeyFromEFuse, header, pos)
		ELSIF encrypt THEN
			MoveRawInt(KeyFromRam, header, pos)
		ELSE
			MoveRawInt(NoEncryption, header, pos)
		END;
		INC(pos, 4);

		(* User Defined word *)
		MoveRawInt(userWord, header, pos);
		INC(pos, 4);

		(* Source Offset *)
		ASSERT(imageStartAdr >= 8C0H);
		ASSERT(imageStartAdr MOD 64 = 0);
		MoveRawInt(imageStartAdr, header, pos);
		INC(pos, 4);

		(* Length of image *)
		ASSERT(imageSize <= 30000H);
		IF ~inPlace THEN
			MoveRawInt(imageSize, header, pos);
		ELSE
			MoveRawInt(0, header, pos);
		END;
		INC(pos, 4);

		(* FSBL Load Address *)
		MoveRawInt(0, header, pos);
		INC(pos, 4);

		(* Start of Execution *)
		IF inPlace THEN
			ASSERT(imageLoadAdr < 32 * 1024 * 1024);
		ELSIF encrypt THEN
			ASSERT(imageLoadAdr = 0);
		ELSE
			ASSERT(imageLoadAdr >= 0);
			ASSERT(imageLoadAdr < 30000H);
		END;
		MoveRawInt(imageLoadAdr, header, pos);
		INC(pos, 4);

		(* Total Image Length *)
		IF ~inPlace THEN
			MoveRawInt(imageSize, header, pos);
		ELSE
			MoveRawInt(0, header, pos);
		END;
		INC(pos, 4);

		(* QSPI Config Word *)
		MoveRawInt(0, header, pos);
		INC(pos, 4);

		(* Header Checksum *)
		FOR i := ChecksumStart TO ChecksumStop BY 4 DO
			chk := chk + ReadRawInt(header, i)
		END;
		MoveRawInt(Invert(chk), header, pos);
		INC(pos, 4);

		(* Unused *)
		ASSERT(pos = 4CH);
		WHILE pos < 0A0H DO
			MoveRawInt(0, header, pos);
			INC(pos, 4)
		END;

		(* Registers *)
		ASSERT(pos = 0A0H);
		ASSERT(LEN(registers) <= MaxRegisterLength);
		Strings.Move(registers, 0, LEN(registers), header, pos);
		INC(pos, LEN(registers));
		WHILE pos < RegisterEnd DO
			MoveRawInt(LONGINT(0FFFFFFFFH), header, pos);
			INC(pos, 4);
			MoveRawInt(LONGINT(0), header, pos);
			INC(pos, 4);
		END;

		(* Unused *)
		WHILE pos < HeaderSize DO
			MoveRawInt(LONGINT(0FFFFFFFFH), header, pos);
			INC(pos, 4)
		END
	END CreateBootHeader;

	(** Insert a raw integer in the array at location [ofs .. ofs + 4). *)
	PROCEDURE MoveRawInt (int: LONGINT; VAR dest: ARRAY OF CHAR; ofs: LONGINT);
	BEGIN
		dest[ofs] := CHR(int MOD 100H);
		dest[ofs + 1] := CHR(int DIV 100H MOD 100H);
		dest[ofs + 2] := CHR(int DIV 10000H MOD 100H);
		dest[ofs + 3] := CHR(int DIV 1000000H MOD 100H)
	END MoveRawInt;

	(** Get an integer from the array at positions [ofs .. ofs + 4). *)
	PROCEDURE ReadRawInt (CONST src: ARRAY OF CHAR; ofs: LONGINT): LONGINT;
	VAR
		adr: LONGINT;
	BEGIN
		adr := LONGINT(ORD(src[ofs])) + LONGINT(ORD(src[ofs + 1])) * 100H + LONGINT(ORD(src[ofs + 2])) * 10000H + LONGINT(ORD(src[ofs + 3])) * 1000000H;
		RETURN adr
	END ReadRawInt;

	(** Invert bits of a LONGINT *)
	PROCEDURE Invert (val: LONGINT): LONGINT;
	BEGIN
		RETURN SYSTEM.VAL(LONGINT, -SYSTEM.VAL(SET, ADDRESS(val)))
	END Invert;
END ZynqTools.

ZynqTools.GenerateBootROM Fsbl.Bin ~
ZynqTools.GenerateBootROM out.bin ~
FoxARMInstructionSet.Disassemble BOOT.BIN ~
FoxARMInstructionSet.Disassemble cfsbl/BOOT.BIN ~
FoxARMInstructionSet.Disassemble Fsbl.Bin -a=2ffc0H~
FoxARMInstructionSet.Disassemble C:/Users/tim/SDbackup/BOOT.BIN ~

ZynqTools.ReplaceImageInBootROM cfsbl/BOOT.BIN Fsbl.Bin ~
ZynqTools.StripHeader cfsbl/BOOT.BIN out.bin ~

ZynqTools.Deploy -s A2.Bin ~

ZynqTools.ImportTCL -d=ZBL:/ Zedboard boot/ps7_init.tcl ~
ZynqTools.ImportTCL -d=ZBL:/ Zybo Zybo-boot/ps7_init.tcl ~
ZynqTools.ImportTCL -d=basel/bootloader KRM KRM-boot/ps7_init.tcl ~