summaryrefslogtreecommitdiffstats
path: root/firmware/target
diff options
context:
space:
mode:
Diffstat (limited to 'firmware/target')
-rw-r--r--firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c10
-rw-r--r--firmware/target/arm/imx31/gigabeat-s/clkctl-imx31.c45
-rw-r--r--firmware/target/arm/imx31/gigabeat-s/clkctl-imx31.h86
-rw-r--r--firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c19
-rw-r--r--firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c173
-rw-r--r--firmware/target/arm/imx31/gigabeat-s/spi-imx31.c355
-rw-r--r--firmware/target/arm/imx31/gigabeat-s/spi-imx31.h56
-rw-r--r--firmware/target/arm/imx31/gigabeat-s/system-target.h3
8 files changed, 687 insertions, 60 deletions
diff --git a/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c b/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
index fc9abdb0e9..051b1c8479 100644
--- a/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
@@ -23,7 +23,7 @@
#include "backlight.h"
#include "lcd.h"
#include "power.h"
-#include "spi-imx31.h"
+#include "mc13783.h"
#include "debug.h"
bool _backlight_init(void)
@@ -33,14 +33,14 @@ bool _backlight_init(void)
void _backlight_on(void)
{
- // This relies on the SPI interface being initialised already
- spi_send(51, 1);
+ /* LEDEN=1 */
+ mc13783_set(MC13783_LED_CONTROL0, (1 << 0));
}
void _backlight_off(void)
{
- // This relies on the SPI interface being initialised already
- spi_send(51, 0);
+ /* LEDEN=0 */
+ mc13783_clear(MC13783_LED_CONTROL0, (1 << 0));
}
/* Assumes that the backlight has been initialized */
diff --git a/firmware/target/arm/imx31/gigabeat-s/clkctl-imx31.c b/firmware/target/arm/imx31/gigabeat-s/clkctl-imx31.c
new file mode 100644
index 0000000000..a01fab07d0
--- /dev/null
+++ b/firmware/target/arm/imx31/gigabeat-s/clkctl-imx31.c
@@ -0,0 +1,45 @@
+/***************************************************************************
+ * __________ __ ___.
+ * Open \______ \ ____ ____ | | _\_ |__ _______ ___
+ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
+ * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
+ * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
+ * \/ \/ \/ \/ \/
+ * $Id$
+ *
+ * Copyright (c) 2008 Michael Sevakis
+ *
+ * Clock control functions for IMX31 processor
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "system.h"
+#include "cpu.h"
+#include "clkctl-imx31.h"
+
+void imx31_clkctl_module_clock_gating(enum IMX31_CG_LIST cg,
+ enum IMX31_CG_MODES mode)
+{
+ volatile unsigned long *reg;
+ unsigned long mask;
+ int shift;
+ int oldlevel;
+
+ if (cg >= CG_NUM_CLOCKS)
+ return;
+
+ reg = &CLKCTL_CGR0 + cg / 16; /* Select CGR0, CGR1, CGR2 */
+ shift = 2*(cg % 16); /* Get field shift */
+ mask = CG_MASK << shift; /* Select field */
+
+ oldlevel = disable_interrupt_save(IRQ_FIQ_STATUS);
+
+ *reg = (*reg & ~mask) | ((mode << shift) & mask);
+
+ restore_interrupt(oldlevel);
+}
diff --git a/firmware/target/arm/imx31/gigabeat-s/clkctl-imx31.h b/firmware/target/arm/imx31/gigabeat-s/clkctl-imx31.h
new file mode 100644
index 0000000000..da15ef2706
--- /dev/null
+++ b/firmware/target/arm/imx31/gigabeat-s/clkctl-imx31.h
@@ -0,0 +1,86 @@
+/***************************************************************************
+ * __________ __ ___.
+ * Open \______ \ ____ ____ | | _\_ |__ _______ ___
+ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
+ * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
+ * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
+ * \/ \/ \/ \/ \/
+ * $Id$
+ *
+ * Copyright (c) 2008 Michael Sevakis
+ *
+ * Clock control functions for IMX31 processor
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef _CLKCTL_IMX31_H_
+#define _CLKCTL_IMX31_H_
+
+enum IMX31_CG_LIST
+{
+ /* CGR0 */
+ CG_SD_MMC1 = 0,
+ CG_SD_MMC2,
+ CG_GPT,
+ CG_EPIT1,
+ CG_EPIT2,
+ CG_IIM,
+ CG_ATA,
+ CG_SDMA,
+ CG_CSPI3,
+ CG_RNG,
+ CG_UART1,
+ CG_UART2,
+ CG_SSI1,
+ CG_I2C1,
+ CG_I2C2,
+ CG_I2C3,
+ /* CGR1 */
+ CG_HANTRO,
+ CG_MEMSTICK1,
+ CG_MEMSTICK2,
+ CG_CSI,
+ CG_RTC,
+ CG_WDOG,
+ CG_PWM,
+ CG_SIM,
+ CG_ECT,
+ CG_USBOTG,
+ CG_KPP,
+ CG_IPU,
+ CG_UART3,
+ CG_UART4,
+ CG_UART5,
+ CG_1_WIRE,
+ /* CGR2 */
+ CG_SSI2,
+ CG_CSPI1,
+ CG_CSPI2,
+ CG_GACC,
+ CG_EMI,
+ CG_RTIC,
+ CG_FIR,
+ CG_NUM_CLOCKS
+};
+
+enum IMX31_CG_MODES
+{
+ CGM_OFF = 0x0, /* Always off */
+ CGM_ON_RUN = 0x1, /* On in run mode, off in wait and doze */
+ CGM_ON_RUN_WAIT = 0x2, /* On in run and wait modes, off in doze */
+ CGM_ON_ALL = 0x3, /* Always on */
+};
+
+#define CG_MASK 0x3 /* bitmask */
+
+/* Enable or disable module clocks independently - module must _not_ be
+ * active! */
+void imx31_clkctl_module_clock_gating(enum IMX31_CG_LIST cg,
+ enum IMX31_CG_MODES mode);
+
+#endif /* _CLKCTL_IMX31_H_ */
diff --git a/firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c b/firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c
index aaa4bde0eb..f0f578b03e 100644
--- a/firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/kernel-imx31.c
@@ -19,6 +19,9 @@
#include "config.h"
#include "system.h"
#include "avic-imx31.h"
+#include "spi-imx31.h"
+#include "mc13783.h"
+#include "clkctl-imx31.h"
#include "kernel.h"
#include "thread.h"
@@ -42,10 +45,11 @@ static __attribute__((interrupt("IRQ"))) void EPIT1_HANDLER(void)
void tick_start(unsigned int interval_in_ms)
{
- CLKCTL_CGR0 |= CGR0_EPIT1(CG_ON_ALL); /* EPIT1 module clock ON -
- before writing regs! */
+ imx31_clkctl_module_clock_gating(CG_EPIT1, CGM_ON_ALL); /* EPIT1 module
+ clock ON - before writing
+ regs! */
EPITCR1 &= ~(EPITCR_OCIEN | EPITCR_EN); /* Disable the counter */
- CLKCTL_WIMR0 &= ~(1 << 23); /* Clear wakeup mask */
+ CLKCTL_WIMR0 &= ~WIM_IPI_INT_EPIT1; /* Clear wakeup mask */
/* mcu_main_clk = 528MHz = 27MHz * 2 * ((9 + 7/9) / 1)
* CLKSRC = ipg_clk = 528MHz / 4 / 2 = 66MHz,
@@ -66,11 +70,18 @@ void tick_start(unsigned int interval_in_ms)
EPITCR1 |= EPITCR_EN; /* Enable the counter */
}
+void kernel_device_init(void)
+{
+ spi_init();
+ mc13783_init();
+}
+
#ifdef BOOTLOADER
void tick_stop(void)
{
avic_disable_int(EPIT1); /* Disable insterrupt */
EPITCR1 &= ~(EPITCR_OCIEN | EPITCR_EN); /* Disable counter */
- CLKCTL_CGR0 &= ~CGR0_EPIT1(CG_MASK); /* EPIT1 module clock OFF */
+ EPITSR1 = EPITSR_OCIF; /* Clear pending */
+ imx31_clkctl_module_clock_gating(CG_EPIT1, CGM_OFF); /* Turn off module clock */
}
#endif
diff --git a/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c b/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
new file mode 100644
index 0000000000..3af8f35f69
--- /dev/null
+++ b/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
@@ -0,0 +1,173 @@
+/***************************************************************************
+ * __________ __ ___.
+ * Open \______ \ ____ ____ | | _\_ |__ _______ ___
+ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
+ * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
+ * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
+ * \/ \/ \/ \/ \/
+ * $Id$
+ *
+ * Copyright (c) 2008 by Michael Sevakis
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "system.h"
+#include "cpu.h"
+#include "spi-imx31.h"
+#include "mc13783.h"
+#include "debug.h"
+#include "kernel.h"
+
+/* This is all based on communicating with the MC13783 PMU which is on
+ * CSPI2 with the chip select at 0. The LCD controller resides on
+ * CSPI3 cs1, but we have no idea how to communicate to it */
+static struct spi_node mc13783_spi =
+{
+ CSPI2_NUM, /* CSPI module 2 */
+ CSPI_CONREG_CHIP_SELECT_SS0 | /* Chip select 0 */
+ CSPI_CONREG_DRCTL_DONT_CARE | /* Don't care about CSPI_RDY */
+ CSPI_CONREG_DATA_RATE_DIV_4 | /* Clock = IPG_CLK/4 - 16.5MHz */
+ CSPI_BITCOUNT(32-1) | /* All 32 bits are to be transferred */
+ CSPI_CONREG_SSPOL | /* SS active high */
+ CSPI_CONREG_SSCTL | /* Negate SS between SPI bursts */
+ CSPI_CONREG_MODE, /* Master mode */
+ 0, /* SPI clock - no wait states */
+};
+
+static int mc13783_thread_stack[DEFAULT_STACK_SIZE/sizeof(int)];
+static const char *mc13783_thread_name = "pmic";
+static struct wakeup mc13783_wake;
+
+static __attribute__((noreturn)) void mc13783_interrupt_thread(void)
+{
+ while (1)
+ {
+ wakeup_wait(&mc13783_wake, TIMEOUT_BLOCK);
+ }
+}
+
+static __attribute__((interrupt("IRQ"))) void mc13783_interrupt(void)
+{
+ wakeup_signal(&mc13783_wake);
+}
+
+void mc13783_init(void)
+{
+ /* Serial interface must have been initialized first! */
+ wakeup_init(&mc13783_wake);
+
+ /* Enable the PMIC SPI module */
+ spi_enable_module(&mc13783_spi);
+
+ create_thread(mc13783_interrupt_thread, mc13783_thread_stack,
+ sizeof(mc13783_thread_stack), 0, mc13783_thread_name
+ IF_PRIO(, PRIORITY_REALTIME) IF_COP(, CPU));
+}
+
+void mc13783_set(unsigned address, uint32_t bits)
+{
+ spi_lock(&mc13783_spi);
+ uint32_t data = mc13783_read(address);
+ mc13783_write(address, data | bits);
+ spi_unlock(&mc13783_spi);
+}
+
+void mc13783_clear(unsigned address, uint32_t bits)
+{
+ spi_lock(&mc13783_spi);
+ uint32_t data = mc13783_read(address);
+ mc13783_write(address, data & ~bits);
+ spi_unlock(&mc13783_spi);
+}
+
+int mc13783_write(unsigned address, uint32_t data)
+{
+ struct spi_transfer xfer;
+ uint32_t packet;
+
+ if (address >= MC13783_NUM_REGS)
+ return -1;
+
+ packet = (1 << 31) | (address << 25) | (data & 0xffffff);
+ xfer.txbuf = &packet;
+ xfer.rxbuf = &packet;
+ xfer.count = 1;
+
+ if (!spi_transfer(&mc13783_spi, &xfer))
+ return -1;
+
+ return 1 - xfer.count;
+}
+
+int mc13783_write_multiple(unsigned start, const uint32_t *data, int count)
+{
+ int i;
+ struct spi_transfer xfer;
+ uint32_t packets[64];
+
+ if (start + count > MC13783_NUM_REGS)
+ return -1;
+
+ /* Prepare payload */
+ for (i = 0; i < count; i++, start++)
+ {
+ packets[i] = (1 << 31) | (start << 25) | (data[i] & 0xffffff);
+ }
+
+ xfer.txbuf = packets;
+ xfer.rxbuf = packets;
+ xfer.count = count;
+
+ if (!spi_transfer(&mc13783_spi, &xfer))
+ return -1;
+
+ return count - xfer.count;
+}
+
+uint32_t mc13783_read(unsigned address)
+{
+ uint32_t packet;
+ struct spi_transfer xfer;
+
+ if (address >= MC13783_NUM_REGS)
+ return (uint32_t)-1;
+
+ packet = address << 25;
+
+ xfer.txbuf = &packet;
+ xfer.rxbuf = &packet;
+ xfer.count = 1;
+
+ if (!spi_transfer(&mc13783_spi, &xfer))
+ return (uint32_t)-1;
+
+ return packet & 0xffffff;
+}
+
+int mc13783_read_multiple(unsigned start, uint32_t *buffer, int count)
+{
+ int i;
+ uint32_t packets[64];
+ struct spi_transfer xfer;
+
+ if (start + count > MC13783_NUM_REGS)
+ return -1;
+
+ xfer.txbuf = packets;
+ xfer.rxbuf = buffer;
+ xfer.count = count;
+
+ /* Prepare TX payload */
+ for (i = 0; i < count; i++, start++)
+ packets[i] = start << 25;
+
+ if (!spi_transfer(&mc13783_spi, &xfer))
+ return -1;
+
+ return count - xfer.count;
+}
diff --git a/firmware/target/arm/imx31/gigabeat-s/spi-imx31.c b/firmware/target/arm/imx31/gigabeat-s/spi-imx31.c
index 10ee3f44c0..bcbe85a76b 100644
--- a/firmware/target/arm/imx31/gigabeat-s/spi-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/spi-imx31.c
@@ -18,66 +18,325 @@
****************************************************************************/
#include "cpu.h"
#include "spi-imx31.h"
+#include "avic-imx31.h"
+#include "clkctl-imx31.h"
#include "debug.h"
#include "kernel.h"
- /* This is all based on communicating with the MC13783 PMU which is on
- * CSPI2 with the chip select at 0. The LCD controller resides on
- * CSPI3 cs1, but we have no idea how to communicate to it */
-
-void spi_init(void) {
- CSPI_CONREG2 |= (2 << 20); // Burst will be triggered at SPI_RDY low
- CSPI_CONREG2 |= (2 << 16); // Clock = IPG_CLK/16 - we want about 20mhz
- CSPI_CONREG2 |= (31 << 8); // All 32 bits are to be transferred
- CSPI_CONREG2 |= (1 << 3); // Start burst on TXFIFO write.
- CSPI_CONREG2 |= (1 << 1); // Master mode.
- CSPI_CONREG2 |= 1; // Enable CSPI2;
+/* Forward interrupt handler declarations */
+#if (SPI_MODULE_MASK & USE_CSPI1_MODULE)
+static __attribute__((interrupt("IRQ"))) void CSPI1_HANDLER(void);
+#endif
+#if (SPI_MODULE_MASK & USE_CSPI2_MODULE)
+static __attribute__((interrupt("IRQ"))) void CSPI2_HANDLER(void);
+#endif
+#if (SPI_MODULE_MASK & USE_CSPI3_MODULE)
+static __attribute__((interrupt("IRQ"))) void CSPI3_HANDLER(void);
+#endif
+
+/* State data associatated with each CSPI module */
+static struct spi_module_descriptor
+{
+ volatile unsigned long *base;
+ int enab;
+ struct spi_node *last;
+ enum IMX31_CG_LIST cg;
+ enum IMX31_INT_LIST ints;
+ int byte_size;
+ void (*handler)(void);
+ struct mutex m;
+ struct wakeup w;
+ struct spi_transfer *trans;
+ int rxcount;
+} spi_descs[SPI_NUM_CSPI] =
+/* Init non-zero members */
+{
+#if (SPI_MODULE_MASK & USE_CSPI1_MODULE)
+ {
+ .base = (unsigned long *)CSPI1_BASE_ADDR,
+ .cg = CG_CSPI1,
+ .ints = CSPI1,
+ .handler = CSPI1_HANDLER,
+ },
+#endif
+#if (SPI_MODULE_MASK & USE_CSPI2_MODULE)
+ {
+ .base = (unsigned long *)CSPI2_BASE_ADDR,
+ .cg = CG_CSPI2,
+ .ints = CSPI2,
+ .handler = CSPI2_HANDLER,
+ },
+#endif
+#if (SPI_MODULE_MASK & USE_CSPI3_MODULE)
+ {
+ .base = (unsigned long *)CSPI3_BASE_ADDR,
+ .cg = CG_CSPI3,
+ .ints = CSPI3,
+ .handler = CSPI3_HANDLER,
+ },
+#endif
+};
+
+/* Common code for interrupt handlers */
+static void spi_interrupt(enum spi_module_number spi)
+{
+ struct spi_module_descriptor *desc = &spi_descs[spi];
+ volatile unsigned long * const base = desc->base;
+ struct spi_transfer *trans = desc->trans;
+ int inc = desc->byte_size + 1;
+
+ if (desc->rxcount > 0)
+ {
+ /* Data received - empty out RXFIFO */
+ while ((base[CSPI_STATREG_I] & CSPI_STATREG_RR) != 0)
+ {
+ uint32_t word = base[CSPI_RXDATA_I];
+
+ switch (desc->byte_size & 3)
+ {
+ case 3:
+ *(unsigned char *)(trans->rxbuf + 3) = word >> 24;
+ case 2:
+ *(unsigned char *)(trans->rxbuf + 2) = word >> 16;
+ case 1:
+ *(unsigned char *)(trans->rxbuf + 1) = word >> 8;
+ case 0:
+ *(unsigned char *)(trans->rxbuf + 0) = word;
+ }
+
+ trans->rxbuf += inc;
+
+ if (--desc->rxcount < 4)
+ {
+ unsigned long intreg = base[CSPI_INTREG_I];
+
+ if (desc->rxcount <= 0)
+ {
+ /* No more to receive - stop RX interrupts */
+ intreg &= ~(CSPI_INTREG_RHEN | CSPI_INTREG_RREN);
+ base[CSPI_INTREG_I] = intreg;
+ break;
+ }
+ else if (!(intreg & CSPI_INTREG_RREN))
+ {
+ /* < 4 words expected - switch to RX ready */
+ intreg &= ~CSPI_INTREG_RHEN;
+ base[CSPI_INTREG_I] = intreg | CSPI_INTREG_RREN;
+ }
+ }
+ }
+ }
+
+ if (trans->count > 0)
+ {
+ /* Data to transmit - fill TXFIFO or write until exhausted */
+ while ((base[CSPI_STATREG_I] & CSPI_STATREG_TF) == 0)
+ {
+ uint32_t word = 0;
+
+ switch (desc->byte_size & 3)
+ {
+ case 3:
+ word = *(unsigned char *)(trans->txbuf + 3) << 24;
+ case 2:
+ word |= *(unsigned char *)(trans->txbuf + 2) << 16;
+ case 1:
+ word |= *(unsigned char *)(trans->txbuf + 1) << 8;
+ case 0:
+ word |= *(unsigned char *)(trans->txbuf + 0);
+ }
+
+ trans->txbuf += inc;
+
+ base[CSPI_TXDATA_I] = word;
+
+ if (--trans->count <= 0)
+ {
+ /* Out of data - stop TX interrupts */
+ base[CSPI_INTREG_I] &= ~CSPI_INTREG_THEN;
+ break;
+ }
+ }
+ }
+
+ /* If all interrupts have been remasked - we're done */
+ if (base[CSPI_INTREG_I] == 0)
+ {
+ base[CSPI_STATREG_I] = CSPI_STATREG_TC | CSPI_STATREG_BO;
+ wakeup_signal(&desc->w);
+ }
+}
+
+/* Interrupt handlers for each CSPI module */
+#if (SPI_MODULE_MASK & USE_CSPI1_MODULE)
+static __attribute__((interrupt("IRQ"))) void CSPI1_HANDLER(void)
+{
+ spi_interrupt(CSPI1_NUM);
+}
+#endif
+
+#if (SPI_MODULE_MASK & USE_CSPI2_MODULE)
+static __attribute__((interrupt("IRQ"))) void CSPI2_HANDLER(void)
+{
+ spi_interrupt(CSPI2_NUM);
+}
+#endif
+
+#if (SPI_MODULE_MASK & USE_CSPI3_MODULE)
+static __attribute__((interrupt("IRQ"))) void CSPI3_HANDLER(void)
+{
+ spi_interrupt(CSPI3_NUM);
+}
+#endif
+
+/* Write the context for the node and remember it to avoid unneeded reconfigure */
+static bool spi_set_context(struct spi_node *node,
+ struct spi_module_descriptor *desc)
+{
+ volatile unsigned long * const base = desc->base;
+
+ if ((base[CSPI_CONREG_I] & CSPI_CONREG_EN) == 0)
+ return false;
+
+ if (node != desc->last)
+ {
+ /* Switch the module's node */
+ desc->last = node;
+ desc->byte_size = (((node->conreg >> 8) & 0x1f) + 1 + 7) / 8 - 1;
+
+ /* Keep reserved and start bits cleared. Keep enabled bit. */
+ base[CSPI_CONREG_I] =
+ (node->conreg & ~(0xfcc8e000 | CSPI_CONREG_XCH | CSPI_CONREG_SMC))
+ | CSPI_CONREG_EN;
+ /* Set the wait-states */
+ base[CSPI_PERIODREG_I] = node->periodreg & 0xffff;
+ /* Clear out any spuriously-pending interrupts */
+ base[CSPI_STATREG_I] = CSPI_STATREG_TC | CSPI_STATREG_BO;
+ }
+
+ return true;
}
-static int spi_transfer(int address, long data, long* buffer, bool read) {
- return -1; /* Disable for now - hangs - and we'll use interrupts */
+/* Initialize each of the used SPI descriptors */
+void spi_init(void)
+{
+ int i;
- unsigned long packet = 0;
- if(!read) {
- /* Set the appropriate bit in the packet to indicate a write */
- packet |= (1<<31);
+ for (i = 0; i < SPI_NUM_CSPI; i++)
+ {
+ struct spi_module_descriptor * const desc = &spi_descs[i];
+ mutex_init(&desc->m);
+ wakeup_init(&desc->w);
}
- /* Set the address of the packet */
- packet |= (address << 25);
-
- /* Ensure data only occupies 24 bits, then mash the data into the packet */
- data &= ~(DATAMASK);
- packet |= data;
-
- /* Wait for some room in TXFIFO */
- while(CSPI_STATREG2 & (1<<2));
-
- /* Send the packet */
- CSPI_TXDATA2 = packet;
-
- /* Poll the XCH bit to wait for the end of the transfer, with
- * a one second timeout */
- int newtick = current_tick + HZ;
- while((CSPI_CONREG2 & (1<<2)) && (current_tick < newtick));
-
- if(newtick > current_tick) {
- *buffer = CSPI_RXDATA2;
- return 0;
- } else {
- /* Indicate the fact that the transfer timed out */
- return -1;
+}
+
+/* Get mutually-exclusive access to the node */
+void spi_lock(struct spi_node *node)
+{
+ mutex_lock(&spi_descs[node->num].m);
+}
+
+/* Release mutual exclusion */
+void spi_unlock(struct spi_node *node)
+{
+ mutex_unlock(&spi_descs[node->num].m);
+}
+
+/* Enable the specified module for the node */
+void spi_enable_module(struct spi_node *node)
+{
+ struct spi_module_descriptor * const desc = &spi_descs[node->num];
+
+ mutex_lock(&desc->m);
+
+ if (++desc->enab == 1)
+ {
+ /* First enable for this module */
+ volatile unsigned long * const base = desc->base;
+
+ /* Enable clock-gating register */
+ imx31_clkctl_module_clock_gating(desc->cg, CGM_ON_ALL);
+
+ /* Reset */
+ base[CSPI_CONREG_I] &= ~CSPI_CONREG_EN;
+ base[CSPI_CONREG_I] |= CSPI_CONREG_EN;
+ base[CSPI_INTREG_I] = 0;
+ base[CSPI_STATREG_I] = CSPI_STATREG_TC | CSPI_STATREG_BO;
+
+ /* Enable interrupt at controller level */
+ avic_enable_int(desc->ints, IRQ, 6, desc->handler);
}
+
+ mutex_unlock(&desc->m);
}
-void spi_send(int address, unsigned long data) {
- long dummy;
- if(spi_transfer(address, data, &dummy, false)) {
- DEBUGF("SPI Send timed out");
+/* Disabled the specified module for the node */
+void spi_disable_module(struct spi_node *node)
+{
+ struct spi_module_descriptor * const desc = &spi_descs[node->num];
+
+ mutex_lock(&desc->m);
+
+ if (desc->enab > 0 && --desc->enab == 0)
+ {
+ /* Last enable for this module */
+ volatile unsigned long * const base = desc->base;
+
+ /* Disable interrupt at controller level */
+ avic_disable_int(desc->ints);
+
+ /* Disable interface */
+ base[CSPI_CONREG_I] &= ~CSPI_CONREG_EN;
+
+ /* Disable interface clock */
+ imx31_clkctl_module_clock_gating(desc->cg, CGM_OFF);
}
+
+ mutex_unlock(&desc->m);
}
-void spi_read(int address, unsigned long* buffer) {
- if(spi_transfer(address, 0, buffer, true)) {
- DEBUGF("SPI read timed out");
+/* Send and/or receive data on the specified node */
+int spi_transfer(struct spi_node *node, struct spi_transfer *trans)
+{
+ struct spi_module_descriptor * const desc = &spi_descs[node->num];
+ int retval;
+
+ if (trans->count <= 0)
+ return true;
+
+ mutex_lock(&desc->m);
+
+ retval = spi_set_context(node, desc);
+
+ if (retval)
+ {
+ volatile unsigned long * const base = desc->base;
+ unsigned long intreg;
+
+ desc->trans = trans;
+ desc->rxcount = trans->count;
+
+ /* Enable needed interrupts - FIFOs will start filling */
+ intreg = CSPI_INTREG_THEN;
+
+ intreg |= (trans->count < 4) ?
+ CSPI_INTREG_RREN : /* Must grab data on every word */
+ CSPI_INTREG_RHEN; /* Enough data to wait for half-full */
+
+ base[CSPI_INTREG_I] = intreg;
+
+ /* Start transfer */
+ base[CSPI_CONREG_I] |= CSPI_CONREG_XCH;
+
+ if (wakeup_wait(&desc->w, HZ) != OBJ_WAIT_SUCCEEDED)
+ {
+ base[CSPI_INTREG_I] = 0;
+ base[CSPI_CONREG_I] &= ~CSPI_CONREG_XCH;
+ retval = false;
+ }
}
+
+ mutex_unlock(&desc->m);
+
+ return retval;
}
diff --git a/firmware/target/arm/imx31/gigabeat-s/spi-imx31.h b/firmware/target/arm/imx31/gigabeat-s/spi-imx31.h
index 5f6cfc984f..c17a272aaa 100644
--- a/firmware/target/arm/imx31/gigabeat-s/spi-imx31.h
+++ b/firmware/target/arm/imx31/gigabeat-s/spi-imx31.h
@@ -16,8 +16,58 @@
* KIND, either express or implied.
*
****************************************************************************/
-#define DATAMASK 0xFF000000
+#ifndef _SPI_IMX31_H_
+#define _SPI_IMX31_H_
+#define USE_CSPI1_MODULE (1 << 0)
+#define USE_CSPI2_MODULE (1 << 1)
+#define USE_CSPI3_MODULE (1 << 2)
+
+/* SPI_MODULE_MASK is set in target's config */
+enum spi_module_number
+{
+#if (SPI_MODULE_MASK & USE_CSPI1_MODULE)
+ CSPI1_NUM = 0,
+#endif
+#if (SPI_MODULE_MASK & USE_CSPI2_MODULE)
+ CSPI2_NUM,
+#endif
+#if (SPI_MODULE_MASK & USE_CSPI3_MODULE)
+ CSPI3_NUM,
+#endif
+ SPI_NUM_CSPI,
+};
+
+struct spi_node
+{
+ enum spi_module_number num; /* Module number (CSPIx_NUM) */
+ unsigned long conreg; /* CSPI conreg setup */
+ unsigned long periodreg; /* CSPI periodreg setup */
+};
+
+struct spi_transfer
+{
+ const void *txbuf;
+ void *rxbuf;
+ int count;
+};
+
+/* One-time init of SPI driver */
void spi_init(void);
-void spi_send(int address, unsigned long data);
-void spi_read(int address, unsigned long* buffer);
+
+/* Enable the specified module for the node */
+void spi_enable_module(struct spi_node *node);
+
+/* Disabled the specified module for the node */
+void spi_disable_module(struct spi_node *node);
+
+/* Lock module mutex */
+void spi_lock(struct spi_node *node);
+
+/* Unlock module mutex */
+void spi_unlock(struct spi_node *node);
+
+/* Send and/or receive data on the specified node */
+int spi_transfer(struct spi_node *node, struct spi_transfer *trans);
+
+#endif /* _SPI_IMX31_H_ */
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-target.h b/firmware/target/arm/imx31/gigabeat-s/system-target.h
index 327d72b7bc..8db0c78eb7 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/system-target.h
@@ -35,6 +35,9 @@ static inline void udelay(unsigned int usecs)
void system_prepare_fw_start(void);
void tick_stop(void);
+void kernel_device_init(void);
+
+#define KDEV_INIT
#define HAVE_INVALIDATE_ICACHE
static inline void invalidate_icache(void)