--- trunk/src/devices/dev_mp.c	2007/10/08 16:18:00	4
+++ trunk/src/devices/dev_mp.c	2007/10/08 16:19:56	24
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 2003-2005  Anders Gavare.  All rights reserved.
+ *  Copyright (C) 2003-2006  Anders Gavare.  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
  *  modification, are permitted provided that the following conditions are met:
@@ -25,7 +25,7 @@
  *  SUCH DAMAGE.
  *   
  *
- *  $Id: dev_mp.c,v 1.25 2005/02/25 06:27:48 debug Exp $
+ *  $Id: dev_mp.c,v 1.36 2006/05/04 19:49:25 debug Exp $
  *
  *  This is a fake multiprocessor (MP) device. It can be useful for
  *  theoretical experiments, but probably bares no resemblance to any
@@ -42,7 +42,8 @@
 #include "machine.h"
 #include "memory.h"
 #include "misc.h"
-#include "mp.h"
+
+#include "testmachine/dev_mp.h"
 
 
 struct mp_data {
@@ -50,20 +51,27 @@
 	uint64_t	startup_addr;
 	uint64_t	stack_addr;
 	uint64_t	pause_addr;
+
+	/*  Each CPU has an array of pending ipis.  */
+	int		*n_pending_ipis;
+	int		**ipi;
 };
 
 
+extern int single_step;
+
+
 /*
  *  dev_mp_access():
  */
