Merge e3f3c7cd09 into 8773bd95f0

Use framebuffer pitch instead of width where necessary

build/sync.sh

@@ -74,6 +74,12 @@ else
 	sudo modprobe nbd
 	[ ! -d $TMPMOUNT ] && mkdir $TMPMOUNT
 	case $1 in
+		flush)
+			mount_vdisk
+			sudo blockdev --flushbufs /dev/nbd0
+			sudo dosfsck -w -r -l -v -t /dev/nbd0
+			umount_vdisk
+			;;
 		diff)
 			mount_vdisk
 			diff -x *.MAP --color=always -r ../src/ $TMPMOUNT/ | less -R -p "diff -x.*|Only in.*"

src/Demo/Graphics/32BitColor.ZC

@@ -4,7 +4,7 @@
 
 I0 DrawPixel(I64 x, I64 y, CBGR24 color)
 {
-	text.fb_alias[x + y * GR_WIDTH] = color;
+	text.fb_alias[x + y * sys_framebuffer_width] = color;
 }
 
 I0 Color32Demo()

src/Demo/Graphics/32BitPaint.ZC

@@ -32,12 +32,12 @@ U0 PaintGlobalsInit()
 
 I0 DrawImagePixel(I64 x, I64 y, CBGR24 color)
 {
-	paint.fb[x + y * GR_WIDTH] = color;
+	paint.fb[x + y * sys_framebuffer_width] = color;
 }
 
 I0 DrawScreenPixel(I64 x, I64 y, CBGR24 color)
 {
-	text.fb_alias[x + y * GR_WIDTH] = color;
+	text.fb_alias[x + y * sys_framebuffer_width] = color;
 }
 
 I0 DrawScreenChar(I64 x, I64 y, I64 char)
@@ -136,7 +136,7 @@ U0 PaintLoadImage()
 
 U32 PaintImagePeek(I64 x, I64 y)
 {
-	return paint.fb[x + y * GR_WIDTH];
+	return paint.fb[x + y * sys_framebuffer_width];
 }
 
 I0 Paint32()
@@ -176,7 +176,7 @@ I0 Paint32()
 		// draw the image where the brush was last covering it
 		for (i = paint.last_x; i < paint.last_x + paint.last_thick; i++)
 			for (j = paint.last_y; j < paint.last_y + paint.last_thick; j++)
-				DrawScreenPixel(i, j, paint.fb[i + j * GR_WIDTH]);
+				DrawScreenPixel(i, j, paint.fb[i + j * sys_framebuffer_width]);
 
 		// draw the brush on the screen
 		DrawScreenBrush(mouse.pos.x, mouse.pos.y, paint.color, paint.thick);
@@ -193,7 +193,7 @@ I0 Paint32()
 		// draw the image where the color strings where covering it 
 		for (i = 0; i < 3 * FONT_HEIGHT; i++)
 			for (j = 0; j < 12 * FONT_WIDTH; j++)
-				DrawScreenPixel(j, i, paint.fb[j + i * GR_WIDTH]);
+				DrawScreenPixel(j, i, paint.fb[j + i * sys_framebuffer_width]);
 
 		// draw the color strings on screen
 		DrawScreenStr(0, 0 * FONT_HEIGHT, str_r);

src/Kernel/Display.ZC

@@ -60,17 +60,17 @@ See also $LK,"GrUpdateScreen",A="MN:GrUpdateScreen"$().
 		{//Scroll screen down
 
 			MemCopy(text.fb_alias,
-					text.fb_alias		+ sys_framebuffer_width * FONT_HEIGHT,
-					(text.screen_size	- sys_framebuffer_width * FONT_HEIGHT) * sizeof(U32));
+					text.fb_alias		+ sys_framebuffer_pidth * FONT_HEIGHT,
+					(text.screen_size	- sys_framebuffer_pidth * FONT_HEIGHT) * sizeof(U32));
 
