diff --git a/nuttx/arch/arm/src/calypso/calypso_armio.c b/nuttx/arch/arm/src/calypso/calypso_armio.c index 59c5a79e76..56f049f945 100644 --- a/nuttx/arch/arm/src/calypso/calypso_armio.c +++ b/nuttx/arch/arm/src/calypso/calypso_armio.c @@ -41,6 +41,7 @@ #include #include +#include "up_arch.h" /**************************************************************************** * HW access @@ -92,10 +93,10 @@ static int kbd_gpio_irq(int irq, uint32_t *regs) void calypso_armio(void) { /* Enable ARMIO clock */ - writew(1<<5, ARMIO_REG(CNTL_REG)); + putreg16(1<<5, ARMIO_REG(CNTL_REG)); /* Mask GPIO interrupt and keypad interrupt */ - writew(KBD_INT|GPIO_INT, ARMIO_REG(KBD_GPIO_MASKIT)); + putreg16(KBD_INT|GPIO_INT, ARMIO_REG(KBD_GPIO_MASKIT)); /* Attach and enable the interrupt */ irq_attach(IRQ_KEYPAD_GPIO, (xcpt_t)kbd_gpio_irq); diff --git a/nuttx/arch/arm/src/calypso/calypso_heap.c b/nuttx/arch/arm/src/calypso/calypso_heap.c index 70c44da018..a6b5e55c96 100644 --- a/nuttx/arch/arm/src/calypso/calypso_heap.c +++ b/nuttx/arch/arm/src/calypso/calypso_heap.c @@ -1,5 +1,5 @@ /**************************************************************************** - * calypso_heap.c + * arch/arm/src/calypso/calypso_heap.c * Initialize memory interfaces of Calypso MCU * * (C) 2010 by Harald Welte diff --git a/nuttx/arch/arm/src/calypso/calypso_irq.c b/nuttx/arch/arm/src/calypso/calypso_irq.c index a37fc6f389..fa818df402 100644 --- a/nuttx/arch/arm/src/calypso/calypso_irq.c +++ b/nuttx/arch/arm/src/calypso/calypso_irq.c @@ -50,6 +50,7 @@ #include #include "arm.h" +#include "up_arch.h" /**************************************************************************** * Pre-processor Definitions @@ -124,12 +125,12 @@ static void _irq_enable(enum irq_nr nr, int enable) nr -= 16; } - val = readw(reg); + val = getreg16(reg); if (enable) val &= ~(1 << nr); else val |= (1 << nr); - writew(val, reg); + putreg16(val, reg); } static void set_default_priorities(void) @@ -142,14 +143,14 @@ static void set_default_priorities(void) if (prio > 31) prio = 31; - val = readw(IRQ_REG(ILR_IRQ(i))); + val = getreg16(IRQ_REG(ILR_IRQ(i))); val &= ~(0x1f << 2); val |= prio << 2; /* Make edge mode default. Hopefully causes less trouble */ val |= 0x02; - writew(val, IRQ_REG(ILR_IRQ(i))); + putreg16(val, IRQ_REG(ILR_IRQ(i))); } } @@ -190,12 +191,12 @@ void up_irqinitialize(void) set_default_priorities(); /* mask all interrupts off */ - writew(0xffff, IRQ_REG(MASK_IT_REG1)); - writew(0xffff, IRQ_REG(MASK_IT_REG2)); + putreg16(0xffff, IRQ_REG(MASK_IT_REG1)); + putreg16(0xffff, IRQ_REG(MASK_IT_REG2)); /* clear all pending interrupts */ - writew(0, IRQ_REG(IT_REG1)); - writew(0, IRQ_REG(IT_REG2)); + putreg16(0, IRQ_REG(IT_REG1)); + putreg16(0, IRQ_REG(IT_REG2)); /* enable interrupts globally to the ARM core */ #ifndef CONFIG_SUPPRESS_INTERRUPTS @@ -251,7 +252,7 @@ int up_prioritize_irq(int nr, int prio) prio = 31; val = prio << 2; - writew(val, IRQ_REG(ILR_IRQ(nr))); + putreg16(val, IRQ_REG(ILR_IRQ(nr))); return 0; // XXX: what's the return??? } @@ -273,13 +274,13 @@ void up_decodeirq(uint32_t *regs) current_regs = regs; /* Detect & deliver the IRQ */ - num = readb(IRQ_REG(IRQ_NUM)) & 0x1f; + num = getreg8(IRQ_REG(IRQ_NUM)) & 0x1f; irq_dispatch(num, regs); /* Start new IRQ agreement */ - tmp = readb(IRQ_REG(IRQ_CTRL)); + tmp = getreg8(IRQ_REG(IRQ_CTRL)); tmp |= 0x01; - writeb(tmp, IRQ_REG(IRQ_CTRL)); + putreg8(tmp, IRQ_REG(IRQ_CTRL)); current_regs = saved_regs; } @@ -300,13 +301,13 @@ void calypso_fiq(void) current_regs = (uint32_t *)# /* Detect & deliver like an IRQ but we are in FIQ context */ - num = readb(IRQ_REG(FIQ_NUM)) & 0x1f; + num = getreg8(IRQ_REG(FIQ_NUM)) & 0x1f; irq_dispatch(num, regs); /* Start new FIQ agreement */ - tmp = readb(IRQ_REG(IRQ_CTRL)); + tmp = getreg8(IRQ_REG(IRQ_CTRL)); tmp |= 0x02; - writeb(tmp, IRQ_REG(IRQ_CTRL)); + putreg8(tmp, IRQ_REG(IRQ_CTRL)); current_regs = regs; } diff --git a/nuttx/arch/arm/src/calypso/calypso_serial.c b/nuttx/arch/arm/src/calypso/calypso_serial.c index e64743ae12..69fe26a794 100644 --- a/nuttx/arch/arm/src/calypso/calypso_serial.c +++ b/nuttx/arch/arm/src/calypso/calypso_serial.c @@ -1,5 +1,5 @@ /**************************************************************************** - * calypso/calypso_serial.c + * arch/arm/src/calypso/calypso_serial.c * * Copyright (C) 2011 Stefan Richter. All rights reserved. * Author: Stefan Richter diff --git a/nuttx/arch/arm/src/calypso/calypso_spi.c b/nuttx/arch/arm/src/calypso/calypso_spi.c index cc20f20726..1158c7f2f6 100644 --- a/nuttx/arch/arm/src/calypso/calypso_spi.c +++ b/nuttx/arch/arm/src/calypso/calypso_spi.c @@ -1,10 +1,12 @@ /**************************************************************************** - * calypso_spi.c + * arch/arm/src/calypso/calypso_spi.c * SPI driver for TI Calypso * + * Copyright (C) 2010 Harald Welte * Copyright (C) 2011 Stefan Richter * - * All rights reserved. + * Part of this source code is derivated from Osmocom-BB project and was + * relicensed as BSD with permission from original authors. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +40,11 @@ #include #include +#include + +#include "up_arch.h" +#include "calypso_spi.h" + #warning "MOST OF SPI API IS INCOMPLETE! (Wrapper around Osmocom driver)" extern void spi_init(void); extern int spi_xfer(uint8_t dev_idx, uint8_t bitlen, const void *dout, void *din); @@ -127,6 +134,90 @@ static struct calypso_spidev_s g_spidev = .nbits = 0, }; +void spi_init(void) +{ + putreg16(SPI_SET1_EN_CLK | SPI_SET1_WR_IRQ_DIS | SPI_SET1_RDWR_IRQ_DIS, + SPI_REG(REG_SET1)); + + putreg16(0x0001, SPI_REG(REG_SET2)); +} + +int spi_xfer(uint8_t dev_idx, uint8_t bitlen, const void *dout, void *din) +{ + uint8_t bytes_per_xfer; + uint8_t reg_status, reg_ctrl = 0; + uint32_t tmp; + + if (bitlen == 0) + return 0; + + if (bitlen > 32) + return -1; + + if (dev_idx > 4) + return -1; + + bytes_per_xfer = bitlen / 8; + if (bitlen % 8) + bytes_per_xfer ++; + + reg_ctrl |= (bitlen - 1) << SPI_CTRL_NB_SHIFT; + reg_ctrl |= (dev_idx & 0x7) << SPI_CTRL_AD_SHIFT; + + if (bitlen <= 8) { + tmp = *(uint8_t *)dout; + tmp <<= 24 + (8-bitlen); /* align to MSB */ + } else if (bitlen <= 16) { + tmp = *(uint16_t *)dout; + tmp <<= 16 + (16-bitlen); /* align to MSB */ + } else { + tmp = *(uint32_t *)dout; + tmp <<= (32-bitlen); /* align to MSB */ + } + dbg("spi_xfer(dev_idx=%u, bitlen=%u, data_out=0x%08x): ", + dev_idx, bitlen, tmp); + + /* fill transmit registers */ + putreg16(tmp >> 16, SPI_REG(REG_TX_MSB)); + putreg16(tmp & 0xffff, SPI_REG(REG_TX_LSB)); + + /* initiate transfer */ + if (din) + reg_ctrl |= SPI_CTRL_RDWR; + else + reg_ctrl |= SPI_CTRL_WR; + putreg16(reg_ctrl, SPI_REG(REG_CTRL)); + dbg("reg_ctrl=0x%04x ", reg_ctrl); + + /* wait until the transfer is complete */ + while (1) { + reg_status = getreg16(SPI_REG(REG_STATUS)); + dbg("status=0x%04x ", reg_status); + if (din && (reg_status & SPI_STATUS_RE)) + break; + else if (reg_status & SPI_STATUS_WE) + break; + } + /* FIXME: calibrate how much delay we really need (seven 13MHz cycles) */ + usleep(1000); + + if (din) { + tmp = getreg16(SPI_REG(REG_RX_MSB)) << 16; + tmp |= getreg16(SPI_REG(REG_RX_LSB)); + dbg("data_in=0x%08x ", tmp); + + if (bitlen <= 8) + *(uint8_t *)din = tmp & 0xff; + else if (bitlen <= 16) + *(uint16_t *)din = tmp & 0xffff; + else + *(uint32_t *)din = tmp; + } + dbg('\n'); + + return 0; +} + FAR struct spi_dev_s *up_spiinitialize(int port) { switch(port) { diff --git a/nuttx/arch/arm/src/calypso/calypso_timer.c b/nuttx/arch/arm/src/calypso/calypso_timer.c index e42b95d147..39eca03073 100644 --- a/nuttx/arch/arm/src/calypso/calypso_timer.c +++ b/nuttx/arch/arm/src/calypso/calypso_timer.c @@ -45,6 +45,7 @@ #include #include +#include "up_arch.h" #define BASE_ADDR_TIMER 0xfffe3800 #define TIMER2_OFFSET 0x3000 @@ -74,12 +75,12 @@ void hwtimer_enable(int num, int on) return; } - ctl = readb(TIMER_REG(num, CNTL_TIMER)); + ctl = getreg8(TIMER_REG(num, CNTL_TIMER)); if (on) ctl |= CNTL_START|CNTL_CLOCK_ENABLE; else ctl &= ~CNTL_START; - writeb(ctl, TIMER_REG(num, CNTL_TIMER)); + putreg8(ctl, TIMER_REG(num, CNTL_TIMER)); } void hwtimer_config(int num, uint8_t pre_scale, int auto_reload) @@ -90,22 +91,22 @@ void hwtimer_config(int num, uint8_t pre_scale, int auto_reload) if (auto_reload) ctl |= CNTL_AUTO_RELOAD; - writeb(ctl, TIMER_REG(num, CNTL_TIMER)); + putreg8(ctl, TIMER_REG(num, CNTL_TIMER)); } void hwtimer_load(int num, uint16_t val) { - writew(val, TIMER_REG(num, LOAD_TIMER)); + putreg16(val, TIMER_REG(num, LOAD_TIMER)); } uint16_t hwtimer_read(int num) { - uint8_t ctl = readb(TIMER_REG(num, CNTL_TIMER)); + uint8_t ctl = getreg8(TIMER_REG(num, CNTL_TIMER)); /* somehow a read results in an abort */ if ((ctl & (CNTL_START|CNTL_CLOCK_ENABLE)) != (CNTL_START|CNTL_CLOCK_ENABLE)) return 0xFFFF; - return readw(TIMER_REG(num, READ_TIMER)); + return getreg16(TIMER_REG(num, READ_TIMER)); } /************************************************************ @@ -143,18 +144,18 @@ static void wdog_irq(__unused enum irq_nr nr) void wdog_enable(int on) { if (!on) { - writew(WD_MODE_DIS_ARM, WDOG_REG(WD_MODE)); - writew(WD_MODE_DIS_CONFIRM, WDOG_REG(WD_MODE)); + putreg16(WD_MODE_DIS_ARM, WDOG_REG(WD_MODE)); + putreg16(WD_MODE_DIS_CONFIRM, WDOG_REG(WD_MODE)); } } void wdog_reset(void) { // enable watchdog - writew(WD_MODE_ENABLE, WDOG_REG(WD_MODE)); + putreg16(WD_MODE_ENABLE, WDOG_REG(WD_MODE)); // force expiration - writew(0x0000, WDOG_REG(WD_LOAD_TIMER)); - writew(0x0000, WDOG_REG(WD_LOAD_TIMER)); + putreg16(0x0000, WDOG_REG(WD_LOAD_TIMER)); + putreg16(0x0000, WDOG_REG(WD_LOAD_TIMER)); } /************************************************************ diff --git a/nuttx/arch/arm/src/calypso/clock.c b/nuttx/arch/arm/src/calypso/clock.c index f2f36fbec5..3c753cd863 100644 --- a/nuttx/arch/arm/src/calypso/clock.c +++ b/nuttx/arch/arm/src/calypso/clock.c @@ -47,6 +47,8 @@ #include #include +#include "up_arch.h" + #define REG_DPLL 0xffff9800 #define DPLL_LOCK (1 << 0) #define DPLL_BREAKLN (1 << 1) @@ -98,7 +100,7 @@ enum memif_reg { static void dump_reg16(uint32_t addr, char *name) { - printf("%s=0x%04x\n", name, readw(addr)); + printf("%s=0x%04x\n", name, getreg16(addr)); } void calypso_clk_dump(void) @@ -114,31 +116,31 @@ void calypso_pll_set(uint16_t inp) { uint8_t mult = inp >> 8; uint8_t div = inp & 0xff; - uint16_t reg = readw(REG_DPLL); + uint16_t reg = getreg16(REG_DPLL); reg &= ~0x0fe0; reg |= (div & 0x3) << DPLL_PLL_DIV_SHIFT; reg |= (mult & 0x1f) << DPLL_PLL_MULT_SHIFT; reg |= DPLL_PLL_ENABLE; - writew(reg, REG_DPLL); + putreg16(reg, REG_DPLL); } void calypso_reset_set(enum calypso_rst calypso_rst, int active) { - uint8_t reg = readb(CLKM_REG(CNTL_RST)); + uint8_t reg = getreg8(CLKM_REG(CNTL_RST)); if (active) reg |= calypso_rst; else reg &= ~calypso_rst; - writeb(reg, CLKM_REG(CNTL_RST)); + putreg8(reg, CLKM_REG(CNTL_RST)); } int calypso_reset_get(enum calypso_rst calypso_rst) { - uint8_t reg = readb(CLKM_REG(CNTL_RST)); + uint8_t reg = getreg8(CLKM_REG(CNTL_RST)); if (reg & calypso_rst) return 1; @@ -148,8 +150,8 @@ int calypso_reset_get(enum calypso_rst calypso_rst) void calypso_clock_set(uint8_t vtcxo_div2, uint16_t inp, enum mclk_div mclk_div) { - uint16_t cntl_clock = readw(CLKM_REG(CNTL_CLK)); - uint16_t cntl_arm_clk = readw(CLKM_REG(CNTL_ARM_CLK)); + uint16_t cntl_clock = getreg16(CLKM_REG(CNTL_CLK)); + uint16_t cntl_arm_clk = getreg16(CLKM_REG(CNTL_ARM_CLK)); /* First set the vtcxo_div2 */ cntl_clock &= ~CLK_VCLKOUT_DIV2; @@ -157,7 +159,7 @@ void calypso_clock_set(uint8_t vtcxo_div2, uint16_t inp, enum mclk_div mclk_div) cntl_clock |= CLK_VTCXO_DIV2; else cntl_clock &= ~CLK_VTCXO_DIV2; - writew(cntl_clock, CLKM_REG(CNTL_CLK)); + putreg16(cntl_clock, CLKM_REG(CNTL_CLK)); /* Then configure the MCLK divider */ cntl_arm_clk &= ~ARM_CLK_CLKIN_SEL0; @@ -168,7 +170,7 @@ void calypso_clock_set(uint8_t vtcxo_div2, uint16_t inp, enum mclk_div mclk_div) cntl_arm_clk &= ~ARM_CLK_MCLK_DIV5; cntl_arm_clk &= ~(0x7 << ARM_CLK_MCLK_DIV_SHIFT); cntl_arm_clk |= (mclk_div << ARM_CLK_MCLK_DIV_SHIFT); - writew(cntl_arm_clk, CLKM_REG(CNTL_ARM_CLK)); + putreg16(cntl_arm_clk, CLKM_REG(CNTL_ARM_CLK)); /* Then finally set the PLL */ calypso_pll_set(inp); @@ -177,32 +179,32 @@ void calypso_clock_set(uint8_t vtcxo_div2, uint16_t inp, enum mclk_div mclk_div) void calypso_mem_cfg(enum calypso_bank bank, uint8_t ws, enum calypso_mem_width width, int we) { - writew((ws & 0x1f) | ((width & 3) << 5) | ((we & 1) << 7), + putreg16((ws & 0x1f) | ((width & 3) << 5) | ((we & 1) << 7), BASE_ADDR_MEMIF + bank); } void calypso_bootrom(int enable) { - uint16_t conf = readw(MEMIF_REG(EXTRA_CONF)); + uint16_t conf = getreg16(MEMIF_REG(EXTRA_CONF)); conf |= (3 << 8); if (enable) conf &= ~(1 << 9); - writew(conf, MEMIF_REG(EXTRA_CONF)); + putreg16(conf, MEMIF_REG(EXTRA_CONF)); } void calypso_debugunit(int enable) { - uint16_t conf = readw(MEMIF_REG(EXTRA_CONF)); + uint16_t conf = getreg16(MEMIF_REG(EXTRA_CONF)); if (enable) conf &= ~(1 << 11); else conf |= (1 << 11); - writew(conf, MEMIF_REG(EXTRA_CONF)); + putreg16(conf, MEMIF_REG(EXTRA_CONF)); } #define REG_RHEA_CNTL 0xfffff900 @@ -212,7 +214,7 @@ void calypso_debugunit(int enable) void calypso_rhea_cfg(uint8_t fac0, uint8_t fac1, uint8_t timeout, uint8_t ws_h, uint8_t ws_l, uint8_t w_en0, uint8_t w_en1) { - writew(fac0 | (fac1 << 4) | (timeout << 8), REG_RHEA_CNTL); - writew(ws_h | (ws_l << 5), REG_API_CNTL); - writew(w_en0 | (w_en1 << 1), REG_ARM_RHEA); + putreg16(fac0 | (fac1 << 4) | (timeout << 8), REG_RHEA_CNTL); + putreg16(ws_h | (ws_l << 5), REG_API_CNTL); + putreg16(w_en0 | (w_en1 << 1), REG_ARM_RHEA); }