better multiplexing support

git-svn-id: file:///srv/svn/repos/haiku/haiku/trunk@16016 a95241bf-73f2-0310-859d-f6bbb57e9c96

src/add-ons/kernel/bus_managers/ps2/ps2_common.c

@@ -48,7 +48,8 @@ sem_id gDeviceOpenSemaphore;
 
 static sem_id sKbcSem;
 static int32 sIgnoreInterrupts = 0;
-static bool sMultiplexingActive = false;
+
+bool gMultiplexingActive = false;
 
 
 inline uint8
@@ -68,6 +69,8 @@ ps2_read_data()
 inline void
 ps2_write_ctrl(uint8 ctrl)
 {
+	dprintf("ps2_write_ctrl 0x%02x\n", ctrl);
+
 	gIsa->write_io_8(PS2_PORT_CTRL, ctrl);
 }
 
@@ -75,6 +78,8 @@ ps2_write_ctrl(uint8 ctrl)
 inline void
 ps2_write_data(uint8 data)
 {
+	dprintf("ps2_write_data 0x%02x\n", data);
+
 	gIsa->write_io_8(PS2_PORT_DATA, data);
 }
 
@@ -162,23 +167,19 @@ ps2_command(uint8 cmd, const uint8 *out, int out_count, uint8 *in, int in_count)
 	return res;
 }
 
-
 status_t
 ps2_keyboard_command(uint8 cmd, const uint8 *out, int out_count, uint8 *in, int in_count)
 {
-
+       return ps2_dev_command(&ps2_device[PS2_DEVICE_KEYB], cmd, out, out_count, in, in_count);
-	return B_OK;
 }
 
 
 status_t
 ps2_mouse_command(uint8 cmd, const uint8 *out, int out_count, uint8 *in, int in_count)
 {
-
+       return ps2_dev_command(&ps2_device[PS2_DEVICE_MOUSE], cmd, out, out_count, in, in_count);
-	return B_OK;
 }  
 
-
 //	#pragma mark -
 
 inline status_t
@@ -205,8 +206,6 @@ ps2_interrupt(void* cookie)
 	uint8 data;
 	ps2_dev *dev;
 	
-	TRACE(("ps2_interrupt\n"));
-	
 	ctrl = ps2_read_ctrl();
 	if (!(ctrl & PS2_STATUS_OUTPUT_BUFFER_FULL))
 		return B_UNHANDLED_INTERRUPT;