-			MemSetU32(text.fb_alias + text.screen_size - sys_framebuffer_width * FONT_HEIGHT, BLACK32, sys_framebuffer_width * FONT_HEIGHT);
+			MemSetU32(text.fb_alias + text.screen_size - sys_framebuffer_pidth * FONT_HEIGHT, BLACK32, sys_framebuffer_pidth * FONT_HEIGHT);
 			text.raw_col -= text.cols ;
 			row = text.rows - 1;
 		}
 		x = col * FONT_WIDTH;
 		y = row * FONT_HEIGHT;
 		ch_bitmap = text.font[ch & 0xFF];
-		framebuffer = text.fb_alias + sys_framebuffer_width * y + x;
+		framebuffer = text.fb_alias + sys_framebuffer_pidth * y + x;
 
 		PUSHFD
 		CLI
@@ -81,7 +81,7 @@ See also $LK,"GrUpdateScreen",A="MN:GrUpdateScreen"$().
 			else
 				*framebuffer++ = BLACK32;
 			if (i & (FONT_WIDTH - 1) == FONT_WIDTH - 1)
-				framebuffer += sys_framebuffer_width - FONT_WIDTH;
+				framebuffer += sys_framebuffer_pidth - FONT_WIDTH;
 			ch_bitmap >>= 1;
 		}
 		POPFD

src/Kernel/KGlobals.ZC

@@ -14,7 +14,7 @@ U8		*rev_bits_table; //Table with U8 bits reversed
 CDate	 local_time_offset;
 F64		*pow10_I64,
 		 sys_os_version = 2.03;
-U64		 sys_os_version_sub = 118;
+U64		 sys_os_version_sub = 119;
 U8		*sys_os_version_str;
 U8		*sys_os_version_full;
 U8		*sys_os_version_nice;

src/Kernel/KMain.ZC

@@ -91,7 +91,8 @@ U0 SysGrInit()
 	text.cols = sys_framebuffer_width  / FONT_WIDTH;
 	text.rows = sys_framebuffer_height / FONT_HEIGHT;
 
-	text.screen_size	= sys_framebuffer_width * sys_framebuffer_height;
+	sys_framebuffer_pidth = sys_framebuffer_pitch / (sys_framebuffer_bpp / 8);
+	text.screen_size	= sys_framebuffer_pidth * sys_framebuffer_height;
 	text.buffer_size	= text.screen_size * 4; //buffer for 32-bit, but only 16 colors now.
 	text.raw_screen		= CAlloc(text.buffer_size);
 	text.fb_alias		= sys_framebuffer_addr;

src/Kernel/KUSB.ZC

