/* macro to handle unaligned little endian data in firmware image */
#define GETLE32(p) ((p)[0] | (p)[1] << 8 | (p)[2] << 16 | (p)[3] << 24)

static int
iwi_load_firmware(struct iwi_softc *sc, const struct iwi_fw *fw)
{
	u_char *p, *end;
	uint32_t sentinel, ctl, src, dst, sum, len, mlen, tmp;
	int ntries, error;

	/* copy firmware image to DMA memory */
	memcpy(sc->fw_virtaddr, fw->data, fw->size);

	/* make sure the adapter will get up-to-date values */
	bus_dmamap_sync(sc->fw_dmat, sc->fw_map, BUS_DMASYNC_PREWRITE);

	/* tell the adapter where the command blocks are stored */
	MEM_WRITE_4(sc, 0x3000a0, 0x27000);

	/*
	 * Store command blocks into adapter's internal memory using register
	 * indirections. The adapter will read the firmware image through DMA
	 * using information stored in command blocks.
	 */
	src = sc->fw_physaddr;
	p = sc->fw_virtaddr;
	end = p + fw->size;
	CSR_WRITE_4(sc, IWI_CSR_AUTOINC_ADDR, 0x27000);

	while (p < end) {
		dst = GETLE32(p); p += 4; src += 4;
		len = GETLE32(p); p += 4; src += 4;
		p += len;

		while (len > 0) {
			mlen = min(len, IWI_CB_MAXDATALEN);

			ctl = IWI_CB_DEFAULT_CTL | mlen;
			sum = ctl ^ src ^ dst;

			/* write a command block */
			CSR_WRITE_4(sc, IWI_CSR_AUTOINC_DATA, ctl);
			CSR_WRITE_4(sc, IWI_CSR_AUTOINC_DATA, src);
			CSR_WRITE_4(sc, IWI_CSR_AUTOINC_DATA, dst);
			CSR_WRITE_4(sc, IWI_CSR_AUTOINC_DATA, sum);

			src += mlen;
			dst += mlen;
			len -= mlen;
		}
	}

	/* write a fictive final command block (sentinel) */
	sentinel = CSR_READ_4(sc, IWI_CSR_AUTOINC_ADDR);
	CSR_WRITE_4(sc, IWI_CSR_AUTOINC_DATA, 0);

	tmp = CSR_READ_4(sc, IWI_CSR_RST);
	tmp &= ~(IWI_RST_MASTER_DISABLED | IWI_RST_STOP_MASTER);
	CSR_WRITE_4(sc, IWI_CSR_RST, tmp);

	/* tell the adapter to start processing command blocks */
	MEM_WRITE_4(sc, 0x3000a4, 0x540100);

	/* wait until the adapter reaches the sentinel */
	for (ntries = 0; ntries < 400; ntries++) {
		if (MEM_READ_4(sc, 0x3000d0) >= sentinel)
			break;
		DELAY(100);
	}
	if (ntries == 400) {
		device_printf(sc->sc_dev,
		    "timeout processing command blocks for %s firmware\n",
		    fw->name);
		error = EIO;
		goto fail5;
	}

	/* we're done with command blocks processing */
	MEM_WRITE_4(sc, 0x3000a4, 0x540c00);

	/* allow interrupts so we know when the firmware is ready */
	CSR_WRITE_4(sc, IWI_CSR_INTR_MASK, IWI_INTR_MASK);

	/* tell the adapter to initialize the firmware */
	CSR_WRITE_4(sc, IWI_CSR_RST, 0);

	tmp = CSR_READ_4(sc, IWI_CSR_CTL);
	CSR_WRITE_4(sc, IWI_CSR_CTL, tmp | IWI_CTL_ALLOW_STANDBY);

	/* wait at most one second for firmware initialization to complete */
	if ((error = msleep(sc, &sc->sc_mtx, 0, "iwiinit", hz)) != 0) {
		device_printf(sc->sc_dev, "timeout waiting for %s firmware "
		    "initialization to complete\n", fw->name);
	}

fail5:
	return error;
}