-int dev_mp_access(struct cpu *cpu, struct memory *mem, uint64_t relative_addr,
-	unsigned char *data, size_t len, int writeflag, void *extra)
+DEVICE_ACCESS(mp)
 {
 	struct mp_data *d = extra;
 	int i, which_cpu;
 	uint64_t idata = 0, odata = 0;
 
-        idata = memory_readmax64(cpu, data, len);
+	if (writeflag == MEM_WRITE)
+	        idata = memory_readmax64(cpu, data, len);
 
 	/*
 	 *  NOTE: It is up to the user of this device to read or write
@@ -113,20 +121,20 @@
 		break;
 
 	case DEV_MP_PAUSE_CPU:
-		/*  Pause all cpus except our selves:  */
+		/*  Pause all cpus except a specific CPU:  */
 		which_cpu = idata;
 
 		for (i=0; i<cpu->machine->ncpus; i++)
-			if (i!=which_cpu)
+			if (i != which_cpu)
 				d->cpus[i]->running = 0;
 		break;
 
 	case DEV_MP_UNPAUSE_CPU:
-		/*  Unpause all cpus except our selves:  */
+		/*  Unpause a specific CPU:  */
 		which_cpu = idata;
-		for (i=0; i<cpu->machine->ncpus; i++)
-			if (i!=which_cpu)
-				d->cpus[i]->running = 1;
+
+		if (which_cpu >= 0 && which_cpu <cpu->machine->ncpus)
+			d->cpus[which_cpu]->running = 1;
 		break;
 
 	case DEV_MP_STARTUPSTACK:
@@ -136,7 +144,12 @@
 		break;
 
 	case DEV_MP_HARDWARE_RANDOM:
-		/*  Return (up to) 64 bits of "hardware random":  */
+		/*
+		 *  Return (up to) 64 bits of "hardware random":
+		 *
+		 *  NOTE: Remember that random() is (usually) 31 bits of
+		 *        random data, _NOT_ 32, hence this construction.
+		 */
 		odata = random();
 		odata = (odata << 31) ^ random();
 		odata = (odata << 31) ^ random();
@@ -153,6 +166,76 @@
 		odata = cpu->machine->physical_ram_in_mb * 1048576;
 		break;
 
+	case DEV_MP_IPI_ONE:
+	case DEV_MP_IPI_MANY:
+		/*
+		 *  idata should be of the form:
+		 *
+		 *		(IPI_nr << 16) | cpu_id
+		 *
+		 *  This will send an Inter-processor interrupt to a specific
+		 *  CPU. (DEV_MP_IPI_MANY sends to all _except_ the specific
+		 *  CPU.)
+		 *
+		 *  Sending an IPI means adding the IPI last in the list of
+		 *  pending IPIs, and asserting the IPI "pin".
+		 */
+		which_cpu = (idata & 0xffff);
+		for (i=0; i<cpu->machine->ncpus; i++) {
+			int send_it = 0;
+			if (relative_addr == DEV_MP_IPI_ONE && i == which_cpu)
+				send_it = 1;
+			if (relative_addr == DEV_MP_IPI_MANY && i != which_cpu)
+				send_it = 1;
+			if (send_it) {
+				d->n_pending_ipis[i] ++;
+				d->ipi[i] = realloc(d->ipi[i],
+				    d->n_pending_ipis[i] * sizeof(int));
+				if (d->ipi[i] == NULL) {
+					fprintf(stderr, "out of memory\n");
+					exit(1);
+				}
+				/*  Add the IPI last in the array:  */
+				d->ipi[i][d->n_pending_ipis[i] - 1] =
+				    idata >> 16;
+				cpu_interrupt(d->cpus[i], MIPS_IPI_INT);
+			}
+		}
+		break;
+
+	case DEV_MP_IPI_READ:
+		/*
+		 *  If the current CPU has any IPIs pending, accessing this
+		 *  address reads the IPI value. (Writing to this address
+		 *  discards _all_ pending IPIs.)  If there is no pending
+		 *  IPI, then 0 is returned. Usage of the value 0 for real
+		 *  IPIs should thus be avoided.
+		 */
+		if (writeflag == MEM_WRITE) {
+			d->n_pending_ipis[cpu->cpu_id] = 0;
+		}
+		odata = 0;
+		if (d->n_pending_ipis[cpu->cpu_id] > 0) {
+			odata = d->ipi[cpu->cpu_id][0];
+			if (d->n_pending_ipis[cpu->cpu_id]-- > 1)
+				memmove(&d->ipi[cpu->cpu_id][0],
+				    &d->ipi[cpu->cpu_id][1],
+				    d->n_pending_ipis[cpu->cpu_id]);
+		}
+		/*  Deassert the interrupt, if there are no pending IPIs:  */
+		if (d->n_pending_ipis[cpu->cpu_id] == 0)
+			cpu_interrupt_ack(d->cpus[cpu->cpu_id], MIPS_IPI_INT);
+		break;
+
+	case DEV_MP_NCYCLES:
+		/*
+		 *  Return approximately the number of cycles executed
+		 *  in this machine.  (This value is not updated for each
+		 *  instruction.)
+		 */
+		odata = cpu->machine->ncycles;
+		break;
+
 	default:
 		fatal("[ dev_mp: unimplemented relative addr 0x%x ]\n",
 		    relative_addr);
@@ -165,12 +248,11 @@
 }
 
 
-/*
- *  devinit_mp():
- */
-int devinit_mp(struct devinit *devinit)
+DEVINIT(mp)
 {
 	struct mp_data *d;
+	int n;
+
 	d = malloc(sizeof(struct mp_data));
 	if (d == NULL) {
 		fprintf(stderr, "out of memory\n");
@@ -181,9 +263,18 @@
 	d->startup_addr = INITIAL_PC;
 	d->stack_addr = INITIAL_STACK_POINTER;
 
-	memory_device_register(devinit->machine->memory,
-	    devinit->name, devinit->addr, DEV_MP_LENGTH,
-	    dev_mp_access, d, MEM_DEFAULT, NULL);
+	n = devinit->machine->ncpus;
+	d->n_pending_ipis = malloc(n * sizeof(int));
+	d->ipi = malloc(n * sizeof(int *));
+	if (d->ipi == NULL || d->n_pending_ipis == NULL) {
+		fprintf(stderr, "out of memory\n");
+		exit(1);
+	}
+	memset(d->n_pending_ipis, 0, sizeof(int) * n);
+	memset(d->ipi, 0, sizeof(int *) * n);
+
+	memory_device_register(devinit->machine->memory, devinit->name,
+	    devinit->addr, DEV_MP_LENGTH, dev_mp_access, d, DM_DEFAULT, NULL);
 
 	return 1;
 }