@@ -0,0 +1,106 @@
+
+#define USBP_CMD 0x00
+#define USBP_STS 0x02
+#define USBP_INTR 0x04
+#define USBP_FRNUM 0x06
+#define USBP_FRBASEADD 0x08
+#define USBP_SOFMOD 0x0C
+#define USBP_PORTSC0 0x10
+#define USBP_PORTSC1 0x12
+#define USB_NUM 16
+
+//USB Pkt ID's
+#define PID_OUT 0xE1
+#define PID_IN 0x69
+#define PID_SOF 0xA5
+#define PID_SETUP 0x2D
+#define PID_DATA0 0xC3
+#define PID_DATA1 0x4B
+#define PID_DATA2 0x87
+#define PID_MDATA 0x0F
+#define PID_ACK 0xD2
+#define PID_NAK 0x5A
+#define PID_STALL 0x1E
+#define PID_NYET 0x96
+#define PID_PRE 0x3C
+#define PID_ERR 0x3C
+#define PID_SPLIT 0x78
+#define PID_PING 0xB4
+
+//USB Std Rqsts
+#define RQ_GET_STAT 0x0
+#define RQ_CLR_FEAT 0x1
+#define RQ_SET_FEAT 0x3
+#define RQ_SET_ADDR 0x5
+#define RQ_SET_DESC 0x7
+#define RQ_GET_CFG 0x8
+#define RQ_SET_CFG 0x9
+#define RQ_GET_INTERFACE 0xA
+#define RQ_SET_INTERFACE 0xB
+#define RQ_SYNC_FRAME 0xC
+
+I64 sys_num_usb = 0;
+CUSB sys_usb_devs[USB_NUM];
+MemSet(sys_usb_devs, 0, USB_NUM * sizeof(CUSB));
+
+U0 USBInitOne(I64 b, I64 d, I64 f)
+{
+	CUSB *u;
+	if (sys_num_usb < USB_NUM && PCIReadU16(b, d, f, 0) == 0x8086)
+	{
+		u = &sys_usb_devs[sys_num_usb++];
+		u->num = sys_num_usb;
+		u->bus = b;
+		u->dev = d;
+		u->fun = f;
+		u->ports = PCIReadU32(b, d, f, 0x20) & ~0x1F;
+	}
+}
+
+U0 USBEndAll()
+{
+	sys_num_usb = 0;
+}
+
+U0 USBInitAll()	//This is only valid for my ICH10 dev
+{
+	if (sys_num_usb)
+		USBEndAll;
+	USBInitOne(0, 29, 0);
+	USBInitOne(0, 29, 1);
+	USBInitOne(0, 29, 2);
+	USBInitOne(0, 29, 3);
+	USBInitOne(0, 26, 0);
+	USBInitOne(0, 26, 1);
+	USBInitOne(0, 26, 2);
+}
+
+CUSBTD *USBAllocTD()
+{
+//	return MAllocAligned(sizeof(CUSBTD), 0x10, dev.uncached_heap);
+	return MAllocAligned(sizeof(CUSBTD), 0x10, dev.uncached_alias + sys_task->code_heap);
+}
+
+U0 USBFreeTD(CUSBTD *tmptd)
+{
+	Free(tmptd);
+}
+
+U32 *USBAllocFrameLst(I64 usb_num, I64 size)
+{
+	//aligned to 0x1000
+	CUSB *u;
+	if (0 <= usb_num < sys_num_usb)
+	{
+		u = &sys_usb_devs[usb_num];
+		Free(u->frame_lst);
+//		u->frame_lst = MAllocAligned(size * sizeof(U32), 0x1000, dev.uncached_heap);
+		u->frame_lst = MAllocAligned(size * sizeof(U32), 0x1000, dev.uncached_alias + sys_task->code_heap);
+		OutU16(u->ports + USBP_CMD, 0);	//Stop
+		OutU16(u->ports + USBP_FRNUM, 0);
+		OutU32(u->ports + USBP_FRBASEADD, u->frame_lst);
+		return u->frame_lst;
+	}
+
+	return NULL;
+}

src/Kernel/Kernel.PRJ

@@ -52,6 +52,7 @@
 #include "Job"
 #include "PCIBIOS"
 #include "PCI"
+#include "KUSB" // currently too far forward in #includes, doesn't precede keyboard/mouse code
 #include "MultiProc"
 #include "EdLite"
 #include "BlkDev/MakeBlkDev"

src/Kernel/KernelA.HH

@@ -1272,7 +1272,7 @@ public class CDocBin
 #assert !($$ & 7)
 };
 
-#define DOC_SCROLL_SPEED	8
+#define DOC_SCROLL_SPEED	6
 
 #define DOCSS_NORMAL			0
 #define DOCSS_SINGLE_QUOTE		1
@@ -3021,6 +3021,62 @@ class CAtapiModeWriteList
 #define ATAR0_CMD 	7
 #define ATAR1_CTRL	2
 