@@ -221,7 +220,12 @@ ps2_interrupt(void* cookie)
 	TRACE(("ps2_interrupt: ctrl 0x%02x, data 0x%02x (%s)\n", ctrl, data, (ctrl & PS2_STATUS_MOUSE_DATA) ? "mouse" : "keyb"));
 
 	if (ctrl & PS2_STATUS_MOUSE_DATA) {
-		uint8 idx = 0;
+		uint8 idx;
+		if (gMultiplexingActive) {
+			idx = ctrl >> 6;
+		} else {
+			idx = 0;
+		}
 		dev = &ps2_device[PS2_DEVICE_MOUSE + idx];
 	} else {
 		dev = &ps2_device[PS2_DEVICE_KEYB];
@@ -264,7 +268,7 @@ ps2_init_driver(void)
 		goto err_4;
 
 	{
-		uint8 d;
+		uint8 d, in, out;
 		status_t res;
 		
 		res = ps2_get_command_byte(&d);
@@ -275,11 +279,56 @@ ps2_init_driver(void)
 		
 		res = ps2_set_command_byte(d);
 		dprintf("ps2_set_command_byte: res 0x%08x, d 0x%02x\n", res, d);
+		
+		in = 0x00;
+		out = 0xf0;
+		res = ps2_command(0xd3, &out, 1, &in, 1);
+		dprintf("step1: res 0x%08x, out 0x%02x, in 0x%02x\n", res, out, in);
+
+		in = 0x00;
+		out = 0x56;
+		res = ps2_command(0xd3, &out, 1, &in, 1);
+		dprintf("step2: res 0x%08x, out 0x%02x, in 0x%02x\n", res, out, in);
+
+		in = 0x00;
+		out = 0xa4;
+		res = ps2_command(0xd3, &out, 1, &in, 1);
+		dprintf("step3: res 0x%08x, out 0x%02x, in 0x%02x\n", res, out, in);
+		
+		if (res == B_OK && in != 0xa4) {
+			dprintf("found active multiplexing v%d.%d\n", (in >> 4), in & 0xf);
+			gMultiplexingActive = true;
+
+			res = ps2_command(0xa8, NULL, 0, NULL, 0);
+			dprintf("step4: res 0x%08x\n", res);
+
+			res = ps2_command(0x90, NULL, 0, NULL, 0);
+			dprintf("step5: res 0x%08x\n", res);
+			res = ps2_command(0xa8, NULL, 0, NULL, 0);
+			dprintf("step6: res 0x%08x\n", res);
+
+			res = ps2_command(0x91, NULL, 0, NULL, 0);
+			dprintf("step7: res 0x%08x\n", res);
+			res = ps2_command(0xa8, NULL, 0, NULL, 0);
+			dprintf("step8: res 0x%08x\n", res);
+
+			res = ps2_command(0x92, NULL, 0, NULL, 0);
+			dprintf("step9: res 0x%08x\n", res);
+			res = ps2_command(0xa8, NULL, 0, NULL, 0);
+			dprintf("step10: res 0x%08x\n", res);
+
+			res = ps2_command(0x93, NULL, 0, NULL, 0);
+			dprintf("step11: res 0x%08x\n", res);
+			res = ps2_command(0xa8, NULL, 0, NULL, 0);
+			dprintf("step12: res 0x%08x\n", res);
+			
+			
+		}
 	}
 
 	ps2_service_handle_device_added(&ps2_device[PS2_DEVICE_KEYB]);
 	ps2_service_handle_device_added(&ps2_device[PS2_DEVICE_MOUSE]);
-	if (sMultiplexingActive) {
+	if (gMultiplexingActive) {
 		ps2_service_handle_device_added(&ps2_device[PS2_DEVICE_MOUSE + 1]);
 		ps2_service_handle_device_added(&ps2_device[PS2_DEVICE_MOUSE + 2]);
 		ps2_service_handle_device_added(&ps2_device[PS2_DEVICE_MOUSE + 3]);

src/add-ons/kernel/bus_managers/ps2/ps2_common.h

@@ -35,6 +35,8 @@ extern sem_id gDeviceOpenSemaphore;
 extern device_hooks sKeyboardDeviceHooks;
 extern device_hooks sMouseDeviceHooks;
 
+extern bool gMultiplexingActive;
+
 // prototypes from common.c
 
 status_t ps2_init_driver(void);

src/add-ons/kernel/bus_managers/ps2/ps2_dev.c

@@ -16,10 +16,10 @@
 
 ps2_dev ps2_device[PS2_DEV_COUNT] =
 {
-	{ .name = "input/mouse/ps2/0",   .active = false, .result_sem = -1 },
+	{ .name = "input/mouse/ps2/0",   .active = false, .idx = 0, .result_sem = -1 },
-	{ .name = "input/mouse/ps2/1",   .active = false, .result_sem = -1 },
+	{ .name = "input/mouse/ps2/1",   .active = false, .idx = 1, .result_sem = -1 },
-	{ .name = "input/mouse/ps2/2",   .active = false, .result_sem = -1 },
+	{ .name = "input/mouse/ps2/2",   .active = false, .idx = 2, .result_sem = -1 },
-	{ .name = "input/mouse/ps2/3",   .active = false, .result_sem = -1 },
+	{ .name = "input/mouse/ps2/3",   .active = false, .idx = 3, .result_sem = -1 },
 	{ .name = "input/keyboard/at/0", .active = false, .result_sem = -1, .flags = PS2_FLAG_KEYB }
 };
 
@@ -115,3 +115,57 @@ ps2_dev_handle_int(ps2_dev *dev, uint8 data)
 		return mouse_handle_int(data);
 }
 
+
+status_t
+ps2_dev_command(ps2_dev *dev, uint8 cmd, const uint8 *out, int out_count, uint8 *in, int in_count)
+{
+	status_t res = B_OK;
+	int i, count;
+	
+	dprintf("ps2_dev_command %02x, %d out, in %d, dev %s\n", cmd, out_count, in_count, dev->name);
+	
+
+	dev->result_buf_cnt = in_count;
+	dev->result_buf_idx = 0;
+	dev->result_buf = in;
+
+	for (i = -1; res == B_OK && i < out_count; i++) {
+
+		if (!(dev->flags & PS2_FLAG_KEYB)) {
+			uint8 prefix_cmd;
+			if (gMultiplexingActive)
+				prefix_cmd = 0x90 + dev->idx;
+			else
+				prefix_cmd = 0xd4;
+			res = ps2_wait_write();
+			if (res == B_OK)
+				ps2_write_ctrl(prefix_cmd);
+		}
+
+		res = ps2_wait_write();
+		if (res == B_OK) {
+			ps2_write_data(i == -1 ? cmd : out[i]);
+		}
+	}
+	
+	if (res != B_OK) {
+		dprintf("ps2_dev_command send failed\n");
+	}
+
+	if (in_count != 0) {
+		dprintf("ps2_dev_command waiting for result\n");
+	
+		res = acquire_sem_etc(dev->result_sem, 1, B_RELATIVE_TIMEOUT, 4000000);
+
+		count = in_count - dev->result_buf_cnt;
+		dev->result_buf_cnt = 0;
+	
+		dprintf("ps2_dev_command res %08x, in %d\n", res, count);
+
+		for (i = 0; i < count; i++)
+			dprintf("ps2_dev_command data %02x\n", in[i]);
+	}
+
+	return res;
+}
+

src/add-ons/kernel/bus_managers/ps2/ps2_dev.h

@@ -17,6 +17,7 @@ typedef struct
 	const char *	name;
 	bool			active;
 	uint32			flags;
+	uint8           idx;
 	sem_id			result_sem;
 	uint8 *			result_buf;
 	int				result_buf_idx;
@@ -33,6 +34,9 @@ extern ps2_dev ps2_device[5];
 status_t	ps2_dev_init(void);
 void		ps2_dev_exit(void);
 
+status_t	ps2_dev_command(ps2_dev *dev, uint8 cmd, const uint8 *out, int out_count, uint8 *in, int in_count);
+
+
 void		ps2_dev_publish(ps2_dev *dev);
 void		ps2_dev_unpublish(ps2_dev *dev);
 

src/add-ons/kernel/bus_managers/ps2/ps2_mouse.c

@@ -282,6 +282,7 @@ probe_mouse(size_t *probed_packet_size)
 				&& ps2_set_sample_rate(80) == B_OK) {
 				// get device id, again
 				ps2_mouse_command(PS2_CMD_GET_DEVICE_ID, NULL, 0, &deviceId, 1);
+				TRACE(("probe_mouse(): device id: %2x\n", deviceId));		
 				break;
 			}
 		}

src/add-ons/kernel/bus_managers/ps2/ps2_service.c

@@ -69,7 +69,7 @@ ps2_service_probe_device(ps2_dev *dev)
 	status_t res;
 
 	TRACE(("ps2_service_probe_device %s\n", dev->name));
-
+/*
 	if (dev->flags & PS2_FLAG_KEYB) {
 
 		res = ps2_command(0xae, NULL, 0, NULL, 0);
@@ -86,7 +86,7 @@ ps2_service_probe_device(ps2_dev *dev)
 		res = ps2_command(0xa9, NULL, 0, &d, 1);
 		dprintf("AUX test: res 0x%08x, d 0x%02x\n", res, d);
 	}
-	
+*/	
 	return B_OK;
 }