int kh2200_Controller::_load_firmware(const struct firmware_part *fw, IOPhysicalAddress *dmaPhysical, IOVirtualAddress *dmaVirtual){
	IOLog(DRIVER_NAME ": _load_firmware()\n");
	uint32_t end;
	uint32_t p;
	uint32_t sentinel, ctl, src, dst, sum, len, mlen, tmp;
	int ntries, error;

	/* copy firmware image to DMA memory */
	IOLog(DRIVER_NAME ": - Physical Address: 0x%08x\n", dmaPhysical);
	IOLog(DRIVER_NAME ": -  Virtual Address: 0x%08x\n", dmaVirtual); 
	IOLog(DRIVER_NAME ": - Physical Address: 0x%08x\n", dmaPhysical + 1);
	IOLog(DRIVER_NAME ": -  Virtual Address: 0x%08x\n", dmaVirtual + 1); 
	memcpy(dmaVirtual, fw->data, fw->size);
	IOLog(DRIVER_NAME ": - Just performed memcpy, sleeping for 10 seconds...\n");
	IOSleep(10000);
	//return 0;

	/* make sure the adapter will get up-to-date values */
	//bus_dmamap_sync(sc->fw_dmat, sc->fw_map, BUS_DMASYNC_PREWRITE);

	/* tell the adapter where the command blocks are stored */
	MEM_WRITE_4(myBaseAddress2, 0x3000a0, 0x27000);

	/*
	 * Store command blocks into adapter's internal memory using register
	 * indirections. The adapter will read the firmware image through DMA
	 * using information stored in command blocks.
	 */
	src = (uint32_t)dmaPhysical;
	//p = (u_char *)dmaVirtual;
	p = (uint32_t)dmaVirtual;
		
	end = p + fw->size;
	CSR_WRITE_4(myBaseAddress2, IWI_CSR_AUTOINC_ADDR, 0x27000);

	IOLog(DRIVER_NAME ": - Going to loop from 0x%08x to 0x%08x\n", p, end);
	while (p < end) {
		//dst = GETLE32(p); p += 4; src += 4;
		//len = GETLE32(p); p += 4; src += 4;
		//dst = p; p += 4; src += 4;
		//len = p; p += 4; src += 4;
		dst = p; p += 1; src += 1; // +1 on a pointer moves it 4 bytes, which was what the BSD code was trying to do?
		len = p; p += 1; src += 1; // ^
		p += len;
		IOLog(DRIVER_NAME ": -- p=0x%08x  src=0x%08x  dst=0x%08x  len=0x%08x\n", p, src, dst, len);

		while (len > 0) {
			mlen = min(len, IWI_CB_MAXDATALEN);

			ctl = IWI_CB_DEFAULT_CTL | mlen;
			sum = ctl ^ src ^ dst;

			/* write a command block */
			CSR_WRITE_4(myBaseAddress2, IWI_CSR_AUTOINC_DATA, ctl);
			CSR_WRITE_4(myBaseAddress2, IWI_CSR_AUTOINC_DATA, src);
			CSR_WRITE_4(myBaseAddress2, IWI_CSR_AUTOINC_DATA, dst);
			CSR_WRITE_4(myBaseAddress2, IWI_CSR_AUTOINC_DATA, sum);

			src += mlen;
			dst += mlen;
			len -= mlen;
		}
	}

	/* write a fictive final command block (sentinel) */
	sentinel = CSR_READ_4(myBaseAddress2, IWI_CSR_AUTOINC_ADDR);
	IOLog(DRIVER_NAME ": - sentinel=0x%08x\n", sentinel);
	CSR_WRITE_4(myBaseAddress2, IWI_CSR_AUTOINC_DATA, 0);

	tmp = CSR_READ_4(myBaseAddress2, IWI_CSR_RST);
	tmp &= ~(IWI_RST_MASTER_DISABLED | IWI_RST_STOP_MASTER);
	CSR_WRITE_4(myBaseAddress2, IWI_CSR_RST, tmp);

	/* tell the adapter to start processing command blocks */
	MEM_WRITE_4(myBaseAddress2, 0x3000a4, 0x540100);

	/* wait until the adapter reaches the sentinel */
	//for(ntries = 0; ntries < 400; ntries++){
	for(ntries = 0; ntries < 50; ntries++){
		IOLog(DRIVER_NAME ": - checking sentinel: 0x%08x\n", MEM_READ_4(myBaseAddress2, 0x3000d0));
		if(MEM_READ_4(myBaseAddress2, 0x3000d0) >= sentinel)
			break;
		IOSleep(100);
	}
	if(ntries == 400){
		IOLog(DRIVER_NAME ":- FATAL Timeout during firmware loading\n");
		error = EIO;
		goto fail5;
	}

	/* we're done with command blocks processing */
	MEM_WRITE_4(myBaseAddress2, 0x3000a4, 0x540c00);

	/* allow interrupts so we know when the firmware is ready */
	CSR_WRITE_4(myBaseAddress2, IWI_CSR_INTR_MASK, IWI_INTR_MASK);

	/* tell the adapter to initialize the firmware */
	CSR_WRITE_4(myBaseAddress2, IWI_CSR_RST, 0);

	tmp = CSR_READ_4(myBaseAddress2, IWI_CSR_CTL);
	CSR_WRITE_4(myBaseAddress2, IWI_CSR_CTL, tmp | IWI_CTL_ALLOW_STANDBY);

	/* wait at most one second for firmware initialization to complete */
	/*if ((error = msleep(sc, &sc->sc_mtx, 0, "iwiinit", hz)) != 0) {
		device_printf(sc->sc_dev, "timeout waiting for %s firmware "
		    "initialization to complete\n", fw->name);
	}*/
	return 0;

fail5:
	//return error;
	IOLog(DRIVER_NAME ": - WARNING WARNING returning true!\n");
	return 0;
}