+#help_index "Devices;USB"
+class CUSBTD	//Not implemented
+{
+	U32 td[4];
+};
+
+class CUSB	//Not implemented
+{
+	U8 num,	//USB dev num
+	bus, dev, fun;
+	U16 ports, pad;
+	U32 *frame_lst;
+};
+
+#define USBP_CMD 0x00
+#define USBP_STS 0x02
+#define USBP_INTR 0x04
+#define USBP_FRNUM 0x06
+#define USBP_FRBASEADD 0x08
+#define USBP_SOFMOD 0x0C
+#define USBP_PORTSC0 0x10
+#define USBP_PORTSC1 0x12
+#define USB_NUM 16
+
+//USB Pkt ID's
+#define PID_OUT 0xE1
+#define PID_IN 0x69
+#define PID_SOF 0xA5
+#define PID_SETUP 0x2D
+#define PID_DATA0 0xC3
+#define PID_DATA1 0x4B
+#define PID_DATA2 0x87
+#define PID_MDATA 0x0F
+#define PID_ACK 0xD2
+#define PID_NAK 0x5A
+#define PID_STALL 0x1E
+#define PID_NYET 0x96
+#define PID_PRE 0x3C
+#define PID_ERR 0x3C
+#define PID_SPLIT 0x78
+#define PID_PING 0xB4
+
+//USB Std Rqsts
+#define RQ_GET_STAT 0x0
+#define RQ_CLR_FEAT 0x1
+#define RQ_SET_FEAT 0x3
+#define RQ_SET_ADDR 0x5
+#define RQ_SET_DESC 0x7
+#define RQ_GET_CFG 0x8
+#define RQ_SET_CFG 0x9
+#define RQ_GET_INTERFACE 0xA
+#define RQ_SET_INTERFACE 0xB
+#define RQ_SYNC_FRAME 0xC
+
+
+
 #help_index "File/FileNames"
 #define FILEMASK_JIT	"*.ZC*;*.HH*"
 #define FILEMASK_AOT	"*.ZC*;*.HH*;*.PRJ*"

src/Kernel/KernelB.HH

@@ -249,7 +249,7 @@ public _extern SYS_FRAMEBUFFER_WIDTH	U64	sys_framebuffer_width;
 public _extern SYS_FRAMEBUFFER_HEIGHT	U64	sys_framebuffer_height;
 public _extern SYS_FRAMEBUFFER_PITCH	U64	sys_framebuffer_pitch;
 public _extern SYS_FRAMEBUFFER_BPP		U8	sys_framebuffer_bpp;
-
+public									U64	sys_framebuffer_pidth;
 _extern SYS_FRAMEBUFFER_LIST CVideoInfo sys_framebuffer_list[VBE_MODES_NUM];
 
 #help_index "Processor/SMBIOS"

src/System/Gr/GrDC.ZC

@@ -214,7 +214,8 @@ public CDC *DCNew(I64 width, I64 height, CTask *task=NULL, Bool null_bitmap=FALS
 	res->win_task		= task;
 	res->mem_task		= task;
 	res->width			= width;
-	res->width_internal	= (width + 7) & ~7;
+//	res->width_internal	= (width + 7) & ~7;
+	res->width_internal	= width & ~7;
 	res->height			= height;
 	if (null_bitmap)
 		res->flags |= DCF_DONT_DRAW;

src/System/Gr/GrGlobals.ZC

@@ -47,7 +47,7 @@ public CBGR24 gr_palette[COLORS_NUM];
 //See $LK,"SysGrInit",A="MN:SysGrInit"$()
 //Allows consts to be used instead of variables.
 HashPublic("GR_WIDTH", HTT_DEFINE_STR);;
-DefinePrint("GR_WIDTH", "%d", sys_framebuffer_width);
+DefinePrint("GR_WIDTH", "%d", sys_framebuffer_width & ~7);
 HashPublic("GR_HEIGHT", HTT_DEFINE_STR);;
 DefinePrint("GR_HEIGHT", "%d", sys_framebuffer_height);
 
@@ -56,3 +56,5 @@ DefinePrint("TEXT_ROWS", "%d", text.rows);;
 HashPublic("TEXT_ROWS", HTT_DEFINE_STR);;
 DefinePrint("TEXT_COLS", "%d", text.cols);;
 HashPublic("TEXT_COLS", HTT_DEFINE_STR);;
+
+sys_framebuffer_pidth = sys_framebuffer_pitch / (sys_framebuffer_bpp / 8);

src/System/Gr/GrScreen.ZC

@@ -142,8 +142,8 @@ public U0 GrScaleZoom(F64 scale)
 	mouse.offset.x = mouse.pos.x - (mouse.pos.x - mouse.offset.x) * s;
 	mouse.offset.y = mouse.pos.y - (mouse.pos.y - mouse.offset.y) * s;
 	mouse.offset.z = mouse.pos.z - (mouse.pos.z - mouse.offset.z) * s;
-	gr.sx = mouse.pos.x - gr.zoomed_dc->width  >> 1 / gr.screen_zoom;
-	gr.sy = mouse.pos.y - gr.zoomed_dc->height >> 1 / gr.screen_zoom;
+	gr.sx = mouse.pos.x - GR_WIDTH  >> 1 / gr.screen_zoom;
+	gr.sy = mouse.pos.y - GR_HEIGHT >> 1 / gr.screen_zoom;
 	GrFixZoomScale;
 }
 