Jun 20 19:31:23 laptop kernel[0]: kh2200: start()
Jun 20 19:31:23 laptop kernel[0]: kh2200: createWorkLoop()
Jun 20 19:31:23 laptop kernel[0]: kh2200: createOutputQueue()
Jun 20 19:31:23 laptop kernel[0]: kh2200: initDriverObjects()
Jun 20 19:31:23 laptop kernel[0]: kh2200: - vendorID = 0x42208086 deviceID = 0x42208086 revisionID = 0x2800005
Jun 20 19:31:23 laptop kernel[0]: kh2200: - subSVID = 0x27218086 subSID = 0x27218086
Jun 20 19:31:23 laptop kernel[0]: kh2200: _uploadFirmware()
Jun 20 19:31:23 laptop kernel[0]: kh2200: - Successfully read firmware header
Jun 20 19:31:23 laptop kernel[0]: kh2200: -- Firmware version:    0x30000
Jun 20 19:31:23 laptop kernel[0]: kh2200: -- Boot size:           6464
Jun 20 19:31:23 laptop kernel[0]: kh2200: -- Ucode size:          16326
Jun 20 19:31:23 laptop kernel[0]: kh2200: -- Firmware size:       168336
Jun 20 19:31:23 laptop kernel[0]: kh2200: -- Firmware array size: 191142
Jun 20 19:31:23 laptop kernel[0]: kh2200: -- Firmware sum size:   191142
Jun 20 19:31:23 laptop kernel[0]: kh2200: dma_mem_alloc()
Jun 20 19:31:23 laptop kernel[0]: kh2200: - Attempting to load boot code
Jun 20 19:31:23 laptop kernel[0]: kh2200: _load_firmware()
Jun 20 19:31:23 laptop kernel[0]: kh2200: - Physical Address: 0x206e5000
Jun 20 19:31:23 laptop kernel[0]: kh2200: -  Virtual Address: 0x2e7ba000
Jun 20 19:31:23 laptop kernel[0]: kh2200: - Physical Address: 0x206e5004
Jun 20 19:31:23 laptop kernel[0]: kh2200: -  Virtual Address: 0x2e7ba004
Jun 20 19:31:23 laptop kernel[0]: kh2200: - Just performed memcpy, sleeping for 10 seconds...
Jun 20 19:31:33 laptop kernel[0]: kh2200: - Going to loop from 0x2e7ba000 to 0x2e7bb940
Jun 20 19:31:33 laptop kernel[0]: kh2200: -- p=0x5cf74003  src=0x206e5002  dst=0x2e7ba000  len=0x2e7ba001
Jun 20 19:31:33 laptop kernel[0]: kh2200: - sentinel=0x0019ae90
Jun 20 19:31:33 laptop kernel[0]: kh2200: - checking sentinel: 0x00027000
Jun 20 19:31:33 laptop kernel[0]: kh2200: - checking sentinel: 0x00027010
Jun 20 19:31:33 laptop kernel[0]: kh2200: - checking sentinel: 0x00027010
Jun 20 19:31:33 laptop kernel[0]: kh2200: - checking sentinel: 0x00027010
Jun 20 19:31:33 laptop kernel[0]: kh2200: - checking sentinel: 0x00027010
...