@@ -154,7 +154,7 @@ U0 GrZoomInScreen()
 
 	GrFixZoomScale;
 
-	src = gr.dc2->body + gr.sx + gr.sy * gr.dc2->width_internal;
+	src = gr.dc2->body + gr.sx + gr.sy * GR_WIDTH;
 	dst = gr.zoomed_dc->body;
 
 	for (i = 0; i < GR_HEIGHT / gr.screen_zoom; i++)
@@ -359,42 +359,53 @@ U0 DCBlotColor8(CDC *dc, CDC *img)
 
 U0 GrCalcScreenUpdates()
 {
-	U16 *screen, *last_screen = gr.screen_cache;
-	U64 i, *src = text.raw_screen, *dst = text.fb_alias, diffs_size = GR_WIDTH * GR_HEIGHT / 2;
+	U8 *screen, reg RCX *last_screen = gr.screen_cache;
+	U32 *src = text.raw_screen, *dst = text.fb_alias;
+	U64 i, j, x, y, yi;
 
 	if (gr.screen_zoom == 1)
 		screen = gr.dc2->body;
 	else
 		screen = gr.zoomed_dc->body;
 
-	for (i = 0; i < diffs_size; i++)
+	for (y = yi = 0; y < GR_HEIGHT; yi = ++y * GR_WIDTH)
 	{
-		if (screen[i] != last_screen[i])
-			dst[i] = src[i];
+		for (x = 0; x < GR_WIDTH; x++)
+		{
+			i = x + yi;
+			j = x + y * sys_framebuffer_pidth;
+			if (screen[i] != last_screen[i])
+			{
+				dst = text.fb_alias + j;
+				src = text.raw_screen + j;
+				*dst = *src;
+			}
+		}
 	}
-	MemCopy(gr.screen_cache, screen, diffs_size * 2);
+	MemCopy(gr.screen_cache, screen, GR_WIDTH * GR_HEIGHT);
 }
 
 U0 GrUpdateScreen32()
 {
-	U64 size, *dst;
+	U64 x, y, j, i;
+	U32 *dst;
 	U8 *src;
 
 	if (gr.screen_zoom == 1)
-	{
 		src = gr.dc2->body;
-		size = src + gr.dc2->height * gr.dc2->width_internal;
-	}
 	else
 	{
 		GrZoomInScreen;
 		src = gr.zoomed_dc->body;
-		size = src + gr.zoomed_dc->height * gr.zoomed_dc->width_internal;
 	}
-	dst = text.raw_screen;
-	while (src < size) //draw 2 pixels at a time
-		*dst++ = gr_palette[*src++ & 0xFF] | gr_palette[*src++ & 0xFF] << 32;
-
+	for (y = j = i = 0; y < GR_HEIGHT; j = ++y * sys_framebuffer_pidth, i = y * GR_WIDTH)
+	{
+		for (x = 0; x < GR_WIDTH; x++)
+		{
+			dst = text.raw_screen + x + j;
+			*dst = gr_palette[src[x + i] & 0xFF];
+		}
+	}
 
 	GrCalcScreenUpdates;
 
@@ -420,6 +431,6 @@ U0 GrUpdateScreen()
 		(*gr.fp_final_screen_update)(dc);
 	DCDel(dc);
 
-	DCBlotColor4(gr.dc1->body, gr.dc2->body, gr.dc_cache->body, gr.dc2->height * gr.dc2->width_internal >> 3);
+	DCBlotColor4(gr.dc1->body, gr.dc2->body, gr.dc_cache->body, GR_HEIGHT * GR_WIDTH >> 3);
 	GrUpdateScreen32;
 }

src/System/USB.ZC

@@ -0,0 +1,207 @@
+
+#help_index "USB"
+
+extern I64 sys_num_usb;
+extern CUSB sys_usb_devs[USB_NUM];
+extern CUSBTD *USBAllocTD();
+extern U0 USBFreeTD(CUSBTD *tmptd);
+extern U0 USBEndAll();
+extern U0 USBInitAll();
+extern U32 *USBAllocFrameLst(I64 usb_num, I64 size);
+
+U8 *StatCB(CDoc *, CDocEntry *doc_e, CTask *mem_task)
+{
+	//This routine shows the stat of the USB ports
+	U8 *st = MAlloc(128, mem_task);
+	U16 w1, w2, w3, w4;
+	U32 d1;
+	CUSB *u = doc_e->user_data;
+	I64 d = u->ports;
+	w3 = InU16(d + USBP_STS);
+	w1 = InU16(d + USBP_PORTSC0);
+	w2 = InU16(d + USBP_PORTSC1);
+	w4 = InU16(d + USBP_FRNUM);
+	d1 = InU32(d + USBP_FRBASEADD);
+	StrPrint(st, "%X:Stat:%04X  P0:%04X  P1:%04X FRAME:%04X", d1, w3, w1, w2, w4);
+	return st;
+}
+
+U0 PutStat()
+{
+	I64 i;
+	CDocEntry *doc_e;
+	CUSB *u;
+	WinMax;
+
+	"$$FG,GREEN$$Dev Stat Bits\n$$FG$$"
+	"  0:IRQ\n"
+	"  1:IRQ err\n"
+	"  2:Resume\n"
+	"  3:Host Sys Err\n"
+	"  4:Host Process Err\n"
+	"  5:Halted\n"
+	"$$FG,GREEN$$Port Stat Bits\n$$FG$$"
+	"  0:Connection Stat\n"
+	"  1:Connection Stat Change\n"
+	"  2:Port Enabled\n"
+	"  3:Port Enabled Change\n"
+	"4-5:Line Stat\n"
+	"  6:Resume Detect\n"
+	"  8:Low Speed\n"
+	"  9:Port Rst\n"
+	" 10:Overcurrent Active\n"
+	" 11:Overcurrent Indicator\n"
+	" 12:Suspend\n\n";
+
+	for (i = 0; i < sys_num_usb; i++)
+	{
+		u = &sys_usb_devs[i];
+		"$$FG,LTRED$$Dev%d:%X:", i, u->ports;
+		doc_e = DocPrint(DocPut, "$$TX+TC,\" \"$$");
+		doc_e->user_data = &sys_usb_devs[i];
+		doc_e->tag_cb = &StatCB;
+		"$$FG$$\n";
+	}
+
+//	NewLine;
+	"\n";
+}
+
+extern U0 PutQH(U32 h);
+
+U32 *P(U32 d)
+{
+	return d & ~3;
+}
+
+U0 PutTD(U32 t)
+{
+	"TD:$$FG,GREEN$$%08X$$FG$$\n", t;
+	"%08X\n", *P(t);
+	DocDumpMem(t + 4, 8);
+	if (*P(t + 12))
+		DocDumpMem(*P(t + 12), 16);
+	//    DocDumpMem(*P(t+12), *P(t+4) >> 21 & 0x7FF);
+	if (!(*P(t) & 1))
+	{
+		if (*P(t) & 2)
+			PutQH(*P(t));
+		else
+			PutTD(*P(t));
+	}
+}
+
+U0 PutQH(U32 h)
+{
+	"QH:$$FG,RED$$%08X$$FG$$\n", h;
+	"%08X\n", *P(h);
+	"%08X\n", *P(h + 4);
+
+	if (!(*P(h) & 1))
+	{
+		if (*P(h) & 2)
+			PutQH(*P(h));
+		else
+			PutTD(*P(h));
+	}
+
+	if (!(*P(h + 4) & 1))
+	{
+		if (*P(h + 4) & 2)
+			PutQH(*P(h + 4));
+		else
+			PutTD(*P(h + 4));
+	}
+}
+
+U0 PutFrame(U32 f)
+{
+	PutQH(f);
+}
+
+U0 PutFrames()
+{
+	I64 i, d, f, w1, w2;
+	CUSB *u;
+	for (i = 0; i < sys_num_usb; i++)
+	{
+		u = &sys_usb_devs[i];
+		d = u->ports;
+		w1 = InU16(d + USBP_PORTSC0);
+		w2 = InU16(d + USBP_PORTSC1);
+		if (w1 & 1 || w2 & 1)
+		{
+			f = InU32(d + USBP_FRBASEADD);
+			PutFrame(f);
+		}
+	}
+}
+
+U0 Main()
+{
+	USBInitAll;
+	PutStat;
+	PutFrames;
+	"$$FG,RED$$The BIOS sets-up the USB in PS/2 legacy mode.$$FG$$\n";
+}
+
+Main;
+
+///* Not Finished
+#define PORT		5
+#define DEV_ADD_INIT	1
+#define DEV_ADD		1
+#define END_PT0		0
+#define END_PT1		1
+#define END_PT2		2
+#define LEN_MAX		8
+#define TERMINATE	1
+
+U0 SetUpTD()
+{
+  CUSB *u = &sys_usb_devs[PORT];
+  I64 i, d = u->ports;
+  U32 *frm = CAllocAligned(0x1000, 0x1000, Fs->code_heap),
+	*tds = CAllocAligned(256, 16, Fs->code_heap),
+	*buf = CAlloc(128, Fs->code_heap);
+  DocDump(buf, 128);
+
+  "<0>\n"; Sleep(100);
+  OutU16(d + USBP_CMD, 2);	//Reset
+  "<1>\n"; Sleep(100);
+  OutU16(d + USBP_CMD, 0);
+  "<2>\n"; Sleep(100);
+  OutU16(d + USBP_PORTSC0, 4);	//Enable
+  OutU16(d + USBP_PORTSC1, 4);
+  "<3>\n"; Sleep(100);
+
+  tds[0] = &tds[4](U8 *);
+  tds[1] = 0;
+  tds[2] = PID_SETUP + DEV_ADD_INIT << 8 + END_PT0 << 15 + LEN_MAX << 21;
+  tds[3] = buf;
+  buf[0] = 0 + RQ_SET_ADDR << 8 + DEV_ADD << 16;
+  buf[1] = 0 + 0 << 16;
+
+  tds[4] = TERMINATE;
+  tds[5] = 0;
+  tds[6] = PID_SETUP + DEV_ADD_INIT << 8 + END_PT0 << 15 + LEN_MAX << 21;
+  tds[7] = buf(U8 *) + 8;
+  buf[2] = 0 + RQ_SET_ADDR << 8 + DEV_ADD << 16;
+  buf[3] = 0 + 0 << 16;
+
+  frm[0] = &tds[0](U8 *);
+  for (i = 1; i < 0x1000 / 4; i++)
+    frm[i] = TERMINATE;
+
+  OutU16(d + USBP_FRNUM, frm);
+  OutU32(d + USBP_FRBASEADD, frm);
+  "<4>\n"; Sleep(200);
+  OutU16(d + USBP_CMD,1);
+  "<5>\n"; Sleep(1000);
+  OutU16(d + USBP_CMD,0);
+  "<6>\n"; Sleep(200);
+}
+
+SetUpTD;
+
+//*/