Merge remote-tracking branch 'spiriou/wlan_dev' into photon

This commit is contained in:
Gregory Nutt 2017-04-22 08:26:40 -06:00
commit d8e4cbcfd5
27 changed files with 25092 additions and 68 deletions

View File

@ -300,6 +300,8 @@
# endif
#endif
#define STM32_SDIO_USE_DEFAULT_BLOCKSIZE ((uint8_t)-1)
/****************************************************************************
* Private Types
****************************************************************************/
@ -333,6 +335,12 @@ struct stm32_dev_s
size_t remaining; /* Number of bytes remaining in the transfer */
uint32_t xfrmask; /* Interrupt enables for data transfer */
/* Fixed transfer block size support */
#ifdef CONFIG_SDIO_BLOCKSETUP
uint8_t block_size;
#endif
/* DMA data transfer support */
bool widebus; /* Required for DMA support */
@ -443,6 +451,10 @@ static int stm32_attach(FAR struct sdio_dev_s *dev);
static int stm32_sendcmd(FAR struct sdio_dev_s *dev, uint32_t cmd,
uint32_t arg);
#ifdef CONFIG_SDIO_BLOCKSETUP
static void stm32_blocksetup(FAR struct sdio_dev_s *dev,
unsigned int blocklen, unsigned int nblocks);
#endif
static int stm32_recvsetup(FAR struct sdio_dev_s *dev, FAR uint8_t *buffer,
size_t nbytes);
static int stm32_sendsetup(FAR struct sdio_dev_s *dev,
@ -456,8 +468,6 @@ static int stm32_recvlong(FAR struct sdio_dev_s *dev, uint32_t cmd,
uint32_t rlong[4]);
static int stm32_recvshort(FAR struct sdio_dev_s *dev, uint32_t cmd,
uint32_t *rshort);
static int stm32_recvnotimpl(FAR struct sdio_dev_s *dev, uint32_t cmd,
uint32_t *rnotimpl);
/* EVENT handler */
@ -507,7 +517,7 @@ struct stm32_dev_s g_sdiodev =
.attach = stm32_attach,
.sendcmd = stm32_sendcmd,
#ifdef CONFIG_SDIO_BLOCKSETUP
.blocksetup = stm32_blocksetup, /* Not implemented yet */
.blocksetup = stm32_blocksetup,
#endif
.recvsetup = stm32_recvsetup,
.sendsetup = stm32_sendsetup,
@ -516,8 +526,8 @@ struct stm32_dev_s g_sdiodev =
.recvR1 = stm32_recvshortcrc,
.recvR2 = stm32_recvlong,
.recvR3 = stm32_recvshort,
.recvR4 = stm32_recvnotimpl,
.recvR5 = stm32_recvnotimpl,
.recvR4 = stm32_recvshort,
.recvR5 = stm32_recvshortcrc,
.recvR6 = stm32_recvshortcrc,
.recvR7 = stm32_recvshort,
.waitenable = stm32_waitenable,
@ -1015,8 +1025,11 @@ static void stm32_dataconfig(uint32_t timeout, uint32_t dlen, uint32_t dctrl)
regval = getreg32(STM32_SDIO_DCTRL);
regval &= ~(SDIO_DCTRL_DTDIR | SDIO_DCTRL_DTMODE | SDIO_DCTRL_DBLOCKSIZE_MASK);
dctrl &= (SDIO_DCTRL_DTDIR | SDIO_DCTRL_DTMODE | SDIO_DCTRL_DBLOCKSIZE_MASK);
regval |= (dctrl | SDIO_DCTRL_DTEN);
regval |= (dctrl | SDIO_DCTRL_DTEN | SDIO_DCTRL_SDIOEN);
putreg32(regval, STM32_SDIO_DCTRL);
struct stm32_dev_s *priv = &g_sdiodev;
// mcinfo("data cfg: %08x %08x %08x (bs=%d)\n", timeout, dlen, regval, 1<<priv->block_size);
}
/****************************************************************************
@ -1856,7 +1869,7 @@ static int stm32_sendcmd(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t arg)
cmdidx = (cmd & MMCSD_CMDIDX_MASK) >> MMCSD_CMDIDX_SHIFT;
regval |= cmdidx | SDIO_CMD_CPSMEN;
mcinfo("cmd: %08x arg: %08x regval: %08x\n", cmd, arg, regval);
// mcinfo("cmd: %08x arg: %08x regval: %08x\n", cmd, arg, regval);
/* Write the SDIO CMD */
@ -1865,6 +1878,34 @@ static int stm32_sendcmd(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t arg)
return OK;
}
/****************************************************************************
* Name: stm32_blocksetup
*
* Description:
* Configure block size and the number of blocks for next transfer
*
* Input Parameters:
* dev - An instance of the SDIO device interface
* blocklen - The selected block size.
* nblocklen - The number of blocks to transfer
*
* Returned Value:
* None
*
****************************************************************************/
#ifdef CONFIG_SDIO_BLOCKSETUP
static void stm32_blocksetup(FAR struct sdio_dev_s *dev,
unsigned int blocklen, unsigned int nblocks)
{
struct stm32_dev_s *priv = (struct stm32_dev_s *)dev;
/* Configure block size for next transfer */
priv->block_size = stm32_log2(blocklen);
}
#endif
/****************************************************************************
* Name: stm32_recvsetup
*
@ -1911,7 +1952,17 @@ static int stm32_recvsetup(FAR struct sdio_dev_s *dev, FAR uint8_t *buffer,
/* Then set up the SDIO data path */
dblocksize = stm32_log2(nbytes) << SDIO_DCTRL_DBLOCKSIZE_SHIFT;
#ifdef CONFIG_SDIO_BLOCKSETUP
if (priv->block_size != STM32_SDIO_USE_DEFAULT_BLOCKSIZE)
{
dblocksize = priv->block_size << SDIO_DCTRL_DBLOCKSIZE_SHIFT;
}
else
#endif
{
dblocksize = stm32_log2(nbytes) << SDIO_DCTRL_DBLOCKSIZE_SHIFT;
}
stm32_dataconfig(SDIO_DTIMER_DATATIMEOUT, nbytes, dblocksize | SDIO_DCTRL_DTDIR);
/* And enable interrupts */
@ -1965,7 +2016,17 @@ static int stm32_sendsetup(FAR struct sdio_dev_s *dev, FAR const uint8_t *buffer
/* Then set up the SDIO data path */
dblocksize = stm32_log2(nbytes) << SDIO_DCTRL_DBLOCKSIZE_SHIFT;
#ifdef CONFIG_SDIO_BLOCKSETUP
if (priv->block_size != STM32_SDIO_USE_DEFAULT_BLOCKSIZE)
{
dblocksize = priv->block_size << SDIO_DCTRL_DBLOCKSIZE_SHIFT;
}
else
#endif
{
dblocksize = stm32_log2(nbytes) << SDIO_DCTRL_DBLOCKSIZE_SHIFT;
}
stm32_dataconfig(SDIO_DTIMER_DATATIMEOUT, nbytes, dblocksize);
/* Enable TX interrupts */
@ -2061,15 +2122,13 @@ static int stm32_waitresponse(FAR struct sdio_dev_s *dev, uint32_t cmd)
case MMCSD_R1_RESPONSE:
case MMCSD_R1B_RESPONSE:
case MMCSD_R2_RESPONSE:
case MMCSD_R4_RESPONSE:
case MMCSD_R5_RESPONSE:
case MMCSD_R6_RESPONSE:
events = SDIO_RESPDONE_STA;
timeout = SDIO_LONGTIMEOUT;
break;
case MMCSD_R4_RESPONSE:
case MMCSD_R5_RESPONSE:
return -ENOSYS;
case MMCSD_R3_RESPONSE:
case MMCSD_R7_RESPONSE:
events = SDIO_RESPDONE_STA;
@ -2161,6 +2220,7 @@ static int stm32_recvshortcrc(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t
else if ((cmd & MMCSD_RESPONSE_MASK) != MMCSD_R1_RESPONSE &&
(cmd & MMCSD_RESPONSE_MASK) != MMCSD_R1B_RESPONSE &&
(cmd & MMCSD_RESPONSE_MASK) != MMCSD_R5_RESPONSE &&
(cmd & MMCSD_RESPONSE_MASK) != MMCSD_R6_RESPONSE)
{
mcerr("ERROR: Wrong response CMD=%08x\n", cmd);
@ -2201,6 +2261,7 @@ static int stm32_recvshortcrc(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t
putreg32(SDIO_RESPDONE_ICR | SDIO_CMDDONE_ICR, STM32_SDIO_ICR);
*rshort = getreg32(STM32_SDIO_RESP1);
// mcinfo("data: %08x %08x\n", *rshort, respcmd);
return ret;
}
@ -2276,6 +2337,7 @@ static int stm32_recvshort(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t *r
#ifdef CONFIG_DEBUG_MEMCARD_INFO
if ((cmd & MMCSD_RESPONSE_MASK) != MMCSD_R3_RESPONSE &&
(cmd & MMCSD_RESPONSE_MASK) != MMCSD_R4_RESPONSE &&
(cmd & MMCSD_RESPONSE_MASK) != MMCSD_R7_RESPONSE)
{
mcerr("ERROR: Wrong response CMD=%08x\n", cmd);
@ -2300,18 +2362,12 @@ static int stm32_recvshort(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t *r
if (rshort)
{
*rshort = getreg32(STM32_SDIO_RESP1);
// mcinfo("data: %08x\n", *rshort);
}
return ret;
}
/* MMC responses not supported */
static int stm32_recvnotimpl(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t *rnotimpl)
{
putreg32(SDIO_RESPDONE_ICR | SDIO_CMDDONE_ICR, STM32_SDIO_ICR);
return -ENOSYS;
}
/****************************************************************************
* Name: stm32_waitenable
*

1360
config_wlan Normal file

File diff suppressed because it is too large Load Diff

View File

@ -61,6 +61,16 @@ SECTIONS
*(.text .text.*)
*(.fixup)
*(.gnu.warning)
. = ALIGN(0x4);
wlan_firmware_image_location = .;
*(.wlan_firmware_image .wlan_firmware_image.*)
wlan_firmware_image_end = .;
. = ALIGN(0x4);
wlan_nvram_image_location = .;
*(.wlan_nvram_image .wlan_nvram_image.*)
wlan_nvram_image_end = .;
*(.rodata .rodata.*)
*(.gnu.linkonce.t.*)
*(.glue_7)

View File

@ -61,6 +61,7 @@ endif
ifeq ($(CONFIG_PHOTON_WLAN),y)
CSRCS += stm32_wlan.c
CSRCS += stm32_wlan_firmware.c
endif
ifeq ($(CONFIG_STM32_OTGHS),y)

View File

@ -98,6 +98,24 @@ void bcmf_board_initialize(int minor)
bcmf_board_reset(minor, true);
}
/****************************************************************************
* Name: bcmf_board_setup_oob_irq
****************************************************************************/
void bcmf_board_setup_oob_irq(int minor, xcpt_t func, void *arg)
{
if (minor != SDIO_WLAN0_MINOR)
{
return;
}
/* Configure interrupt pin */
stm32_configgpio(GPIO_WLAN0_OOB_INT);
stm32_gpiosetevent(GPIO_WLAN0_OOB_INT, true, false, false, func, arg);
}
/****************************************************************************
* Name: photon_wlan_initialize
****************************************************************************/

File diff suppressed because it is too large Load Diff

View File

@ -8,6 +8,11 @@ if DRIVERS_IEEE80211
config IEEE80211_BROADCOM_FULLMAC
bool
config IEEE80211_BROADCOM_BCM43362
bool "Broadcom 43362 chip support"
depends on IEEE80211_BROADCOM_FULLMAC
default n
config IEEE80211_BROADCOM_FULLMAC_SDIO
bool "Broadcom FullMAC driver on SDIO bus"
depends on ARCH_HAVE_SDIO

View File

@ -41,7 +41,16 @@ ifeq ($(CONFIG_DRIVERS_IEEE80211),y)
# Include IEEE 802.11 drivers into the build
ifeq ($(CONFIG_IEEE80211_BROADCOM_FULLMAC_SDIO),y)
CSRCS += bcmf_driver.c
CSRCS += bcmf_sdio.c
CSRCS += bcmf_core.c
CSRCS += bcmf_sdpcm.c
CSRCS += bcmf_hexdump.c
CSRCS += mmc_sdio.c
endif
ifeq ($(CONFIG_IEEE80211_BROADCOM_BCM43362),y)
CSRCS += bcmf_chip_43362.c
endif
# Include IEEE 802.11 build support

View File

@ -0,0 +1,76 @@
/*
* Copyright (c) 2015 Broadcom
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* 3. Neither the name of Broadcom nor the names of other contributors to this
* software may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* 4. This software may not be used as a standalone product, and may only be used as
* incorporated in your product or device that incorporates Broadcom wireless connectivity
* products and solely for the purpose of enabling the functionalities of such Broadcom products.
*
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT, ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
* ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef BCM43362_CONSTANTS_H_
#define BCM43362_CONSTANTS_H_
/******************************************************
* Architecture Constants
******************************************************/
/* General chip stats */
#define CHIP_RAM_SIZE 0x3C000
/* Backplane architecture */
#define CHIPCOMMON_BASE_ADDRESS 0x18000000 /* Chipcommon core register region */
#define DOT11MAC_BASE_ADDRESS 0x18001000 /* dot11mac core register region */
#define SDIO_BASE_ADDRESS 0x18002000 /* SDIOD Device core register region */
#define WLAN_ARMCM3_BASE_ADDRESS 0x18003000 /* ARMCM3 core register region */
#define SOCSRAM_BASE_ADDRESS 0x18004000 /* SOCSRAM core register region */
#define BACKPLANE_ADDRESS_MASK 0x7FFF
/* Maximum value of bus data credit difference */
#define CHIP_MAX_BUS_DATA_CREDIT_DIFF 7
/* Chipcommon registers */
#define CHIPCOMMON_GPIO_CONTROL ((uint32_t) (CHIPCOMMON_BASE_ADDRESS + 0x6C) )
/******************************************************
* Bit Masks
******************************************************/
#define WL_CHANSPEC_BAND_MASK (0xf000)
#define WL_CHANSPEC_BAND_5G (0x1000)
#define WL_CHANSPEC_BAND_2G (0x2000)
#define WL_CHANSPEC_CTL_SB_MASK (0x0300)
#define WL_CHANSPEC_CTL_SB_LOWER (0x0100)
#define WL_CHANSPEC_CTL_SB_UPPER (0x0200)
#define WL_CHANSPEC_CTL_SB_NONE (0x0300)
#define WL_CHANSPEC_BW_MASK (0x0C00)
#define WL_CHANSPEC_BW_10 (0x0400)
#define WL_CHANSPEC_BW_20 (0x0800)
#define WL_CHANSPEC_BW_40 (0x0C00)
#endif /* ifndef BCM43362_CONSTANTS_H_ */

View File

@ -0,0 +1,28 @@
#include "bcmf_chip_43362.h"
#include "bcm43362_constants.h"
#include <stdint.h>
#include <debug.h>
#include "bcmf_sdio_core.h"
#define WRAPPER_REGISTER_OFFSET (0x100000)
uint32_t bcmf_43362_get_core_base_address(unsigned int core)
{
switch (core)
{
case CHIPCOMMON_CORE_ID:
return CHIPCOMMON_BASE_ADDRESS;
case DOT11MAC_CORE_ID:
return DOT11MAC_BASE_ADDRESS;
case SDIOD_CORE_ID:
return SDIO_BASE_ADDRESS;
case WLAN_ARMCM3_CORE_ID:
return WLAN_ARMCM3_BASE_ADDRESS + WRAPPER_REGISTER_OFFSET;
case SOCSRAM_CORE_ID:
return SOCSRAM_BASE_ADDRESS + WRAPPER_REGISTER_OFFSET;
default:
_err("Invalid core id %d\n", core);
}
return 0;
}

View File

@ -0,0 +1,8 @@
#ifndef __DRIVERS_WIRELESS_IEEE80211_BCM_CHIP_43362_H
#define __DRIVERS_WIRELESS_IEEE80211_BCM_CHIP_43362_H
#include <stdint.h>
uint32_t bcmf_43362_get_core_base_address(unsigned int core);
#endif /* __DRIVERS_WIRELESS_IEEE80211_BCM_CHIP_43362_H */

View File

@ -0,0 +1,390 @@
/****************************************************************************
* drivers/wireless/ieee80211/bcmf_core.c
*
* Copyright (C) 2017 Gregory Nutt. All rights reserved.
* Author: Simon Piriou <spiriou31@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/compiler.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/arch.h>
#include "bcmf_core.h"
#include "bcmf_sdio.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Agent registers (common for every core) */
#define BCMA_IOCTL 0x0408 /* IO control */
#define BCMA_IOST 0x0500 /* IO status */
#define BCMA_RESET_CTL 0x0800 /* Reset control */
#define BCMA_RESET_ST 0x0804
#define BCMA_IOCTL_CLK 0x0001
#define BCMA_IOCTL_FGC 0x0002
#define BCMA_IOCTL_CORE_BITS 0x3FFC
#define BCMA_IOCTL_PME_EN 0x4000
#define BCMA_IOCTL_BIST_EN 0x8000
#define BCMA_IOST_CORE_BITS 0x0FFF
#define BCMA_IOST_DMA64 0x1000
#define BCMA_IOST_GATED_CLK 0x2000
#define BCMA_IOST_BIST_ERROR 0x4000
#define BCMA_IOST_BIST_DONE 0x8000
#define BCMA_RESET_CTL_RESET 0x0001
/* Transfer size properties */
#define BCMF_UPLOAD_TRANSFER_SIZE (64 * 256)
// TODO move in chip configuration data
#define CHIP_RAM_SIZE 0x3C000
extern const char bcmf_nvram_image[];
extern const unsigned int bcmf_nvram_image_len;
extern const uint8_t bcmf_firmware_image[];
extern const unsigned int bcmf_firmware_image_len;
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int bcmf_core_set_backplane_window(FAR struct bcmf_dev_s *priv,
uint32_t address);
static int bcmf_upload_binary(FAR struct bcmf_dev_s *priv,
uint32_t address, uint8_t *buf,
unsigned int len);
static int bcmf_upload_nvram(FAR struct bcmf_dev_s *priv);
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
int bcmf_core_set_backplane_window(FAR struct bcmf_dev_s *priv, uint32_t address)
{
int ret;
int i;
address &= ~SBSDIO_SB_OFT_ADDR_MASK;
for (i = 1; i < 4; i++)
{
uint8_t addr_part = (address >> (8*i)) & 0xff;
uint8_t cur_addr_part = (priv->backplane_current_addr >> (8*i)) & 0xff;
if (addr_part != cur_addr_part)
{
/* Update current backplane base address */
ret = bcmf_write_reg(priv, 1, SBSDIO_FUNC1_SBADDRLOW+i-1,
addr_part);
if (ret != OK)
{
return ret;
}
priv->backplane_current_addr &= ~(0xff << (8*i));
priv->backplane_current_addr |= addr_part << (8*i);
}
}
return OK;
}
int bcmf_upload_binary(FAR struct bcmf_dev_s *priv, uint32_t address,
uint8_t *buf, unsigned int len)
{
unsigned int size;
while (len > 0)
{
/* Set the backplane window to include the start address */
int ret = bcmf_core_set_backplane_window(priv, address);
if (ret != OK)
{
return ret;
}
if (len > BCMF_UPLOAD_TRANSFER_SIZE)
{
size = BCMF_UPLOAD_TRANSFER_SIZE;
}
else
{
size = len;
}
/* Transfer firmware data */
ret = bcmf_transfer_bytes(priv, true, 1,
address & SBSDIO_SB_OFT_ADDR_MASK, buf, size);
if (ret != OK)
{
_err("transfer failed %d %x %d\n", ret, address, size);
return ret;
}
len -= size;
address += size;
buf += size;
}
return OK;
}
int bcmf_upload_nvram(FAR struct bcmf_dev_s *priv)
{
int ret;
uint32_t nvram_sz;
uint32_t token;
/* Round up the size of the image */
nvram_sz = (bcmf_nvram_image_len + 63) & (-64);
_info("nvram size is %d %d bytes\n", nvram_sz, bcmf_nvram_image_len);
/* Write image */
ret = bcmf_upload_binary(priv, CHIP_RAM_SIZE - 4 - nvram_sz,
(uint8_t*)bcmf_nvram_image, bcmf_nvram_image_len);
if ( ret != OK)
{
return ret;
}
/* generate length token */
token = nvram_sz / 4;
token = (~token << 16) | (token & 0x0000FFFF);
/* Write the length token to the last word */
ret = bcmf_write_sbreg(priv, CHIP_RAM_SIZE - 4, (uint8_t*)&token, 4);
if ( ret != OK)
{
return ret;
}
return OK;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: bcmf_read_sbreg
****************************************************************************/
int bcmf_read_sbreg(FAR struct bcmf_dev_s *priv, uint32_t address,
uint8_t *reg, unsigned int len)
{
int ret = bcmf_core_set_backplane_window(priv, address);
if (ret != OK)
{
return ret;
}
return bcmf_transfer_bytes(priv, false, 1,
address & SBSDIO_SB_OFT_ADDR_MASK, reg, len);
}
/****************************************************************************
* Name: bcmf_write_sbreg
****************************************************************************/
int bcmf_write_sbreg(FAR struct bcmf_dev_s *priv, uint32_t address,
uint8_t *reg, unsigned int len)
{
int ret = bcmf_core_set_backplane_window(priv, address);
if (ret != OK)
{
return ret;
}
return bcmf_transfer_bytes(priv, true, 1, address & SBSDIO_SB_OFT_ADDR_MASK,
reg, len);
}
/****************************************************************************
* Name: bcmf_core_upload_firmware
****************************************************************************/
int bcmf_core_upload_firmware(FAR struct bcmf_dev_s *priv)
{
int ret;
_info("upload firmware\n");
/* Disable ARMCM3 core and reset SOCRAM core
* to set device in firmware upload mode */
bcmf_core_disable(priv, WLAN_ARMCM3_CORE_ID);
bcmf_core_reset(priv, SOCSRAM_CORE_ID);
up_mdelay(50);
/* Flash chip firmware */
_info("firmware size is %d bytes\n", bcmf_firmware_image_len);
ret = bcmf_upload_binary(priv, 0, (uint8_t*)bcmf_firmware_image,
bcmf_firmware_image_len);
if (ret != OK)
{
_err("Failed to upload firmware\n");
return ret;
}
/* Flash NVRAM configuration file */
_info("upload nvram configuration\n");
ret = bcmf_upload_nvram(priv);
if (ret != OK)
{
_err("Failed to upload nvram\n");
return ret;
}
/* Firmware upload done, restart ARMCM3 core */
up_mdelay(10);
bcmf_core_reset(priv, WLAN_ARMCM3_CORE_ID);
/* Check ARMCM3 core is running */
up_mdelay(10);
if (!bcmf_core_isup(priv, WLAN_ARMCM3_CORE_ID))
{
_err("Cannot start ARMCM3 core\n");
return -ETIMEDOUT;
}
return OK;
}
bool bcmf_core_isup(FAR struct bcmf_dev_s *priv, unsigned int core)
{
uint32_t value = 0;
uint32_t base = priv->get_core_base_address(core);
bcmf_read_sbregw(priv, base + BCMA_IOCTL, &value);
if ((value & (BCMA_IOCTL_FGC | BCMA_IOCTL_CLK)) != BCMA_IOCTL_CLK)
{
return false;
}
bcmf_read_sbregw(priv, base + BCMA_RESET_CTL, &value);
return (value & BCMA_RESET_CTL_RESET) == 0;
}
void bcmf_core_disable(FAR struct bcmf_dev_s *priv, unsigned int core)
{
uint8_t value;
uint32_t base = priv->get_core_base_address(core);
/* Check if core is already in reset state */
bcmf_read_sbregb(priv, base + BCMA_RESET_CTL, &value);
if ((value & BCMA_RESET_CTL_RESET) != 0)
{
/* Core already disabled */
return;
}
/* Ensure no backplane operation is pending */
up_mdelay(10);
/* Set core in reset state */
bcmf_write_sbregb(priv, base + BCMA_RESET_CTL, BCMA_RESET_CTL_RESET);
up_udelay(1);
/* Write 0 to the IO control and read it back */
bcmf_write_sbregb(priv, base + BCMA_IOCTL, 0);
bcmf_read_sbregb(priv, base + BCMA_IOCTL, &value);
up_udelay(10);
}
void bcmf_core_reset(FAR struct bcmf_dev_s *priv, unsigned int core)
{
uint32_t value;
uint32_t base = priv->get_core_base_address(core);
/* Put core in reset state */
bcmf_core_disable(priv, core);
/* Run initialization sequence */
bcmf_write_sbregb(priv, base + BCMA_IOCTL, BCMA_IOCTL_FGC | BCMA_IOCTL_CLK);
bcmf_read_sbregw(priv, base + BCMA_IOCTL, &value);
bcmf_write_sbregb(priv, base + BCMA_RESET_CTL, 0);
bcmf_read_sbregw(priv, base + BCMA_RESET_CTL, &value);
up_udelay(1);
bcmf_write_sbregb(priv, base + BCMA_IOCTL, BCMA_IOCTL_CLK);
bcmf_read_sbregw(priv, base + BCMA_IOCTL, &value);
up_udelay(1);
}

View File

@ -0,0 +1,83 @@
/****************************************************************************
* drivers/wireless/ieee80211/bcmf_core.h
*
* Copyright (C) 2017 Gregory Nutt. All rights reserved.
* Author: Simon Piriou <spiriou31@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef __DRIVERS_WIRELESS_IEEE80211_BCMF_CORE_H
#define __DRIVERS_WIRELESS_IEEE80211_BCMF_CORE_H
#include <stdint.h>
#include <stdbool.h>
#include "bcmf_driver.h"
int bcmf_read_sbreg(FAR struct bcmf_dev_s *priv, uint32_t address,
uint8_t *reg, unsigned int len);
int bcmf_write_sbreg(FAR struct bcmf_dev_s *priv, uint32_t address,
uint8_t *reg, unsigned int len);
bool bcmf_core_isup(FAR struct bcmf_dev_s *priv, unsigned int core);
void bcmf_core_disable(FAR struct bcmf_dev_s *priv, unsigned int core);
void bcmf_core_reset(FAR struct bcmf_dev_s *priv, unsigned int core);
int bcmf_core_upload_firmware(FAR struct bcmf_dev_s *priv);
static inline int bcmf_read_sbregb(FAR struct bcmf_dev_s *priv,
uint32_t address, uint8_t *reg)
{
return bcmf_read_sbreg(priv, address, reg, 1);
}
static inline int bcmf_read_sbregw(FAR struct bcmf_dev_s *priv,
uint32_t address, uint32_t *reg)
{
return bcmf_read_sbreg(priv, address, (uint8_t*)reg, 4);
}
static inline int bcmf_write_sbregb(FAR struct bcmf_dev_s *priv,
uint32_t address, uint8_t reg)
{
return bcmf_write_sbreg(priv, address, &reg, 1);
}
static inline int bcmf_write_sbregw(FAR struct bcmf_dev_s *priv,
uint32_t address, uint32_t reg)
{
return bcmf_write_sbreg(priv, address, (uint8_t*)&reg, 4);
}
#endif /* __DRIVERS_WIRELESS_IEEE80211_BCMF_CORE_H */

View File

@ -0,0 +1,346 @@
/****************************************************************************
* drivers/wireless/ieee80211/bcmf_driver.c
*
* Copyright (C) 2017 Gregory Nutt. All rights reserved.
* Author: Simon Piriou <spiriou31@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/compiler.h>
#include <stdint.h>
#include <string.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/kmalloc.h>
#include "bcmf_driver.h"
#include "bcmf_sdpcm.h"
#include "bcmf_sdio_core.h"
#include "bcmf_ioctl.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
// TODO move elsewhere
#define DOT11_BSSTYPE_ANY 2
#define WL_SCAN_CHANNEL_TIME 40
#define WL_SCAN_UNASSOC_TIME 40
#define WL_SCAN_PASSIVE_TIME 120
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int bcmf_run_escan(FAR struct bcmf_dev_s *priv);
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
int bcmf_wl_set_mac_address(FAR struct bcmf_dev_s *priv, uint8_t *addr)
{
int ret;
uint32_t out_len = 6;
ret = bcmf_sdpcm_iovar_request(priv, CHIP_STA_INTERFACE, true,
IOVAR_STR_CUR_ETHERADDR, addr,
&out_len);
if (ret != OK)
{
return ret;
}
_info("MAC address updated %02X:%02X:%02X:%02X:%02X:%02X\n",
addr[0], addr[1], addr[2],
addr[3], addr[4], addr[5]);
memcpy(priv->mac_addr, addr, 6);
return OK;
}
int bcmf_wl_enable(FAR struct bcmf_dev_s *priv, bool enable)
{
int ret;
uint32_t out_len;
/* TODO chek device state */
out_len = 0;
ret = bcmf_sdpcm_ioctl(priv, CHIP_STA_INTERFACE, true,
enable ? WLC_UP : WLC_DOWN, NULL, &out_len);
if (ret == OK)
{
/* TODO update device state */
}
return ret;
}
int bcmf_dongle_scantime(FAR struct bcmf_dev_s *priv, int32_t scan_assoc_time,
int32_t scan_unassoc_time, int32_t scan_passive_time)
{
int ret;
uint32_t out_len, value;
out_len = 4;
value = scan_assoc_time;
ret = bcmf_sdpcm_ioctl(priv, CHIP_STA_INTERFACE, true,
WLC_SET_SCAN_CHANNEL_TIME, (uint8_t*)&value,
&out_len);
if (ret != OK)
{
return -EIO;
}
out_len = 4;
value = scan_unassoc_time;
ret = bcmf_sdpcm_ioctl(priv, CHIP_STA_INTERFACE, true,
WLC_SET_SCAN_UNASSOC_TIME, (uint8_t*)&value,
&out_len);
if (ret != OK)
{
return -EIO;
}
out_len = 4;
value = scan_passive_time;
ret = bcmf_sdpcm_ioctl(priv, CHIP_STA_INTERFACE, true,
WLC_SET_SCAN_PASSIVE_TIME, (uint8_t*)&value,
&out_len);
if (ret != OK)
{
return -EIO;
}
return OK;
}
int bcmf_dongle_initialize(FAR struct bcmf_dev_s *priv)
{
int ret;
uint32_t out_len;
uint32_t value;
ret = bcmf_wl_enable(priv, true);
if (ret)
{
return ret;
}
ret = bcmf_dongle_scantime(priv, WL_SCAN_CHANNEL_TIME,
WL_SCAN_UNASSOC_TIME, WL_SCAN_PASSIVE_TIME);
if (ret)
{
return ret;
}
/* FIXME disable power save mode */
out_len = 4;
value = 0;
ret = bcmf_sdpcm_ioctl(priv, CHIP_STA_INTERFACE, true,
WLC_SET_PM, (uint8_t*)&value, &out_len);
if (ret != OK)
{
return ret;
}
/* Set the GMode */
out_len = 4;
value = GMODE_AUTO;
ret = bcmf_sdpcm_ioctl(priv, CHIP_STA_INTERFACE, true,
WLC_SET_GMODE, (uint8_t*)&value, &out_len);
if (ret != OK)
{
return ret;
}
/* TODO configure roaming if needed. Disable for now */
out_len = 4;
value = 1;
ret = bcmf_sdpcm_iovar_request(priv, CHIP_STA_INTERFACE, true,
IOVAR_STR_ROAM_OFF, (uint8_t*)&value,
&out_len);
// FIXME remove
/* Try scan */
value = 0;
out_len = 4;
ret = bcmf_sdpcm_ioctl(priv, CHIP_STA_INTERFACE, true,
WLC_SET_PASSIVE_SCAN, (uint8_t*)&value, &out_len);
bcmf_run_escan(priv);
return OK;
}
int bcmf_run_escan(FAR struct bcmf_dev_s *priv)
{
int ret;
uint32_t out_len;
/* Default request structure */
struct wl_escan_params *params =
(struct wl_escan_params*)kmm_malloc(sizeof(*params));
if (!params)
{
return -ENOMEM;
}
memset(params, 0, sizeof(*params));
params->version = ESCAN_REQ_VERSION;
params->action = WL_SCAN_ACTION_START;
params->sync_id = 0x1234;
memset(&params->params.bssid, 0xFF, sizeof(params->params.bssid));
params->params.bss_type = DOT11_BSSTYPE_ANY;
params->params.scan_type = 0; /* Active scan */
params->params.nprobes = -1;
params->params.active_time = -1;
params->params.passive_time = -1;
params->params.home_time = -1;
params->params.channel_num = 0;
_info("start scan\n");
out_len = sizeof(*params);
ret = bcmf_sdpcm_iovar_request(priv, CHIP_STA_INTERFACE, true,
IOVAR_STR_ESCAN, (uint8_t*)params,
&out_len);
free(params);
if (ret != OK)
{
return -EIO;
}
return OK;
}
/****************************************************************************
* Public Functions
****************************************************************************/
int bcmf_wl_initialize(FAR struct bcmf_dev_s *priv)
{
int ret;
uint32_t out_len;
uint8_t tmp_buf[64];
/* Disable TX Gloming feature */
out_len = 4;
*(uint32_t*)tmp_buf = 0;
ret = bcmf_sdpcm_iovar_request(priv, CHIP_STA_INTERFACE, false,
IOVAR_STR_TX_GLOM, tmp_buf,
&out_len);
if (ret != OK)
{
return -EIO;
}
/* Query MAC address */
out_len = 6;
ret = bcmf_sdpcm_iovar_request(priv, CHIP_STA_INTERFACE, false,
IOVAR_STR_CUR_ETHERADDR, tmp_buf,
&out_len);
if (ret != OK)
{
return -EIO;
}
memcpy(priv->mac_addr, tmp_buf, 6);
_info("MAC address is %02X:%02X:%02X:%02X:%02X:%02X\n",
tmp_buf[0], tmp_buf[1], tmp_buf[2],
tmp_buf[3], tmp_buf[4], tmp_buf[5]);
/* Query firmware version string */
out_len = sizeof(tmp_buf);
ret = bcmf_sdpcm_iovar_request(priv, CHIP_STA_INTERFACE, false,
IOVAR_STR_VERSION, tmp_buf,
&out_len);
if (ret != OK)
{
return -EIO;
}
/* Remove line feed */
out_len = strlen((char*)tmp_buf);
if (out_len > 0 && tmp_buf[out_len-1] == '\n')
{
tmp_buf[out_len-1] = 0;
}
_info("fw version <%s>\n", tmp_buf);
/* FIXME Configure event mask to enable all asynchronous events */
uint8_t event_mask[16];
memset(event_mask, 0xff, sizeof(event_mask));
out_len = sizeof(event_mask);
ret = bcmf_sdpcm_iovar_request(priv, CHIP_STA_INTERFACE, true,
IOVAR_STR_EVENT_MSGS, event_mask,
&out_len);
if (ret != OK)
{
return -EIO;
}
// TODO Create a wlan device name and register network driver
return bcmf_dongle_initialize(priv);
}

View File

@ -0,0 +1,85 @@
/****************************************************************************
* drivers/wireless/ieee80211/bcmf_driver.h
*
* Copyright (C) 2017 Gregory Nutt. All rights reserved.
* Author: Simon Piriou <spiriou31@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef __DRIVERS_WIRELESS_IEEE80211_BCMF_DRIVER_H
#define __DRIVERS_WIRELESS_IEEE80211_BCMF_DRIVER_H
#include <stdbool.h>
#include <nuttx/sdio.h>
#define BCMF_STATUS_BUS_UP (1<<0) /* Chip is flashed and running */
#define BCMF_STATUS_READY (1<<1) /* Chip is ready to receive requests */
#define BCMF_STATUS_SLEEP (1<<2) /* Chip is in low power mode */
#define BCMF_STATUS_WAIT_CONTROL (1<<3) /* Waiting for control response */
/* This structure contains the unique state of the Broadcom FullMAC driver */
struct bcmf_dev_s
{
FAR struct sdio_dev_s *sdio_dev; /* The SDIO device bound to this instance */
int minor; /* Device minor number */
uint32_t backplane_current_addr; /* Current function 1 backplane base addr */
uint32_t (*get_core_base_address)(unsigned int core); /* Get chip specific
base address for evey cores */
sem_t thread_signal; /* Semaphore for processing thread event */
struct wdog_s *waitdog; /* Processing thread waitdog */
bool ready; /* Current device status */
bool sleeping; /* Current sleep status */
volatile bool irq_pending; /* True if interrupt is pending */
uint32_t intstatus; /* Copy of device current interrupt status */
uint8_t max_seq; /* Maximum transmit sequence allowed */
uint8_t tx_seq; /* Transmit sequence number (next) */
uint8_t rx_seq; /* Receive sequence number (expected) */
// FIXME use mutex instead of semaphore
sem_t control_mutex; /* Cannot handle multiple control requests */
sem_t control_timeout; /* Semaphore to wait for control frame rsp */
uint16_t control_reqid; /* Current control request id */
uint8_t *control_rxframe; /* Received control frame response */
uint32_t control_status; /* Last received frame status */
// FIXME use mutex instead of semaphore
sem_t tx_queue_mutex; /* Lock for transmit queue */
dq_queue_t tx_queue; /* Queue of frames to tramsmit */
uint8_t mac_addr[6]; /* Current mac address */
};
#endif /* __DRIVERS_WIRELESS_IEEE80211_BCMF_DRIVER_H */

View File

@ -0,0 +1,36 @@
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <debug.h>
#define LINE_LEN 16
void bcmf_hexdump(uint8_t *data, unsigned int len, unsigned long offset)
{
unsigned int i;
unsigned int char_count = 0;
char char_line[20];
char hex_line[64];
for(i = 0; i < len; i++)
{
if (char_count >= LINE_LEN) {
/* Flush line */
_info("%08x: %s%s\n", offset+i-char_count, hex_line, char_line);
char_count = 0;
}
sprintf(hex_line+3*char_count, "%02x ", data[i]);
sprintf(char_line+char_count, "%c",
data[i] < 0x20 || data[i] >= 0x7f? '.': data[i]);
char_count ++;
}
if (char_count > 0) {
/* Flush last line */
memset(hex_line+3*char_count, ' ', 3*(LINE_LEN-char_count));
hex_line[3*LINE_LEN] = 0;
_info("%08x: %s%s\n", offset+i-char_count, hex_line, char_line);
}
}

File diff suppressed because it is too large Load Diff

View File

@ -45,44 +45,61 @@
#include <string.h>
#include <debug.h>
#include <errno.h>
#include <queue.h>
#include <nuttx/kmalloc.h>
#include <nuttx/sdio.h>
#include <nuttx/arch.h>
#include <nuttx/kthread.h>
#include <nuttx/wireless/ieee80211/mmc_sdio.h>
#include <nuttx/wireless/ieee80211/bcmf_sdio.h>
#include <nuttx/wireless/ieee80211/bcmf_board.h>
#include "bcmf_sdio.h"
#include "bcmf_core.h"
#include "bcmf_sdpcm.h"
// TODO remove
#include "bcmf_ioctl.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define BCMF_DEVICE_RESET_DELAY_MS 10
#define BCMF_DEVICE_START_DELAY_MS 10
#define BCMF_DEVICE_IDLE_DELAY_MS 50
#define BCMF_CLOCK_SETUP_DELAY_MS 500
#define BCMF_THREAD_NAME "bcmf"
#define BCMF_THREAD_STACK_SIZE 2048
#define BCMF_WAITDOG_TIMEOUT_TICK (5*CLOCKS_PER_SEC)
/****************************************************************************
* Private Types
****************************************************************************/
/* This structure is contains the unique state of the Broadcom FullMAC driver */
struct bcmf_dev_s
{
FAR struct sdio_dev_s *sdio_dev; /* The SDIO device bound to this instance */
int minor; /* Device minor number */
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int bcmf_sendcmdpoll(FAR struct bcmf_dev_s *priv,
uint32_t cmd, uint32_t arg);
static int bcmf_probe(FAR struct bcmf_dev_s *priv);
static int bcmf_hwinitialize(FAR struct bcmf_dev_s *priv);
static void bcmf_hwuninitialize(FAR struct bcmf_dev_s *priv);
static int bcmf_chipinitialize(FAR struct bcmf_dev_s *priv);
static int bcmf_oob_irq(int irq, FAR void *context, FAR void *arg);
static int bcmf_sdio_bus_sleep(FAR struct bcmf_dev_s *priv, bool sleep);
static void bcmf_sdio_waitdog_timeout(int argc, wdparm_t arg1, ...);
static int bcmf_sdio_thread(int argc, char **argv);
static int bcmf_sdio_find_block_size(unsigned int size);
/* FIXME remove */
FAR struct bcmf_dev_s *g_sdio_priv;
/****************************************************************************
* Private Data
@ -92,30 +109,78 @@ static void bcmf_hwuninitialize(FAR struct bcmf_dev_s *priv);
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: bcmf_sendcmdpoll
****************************************************************************/
int bcmf_oob_irq(int irq, FAR void *context, FAR void *arg)
{
FAR struct bcmf_dev_s *priv = (struct bcmf_dev_s*)arg;
int bcmf_sendcmdpoll(FAR struct bcmf_dev_s *priv, uint32_t cmd, uint32_t arg)
if (priv->ready)
{
/* Signal bmcf thread */
priv->irq_pending = true;
sem_post(&priv->thread_signal);
}
return OK;
}
int bcmf_sdio_bus_sleep(FAR struct bcmf_dev_s *priv, bool sleep)
{
int ret;
int loops;
uint8_t value;
/* Send the command */
ret = SDIO_SENDCMD(priv->sdio_dev, cmd, arg);
if (ret == OK)
if (priv->sleeping == sleep)
{
/* Then poll-wait until the response is available */
ret = SDIO_WAITRESPONSE(priv->sdio_dev, cmd);
if (ret != OK)
{
_err("ERROR: Wait for response to cmd: %08x failed: %d\n",
cmd, ret);
}
return OK;
}
return ret;
if (sleep)
{
priv->sleeping = true;
return bcmf_write_reg(priv, 1, SBSDIO_FUNC1_CHIPCLKCSR, 0);
}
else
{
/* Request HT Avail */
ret = bcmf_write_reg(priv, 1, SBSDIO_FUNC1_CHIPCLKCSR,
SBSDIO_HT_AVAIL_REQ | SBSDIO_FORCE_HT);
if (ret != OK)
{
_err("HT Avail request failed %d\n", ret);
return ret;
}
/* Wait for High Troughput clock */
loops = 20;
while (--loops > 0)
{
up_mdelay(1);
ret = bcmf_read_reg(priv, 1, SBSDIO_FUNC1_CHIPCLKCSR, &value);
if (ret != OK)
{
return ret;
}
if (value & SBSDIO_HT_AVAIL)
{
/* High Throughput clock is ready */
break;
}
}
if (loops <= 0)
{
_err("HT clock not ready\n");
return -ETIMEDOUT;
}
priv->sleeping = false;
}
return OK;
}
/****************************************************************************
@ -125,35 +190,54 @@ int bcmf_sendcmdpoll(FAR struct bcmf_dev_s *priv, uint32_t cmd, uint32_t arg)
int bcmf_probe(FAR struct bcmf_dev_s *priv)
{
int ret;
uint32_t data = 0;
/* Set device state from reset to idle */
bcmf_sendcmdpoll(priv, MMCSD_CMD0, 0);
up_mdelay(BCMF_DEVICE_START_DELAY_MS);
/* Send IO_SEND_OP_COND command */
ret = bcmf_sendcmdpoll(priv, SDIO_CMD5, 0);
/* Probe sdio card compatible device */
ret = sdio_probe(priv->sdio_dev);
if (ret != OK)
{
goto exit_error;
}
/* Receive R4 response */
ret = SDIO_RECVR4(priv->sdio_dev, SDIO_CMD5, &data);
/* Set FN0 / FN1 / FN2 default block size */
ret = sdio_set_blocksize(priv->sdio_dev, 0, 64);
if (ret != OK)
{
goto exit_error;
}
/* Broadcom chips have 2 additional functions and wide voltage range */
ret = sdio_set_blocksize(priv->sdio_dev, 1, 64);
if (ret != OK)
{
goto exit_error;
}
if ((((data >> 28) & 7) != 2) ||
(((data >> 8) & 0xff80) != 0xff80))
ret = sdio_set_blocksize(priv->sdio_dev, 2, 64);
if (ret != OK)
{
goto exit_error;
}
/* Enable device interrupts for FN0, FN1 and FN2 */
ret = bcmf_write_reg(priv, 0, SDIO_CCCR_INTEN,
(1 << 0) | (1 << 1) | (1 << 2));
if (ret != OK)
{
goto exit_error;
}
/* Default device clock speed is up to 25 Mhz
* We could set EHS bit to operate at a clock rate up to 50 Mhz */
SDIO_CLOCK(priv->sdio_dev, CLOCK_SD_TRANSFER_4BIT);
up_mdelay(BCMF_CLOCK_SETUP_DELAY_MS);
/* Enable bus FN1 */
ret = sdio_enable_function(priv->sdio_dev, 1);
if (ret != OK)
{
goto exit_error;
}
@ -166,6 +250,148 @@ exit_error:
return ret;
}
/****************************************************************************
* Name: bcmf_businitialize
****************************************************************************/
int bcmf_businitialize(FAR struct bcmf_dev_s *priv)
{
int ret;
int loops;
uint8_t value;
/* Send Active Low-Power clock request */
ret = bcmf_write_reg(priv, 1, SBSDIO_FUNC1_CHIPCLKCSR,
SBSDIO_FORCE_HW_CLKREQ_OFF |
SBSDIO_ALP_AVAIL_REQ |
SBSDIO_FORCE_ALP);
if (ret != OK)
{
return ret;
}
loops = 10;
while (--loops > 0)
{
up_mdelay(10);
ret = bcmf_read_reg(priv, 1, SBSDIO_FUNC1_CHIPCLKCSR, &value);
if (ret != OK)
{
return ret;
}
if (value & SBSDIO_ALP_AVAIL)
{
/* Active Low-Power clock is ready */
break;
}
}
if (loops <= 0)
{
_err("failed to enable ALP\n");
return -ETIMEDOUT;
}
/* Clear Active Low-Power clock request */
ret = bcmf_write_reg(priv, 1, SBSDIO_FUNC1_CHIPCLKCSR, 0);
if (ret != OK)
{
return ret;
}
/* Disable pull-ups on SDIO cmd, d0-2 lines */
ret = bcmf_write_reg(priv, 1, SBSDIO_FUNC1_SDIOPULLUP, 0);
if (ret != OK)
{
return ret;
}
/* Do chip specific initialization */
ret = bcmf_chipinitialize(priv);
if (ret != OK)
{
return ret;
}
/* Upload firmware */
ret = bcmf_core_upload_firmware(priv);
if (ret != OK)
{
return ret;
}
/* Enable FN2 (frame transfers) */
ret = sdio_enable_function(priv->sdio_dev, 2);
if (ret != OK)
{
return ret;
}
return OK;
}
int bcmf_bus_setup_interrupts(FAR struct bcmf_dev_s *priv)
{
int ret;
/* Configure gpio interrupt pin */
bcmf_board_setup_oob_irq(priv->minor, bcmf_oob_irq, (void*)priv);
/* Enable function 2 interrupt */
ret = sdio_enable_interrupt(priv->sdio_dev, 0);
if (ret != OK)
{
return ret;
}
ret = sdio_enable_interrupt(priv->sdio_dev, 2);
if (ret != OK)
{
return ret;
}
/* Redirect, configure and enable io for out-of-band interrupt signal */
ret = bcmf_write_reg(priv, 0, SDIO_CCCR_BRCM_SEPINT,
SDIO_SEPINT_MASK | SDIO_SEPINT_OE | SDIO_SEPINT_ACT_HI);
if (ret != OK)
{
return ret;
}
/* Wake up chip to be sure function 2 is running */
ret = bcmf_sdio_bus_sleep(priv, false);
if (ret != OK)
{
return ret;
}
/* FN2 successfully enabled, set core and enable interrupts */
bcmf_write_sbregw(priv,
CORE_BUS_REG(priv->get_core_base_address(SDIOD_CORE_ID),
hostintmask), I_HMB_SW_MASK);
bcmf_write_sbregb(priv,
CORE_BUS_REG(priv->get_core_base_address(SDIOD_CORE_ID),
funcintmask), 2);
bcmf_write_reg(priv, 1, SBSDIO_WATERMARK, 8);
return OK;
}
/****************************************************************************
* Name: bcmf_hwinitialize
****************************************************************************/
@ -210,10 +436,120 @@ void bcmf_hwuninitialize(FAR struct bcmf_dev_s *priv)
bcmf_board_reset(priv->minor, true);
}
int bcmf_sdio_find_block_size(unsigned int size)
{
int ret = 0;
int size_copy = size;
while (size_copy) {
size_copy >>= 1;
ret++;
}
if (size & (size-1))
{
return 1<<ret;
}
return 1<<(ret-1);
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: bcmf_transfer_bytes
****************************************************************************/
int bcmf_transfer_bytes(FAR struct bcmf_dev_s *priv, bool write,
uint8_t function, uint32_t address,
uint8_t *buf, unsigned int len)
{
unsigned int blocklen;
unsigned int nblocks;
/* Use rw_io_direct method if len is 1 */
if (len == 0)
{
return -EINVAL;
}
if (len == 1)
{
if (write)
{
return sdio_io_rw_direct(priv->sdio_dev, write,
function, address, *buf, NULL);
}
return sdio_io_rw_direct(priv->sdio_dev, write,
function, address, 0, buf);
}
/* Find best block settings for transfer */
if (len >= 64)
{
/* Use block mode */
blocklen = 64;
nblocks = (len+63) / 64;
}
else
{
/* Use byte mode */
blocklen = bcmf_sdio_find_block_size(len);
nblocks = 0;
}
return sdio_io_rw_extended(priv->sdio_dev, write,
function, address, true, buf, blocklen, nblocks);
}
/****************************************************************************
* Name: bcmf_read_reg
****************************************************************************/
int bcmf_read_reg(FAR struct bcmf_dev_s *priv, uint8_t function,
uint32_t address, uint8_t *reg)
{
*reg = 0;
return bcmf_transfer_bytes(priv, false, function, address, reg, 1);
}
/****************************************************************************
* Name: bcmf_write_reg
****************************************************************************/
int bcmf_write_reg(FAR struct bcmf_dev_s *priv, uint8_t function,
uint32_t address, uint8_t reg)
{
return bcmf_transfer_bytes(priv, true, function, address, &reg, 1);
}
/****************************************************************************
* Name: bcmf_sem_wait
****************************************************************************/
int bcmf_sem_wait(sem_t *sem, unsigned int timeout_ms)
{
struct timespec abstime;
/* Get the current time */
(void)clock_gettime(CLOCK_REALTIME, &abstime);
abstime.tv_nsec += 1000 * 1000* timeout_ms;
if (abstime.tv_nsec >= 1000 * 1000 * 1000)
{
abstime.tv_sec++;
abstime.tv_nsec -= 1000 * 1000 * 1000;
}
return sem_timedwait(sem, &abstime);
}
/****************************************************************************
* Name: bcmf_sdio_initialize
****************************************************************************/
@ -239,6 +575,45 @@ int bcmf_sdio_initialize(int minor, FAR struct sdio_dev_s *dev)
memset(priv, 0, sizeof(*priv));
priv->sdio_dev = dev;
priv->minor = minor;
priv->ready = false;
priv->sleeping = true;
if ((ret = sem_init(&priv->thread_signal, 0, 0)) != OK)
{
goto exit_free_priv;
}
if ((ret = sem_setprotocol(&priv->thread_signal, SEM_PRIO_NONE)) != OK)
{
goto exit_free_priv;
}
priv->waitdog = wd_create();
if (!priv->waitdog)
{
ret = -ENOMEM;
goto exit_free_priv;
}
if ((ret = sem_init(&priv->control_mutex, 0, 1)) != OK)
{
goto exit_free_waitdog;
}
if ((ret = sem_init(&priv->control_timeout, 0, 0)) != OK)
{
goto exit_free_waitdog;
}
if ((ret = sem_setprotocol(&priv->control_timeout, SEM_PRIO_NONE)) != OK)
{
goto exit_free_waitdog;
}
/* Init transmit frames queue */
if ((ret = sem_init(&priv->tx_queue_mutex, 0, 1)) != OK)
{
goto exit_free_waitdog;
}
sq_init(&priv->tx_queue);
/* Initialize device hardware */
@ -246,7 +621,7 @@ int bcmf_sdio_initialize(int minor, FAR struct sdio_dev_s *dev)
if (ret != OK)
{
goto exit_free_priv;
goto exit_free_waitdog;
}
/* Probe device */
@ -258,14 +633,182 @@ int bcmf_sdio_initialize(int minor, FAR struct sdio_dev_s *dev)
goto exit_uninit_hw;
}
/* TODO Create a wlan device name and register network driver here */
/* Initialize device bus */
ret = bcmf_businitialize(priv);
if (ret != OK)
{
goto exit_uninit_hw;
}
up_mdelay(100);
priv->ready = true;
ret = bcmf_bus_setup_interrupts(priv);
if (ret != OK)
{
goto exit_uninit_hw;
}
/* FIXME global variable for now */
g_sdio_priv = priv;
/* Start the waitdog timer */
wd_start(priv->waitdog, BCMF_WAITDOG_TIMEOUT_TICK, bcmf_sdio_waitdog_timeout, 0);
/* Spawn bcmf daemon thread */
ret = kernel_thread(BCMF_THREAD_NAME, SCHED_PRIORITY_MAX,
BCMF_THREAD_STACK_SIZE, bcmf_sdio_thread,
(FAR char * const *)NULL);
if (ret <= 0)
{
_err("Cannot spawn bcmf thread\n");
ret = -EBADE;
goto exit_uninit_hw;
}
/* sdio bus is ready, init driver */
ret = bcmf_wl_initialize(priv);
if (ret != OK)
{
_err("Cannot init wlan driver %d\n", ret);
ret = -EIO;
goto exit_uninit_hw;
}
return OK;
exit_uninit_hw:
bcmf_hwuninitialize(priv);
exit_free_waitdog:
// TODO
exit_free_priv:
kmm_free(priv);
priv->ready = false;
return ret;
}
int bcmf_chipinitialize(FAR struct bcmf_dev_s *priv)
{
int ret;
uint32_t value = 0;
ret = bcmf_read_sbregw(priv, SI_ENUM_BASE, &value);
if (ret != OK)
{
return ret;
}
_info("chip id is 0x%x\n", value);
int chipid = value & 0xffff;
switch (chipid)
{
case SDIO_DEVICE_ID_BROADCOM_43362:
_info("bcm43362 chip detected\n");
priv->get_core_base_address = bcmf_43362_get_core_base_address;
break;
default:
_err("chip 0x%x is not supported\n", chipid);
return -ENODEV;
}
return OK;
}
void bcmf_sdio_waitdog_timeout(int argc, wdparm_t arg1, ...)
{
FAR struct bcmf_dev_s *priv = g_sdio_priv;
/* Notify bcmf thread */
sem_post(&priv->thread_signal);
}
int bcmf_sdio_thread(int argc, char **argv)
{
int ret;
FAR struct bcmf_dev_s *priv = g_sdio_priv;
_info("Enter\n");
/* FIXME wait for the chip to be ready to receive commands */
up_mdelay(50);
while (1)
{
/* Wait for event (device interrupt, user request or waitdog timer) */
ret = sem_wait(&priv->thread_signal);
if (ret != OK)
{
_err("Error while waiting for semaphore\n");
break;
}
/* Restart the waitdog timer */
wd_start(priv->waitdog, BCMF_WAITDOG_TIMEOUT_TICK,
bcmf_sdio_waitdog_timeout, 0);
/* Wake up device */
bcmf_sdio_bus_sleep(priv, false);
if (priv->irq_pending)
{
/* Woken up by interrupt, read device status */
priv->irq_pending = false;
bcmf_read_sbregw(priv,
CORE_BUS_REG(priv->get_core_base_address(SDIOD_CORE_ID),
intstatus), &priv->intstatus);
/* Clear interrupts */
bcmf_write_sbregw(priv,
CORE_BUS_REG(priv->get_core_base_address(SDIOD_CORE_ID),
intstatus), priv->intstatus);
// _info("intstatus %x\n", priv->intstatus);
}
/* On frame indication, read available frames */
if (priv->intstatus & I_HMB_FRAME_IND)
{
// _info("Frames available\n");
do
{
ret = bcmf_sdpcm_readframe(priv);
} while (ret == OK);
if (ret == -ENODATA)
{
/* All frames processed */
priv->intstatus &= ~I_HMB_FRAME_IND;
}
}
/* Send all queued frames */
do
{
ret = bcmf_sdpcm_sendframe(priv);
} while (ret == OK);
/* If we're done for now, turn off clock request. */
// TODO add wakelock
// bcmf_sdio_bus_sleep(priv, true);
}
return 0;
}

View File

@ -0,0 +1,29 @@
#ifndef __DRIVERS_WIRELESS_IEEE80211_BCMF_SDIO_H
#define __DRIVERS_WIRELESS_IEEE80211_BCMF_SDIO_H
#include "bcmf_sdio_regs.h"
#include "bcmf_sdio_core.h"
#include "bcmf_driver.h"
#include <stdint.h>
#include <stdbool.h>
/* FIXME: Low level bus data transfer function
* To avoid bus error, len will be aligned to:
* - upper power of 2 iflen is lesser than 64
* - upper 64 bytes block if len is greater than 64
*/
int bcmf_transfer_bytes(FAR struct bcmf_dev_s *priv, bool write,
uint8_t function, uint32_t address,
uint8_t *buf, unsigned int len);
int bcmf_read_reg(FAR struct bcmf_dev_s *priv, uint8_t function,
uint32_t address, uint8_t *reg);
int bcmf_write_reg(FAR struct bcmf_dev_s *priv, uint8_t function,
uint32_t address, uint8_t reg);
int bcmf_sem_wait(sem_t *sem, unsigned int timeout_ms);
#endif /* __DRIVERS_WIRELESS_IEEE80211_BCMF_SDIO_H */

View File

@ -0,0 +1,219 @@
/*
* Copyright (c) 2011 Broadcom Corporation
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
* SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
* OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
#ifndef BCMF_SDIO_CHIP_H_
#define BCMF_SDIO_CHIP_H_
#include <stdint.h>
#ifndef PAD
#define _PADLINE(line) pad ## line
#define _XSTR(line) _PADLINE(line)
#define PAD _XSTR(__LINE__)
#endif
/* SDIO device ID */
#define SDIO_DEVICE_ID_BROADCOM_43143 43143
#define SDIO_DEVICE_ID_BROADCOM_43241 0x4324
#define SDIO_DEVICE_ID_BROADCOM_4329 0x4329
#define SDIO_DEVICE_ID_BROADCOM_4330 0x4330
#define SDIO_DEVICE_ID_BROADCOM_4334 0x4334
#define SDIO_DEVICE_ID_BROADCOM_4335_4339 0x4335
#define SDIO_DEVICE_ID_BROADCOM_43362 43362
/*
* Core reg address translation.
* Both macro's returns a 32 bits byte address on the backplane bus.
*/
#define CORE_CC_REG(base, field) \
(base + offsetof(struct chipcregs, field))
#define CORE_BUS_REG(base, field) \
(base + offsetof(struct sdpcmd_regs, field))
#define CORE_SB(base, field) \
(base + offsetof(struct sbconfig, field))
#define BRCMF_MAX_CORENUM 6
#define SI_ENUM_BASE 0x18000000 /* Enumeration space base */
/* Target state register description */
#define SSB_TMSLOW_RESET 0x00000001 /* Reset */
#define SSB_TMSLOW_REJECT 0x00000002 /* Reject (Standard Backplane) */
#define SSB_TMSLOW_REJECT_23 0x00000004 /* Reject (Backplane rev 2.3) */
#define SSB_TMSLOW_CLOCK 0x00010000 /* Clock Enable */
#define SSB_TMSLOW_FGC 0x00020000 /* Force Gated Clocks On */
#define SSB_TMSLOW_PE 0x40000000 /* Power Management Enable */
#define SSB_TMSLOW_BE 0x80000000 /* BIST Enable */
#define I_HMB_SW_MASK ( (uint32_t) 0x000000F0 )
#define I_HMB_FRAME_IND ( 1<<6 )
#define CHIP_STA_INTERFACE 0
#define CHIP_AP_INTERFACE 1
#define CHIP_P2P_INTERFACE 2
enum {
CHIPCOMMON_CORE_ID,
DOT11MAC_CORE_ID,
SDIOD_CORE_ID,
WLAN_ARMCM3_CORE_ID,
SOCSRAM_CORE_ID
};
struct chip_core_info {
uint16_t id;
uint16_t rev;
uint32_t base;
uint32_t wrapbase;
uint32_t caps;
uint32_t cib;
};
struct sbconfig {
uint8_t PAD[0xf00];
uint32_t PAD[2];
uint32_t sbipsflag; /* initiator port ocp slave flag */
uint32_t PAD[3];
uint32_t sbtpsflag; /* target port ocp slave flag */
uint32_t PAD[11];
uint32_t sbtmerrloga; /* (sonics >= 2.3) */
uint32_t PAD;
uint32_t sbtmerrlog; /* (sonics >= 2.3) */
uint32_t PAD[3];
uint32_t sbadmatch3; /* address match3 */
uint32_t PAD;
uint32_t sbadmatch2; /* address match2 */
uint32_t PAD;
uint32_t sbadmatch1; /* address match1 */
uint32_t PAD[7];
uint32_t sbimstate; /* initiator agent state */
uint32_t sbintvec; /* interrupt mask */
uint32_t sbtmstatelow; /* target state */
uint32_t sbtmstatehigh; /* target state */
uint32_t sbbwa0; /* bandwidth allocation table0 */
uint32_t PAD;
uint32_t sbimconfiglow; /* initiator configuration */
uint32_t sbimconfighigh; /* initiator configuration */
uint32_t sbadmatch0; /* address match0 */
uint32_t PAD;
uint32_t sbtmconfiglow; /* target configuration */
uint32_t sbtmconfighigh; /* target configuration */
uint32_t sbbconfig; /* broadcast configuration */
uint32_t PAD;
uint32_t sbbstate; /* broadcast state */
uint32_t PAD[3];
uint32_t sbactcnfg; /* activate configuration */
uint32_t PAD[3];
uint32_t sbflagst; /* current sbflags */
uint32_t PAD[3];
uint32_t sbidlow; /* identification */
uint32_t sbidhigh; /* identification */
};
/* sdio core registers */
struct sdpcmd_regs {
uint32_t corecontrol; /* 0x00, rev8 */
uint32_t corestatus; /* rev8 */
uint32_t PAD[1];
uint32_t biststatus; /* rev8 */
/* PCMCIA access */
uint16_t pcmciamesportaladdr; /* 0x010, rev8 */
uint16_t PAD[1];
uint16_t pcmciamesportalmask; /* rev8 */
uint16_t PAD[1];
uint16_t pcmciawrframebc; /* rev8 */
uint16_t PAD[1];
uint16_t pcmciaunderflowtimer; /* rev8 */
uint16_t PAD[1];
/* interrupt */
uint32_t intstatus; /* 0x020, rev8 */
uint32_t hostintmask; /* rev8 */
uint32_t intmask; /* rev8 */
uint32_t sbintstatus; /* rev8 */
uint32_t sbintmask; /* rev8 */
uint32_t funcintmask; /* rev4 */
uint32_t PAD[2];
uint32_t tosbmailbox; /* 0x040, rev8 */
uint32_t tohostmailbox; /* rev8 */
uint32_t tosbmailboxdata; /* rev8 */
uint32_t tohostmailboxdata; /* rev8 */
/* synchronized access to registers in SDIO clock domain */
uint32_t sdioaccess; /* 0x050, rev8 */
uint32_t PAD[3];
/* PCMCIA frame control */
uint8_t pcmciaframectrl; /* 0x060, rev8 */
uint8_t PAD[3];
uint8_t pcmciawatermark; /* rev8 */
uint8_t PAD[155];
/* interrupt batching control */
uint32_t intrcvlazy; /* 0x100, rev8 */
uint32_t PAD[3];
/* counters */
uint32_t cmd52rd; /* 0x110, rev8 */
uint32_t cmd52wr; /* rev8 */
uint32_t cmd53rd; /* rev8 */
uint32_t cmd53wr; /* rev8 */
uint32_t abort; /* rev8 */
uint32_t datacrcerror; /* rev8 */
uint32_t rdoutofsync; /* rev8 */
uint32_t wroutofsync; /* rev8 */
uint32_t writebusy; /* rev8 */
uint32_t readwait; /* rev8 */
uint32_t readterm; /* rev8 */
uint32_t writeterm; /* rev8 */
uint32_t PAD[40];
uint32_t clockctlstatus; /* rev8 */
uint32_t PAD[7];
uint32_t PAD[128]; /* DMA engines */
/* SDIO/PCMCIA CIS region */
char cis[512]; /* 0x400-0x5ff, rev6 */
/* PCMCIA function control registers */
char pcmciafcr[256]; /* 0x600-6ff, rev6 */
uint16_t PAD[55];
/* PCMCIA backplane access */
uint16_t backplanecsr; /* 0x76E, rev6 */
uint16_t backplaneaddr0; /* rev6 */
uint16_t backplaneaddr1; /* rev6 */
uint16_t backplaneaddr2; /* rev6 */
uint16_t backplaneaddr3; /* rev6 */
uint16_t backplanedata0; /* rev6 */
uint16_t backplanedata1; /* rev6 */
uint16_t backplanedata2; /* rev6 */
uint16_t backplanedata3; /* rev6 */
uint16_t PAD[31];
/* sprom "size" & "blank" info */
uint16_t spromstatus; /* 0x7BE, rev2 */
uint32_t PAD[464];
uint16_t PAD[0x80];
};
#ifdef CONFIG_IEEE80211_BROADCOM_BCM43362
#include "bcmf_chip_43362.h"
#endif
#endif /* _BCMF_SDIO_CHIP_H_ */

View File

@ -0,0 +1,167 @@
/*
* Copyright (c) 2010 Broadcom Corporation
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
* SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
* OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
#ifndef _BRCM_SDH_H_
#define _BRCM_SDH_H_
#define SDIO_FUNC_0 0
#define SDIO_FUNC_1 1
#define SDIO_FUNC_2 2
#define SDIOD_FBR_SIZE 0x100
/* io_en */
#define SDIO_FUNC_ENABLE_1 0x02
#define SDIO_FUNC_ENABLE_2 0x04
/* io_rdys */
#define SDIO_FUNC_READY_1 0x02
#define SDIO_FUNC_READY_2 0x04
/* intr_status */
#define INTR_STATUS_FUNC1 0x2
#define INTR_STATUS_FUNC2 0x4
/* Maximum number of I/O funcs */
#define SDIOD_MAX_IOFUNCS 7
/* mask of register map */
#define REG_F0_REG_MASK 0x7FF
#define REG_F1_MISC_MASK 0x1FFFF
/* as of sdiod rev 0, supports 3 functions */
#define SBSDIO_NUM_FUNCTION 3
/* function 0 vendor specific CCCR registers */
#define SDIO_CCCR_BRCM_CARDCAP 0xf0
#define SDIO_CCCR_BRCM_CARDCAP_CMD14_SUPPORT 0x02
#define SDIO_CCCR_BRCM_CARDCAP_CMD14_EXT 0x04
#define SDIO_CCCR_BRCM_CARDCAP_CMD_NODEC 0x08
#define SDIO_CCCR_BRCM_CARDCTRL 0xf1
#define SDIO_CCCR_BRCM_CARDCTRL_WLANRESET 0x02
#define SDIO_CCCR_BRCM_SEPINT 0xf2
#define SDIO_SEPINT_MASK 0x01
#define SDIO_SEPINT_OE 0x02
#define SDIO_SEPINT_ACT_HI 0x04
/* function 1 miscellaneous registers */
/* sprom command and status */
#define SBSDIO_SPROM_CS 0x10000
/* sprom info register */
#define SBSDIO_SPROM_INFO 0x10001
/* sprom indirect access data byte 0 */
#define SBSDIO_SPROM_DATA_LOW 0x10002
/* sprom indirect access data byte 1 */
#define SBSDIO_SPROM_DATA_HIGH 0x10003
/* sprom indirect access addr byte 0 */
#define SBSDIO_SPROM_ADDR_LOW 0x10004
/* sprom indirect access addr byte 0 */
#define SBSDIO_SPROM_ADDR_HIGH 0x10005
/* xtal_pu (gpio) output */
#define SBSDIO_CHIP_CTRL_DATA 0x10006
/* xtal_pu (gpio) enable */
#define SBSDIO_CHIP_CTRL_EN 0x10007
/* rev < 7, watermark for sdio device */
#define SBSDIO_WATERMARK 0x10008
/* control busy signal generation */
#define SBSDIO_DEVICE_CTL 0x10009
/* SB Address Window Low (b15) */
#define SBSDIO_FUNC1_SBADDRLOW 0x1000A
/* SB Address Window Mid (b23:b16) */
#define SBSDIO_FUNC1_SBADDRMID 0x1000B
/* SB Address Window High (b31:b24) */
#define SBSDIO_FUNC1_SBADDRHIGH 0x1000C
/* Frame Control (frame term/abort) */
#define SBSDIO_FUNC1_FRAMECTRL 0x1000D
/* ChipClockCSR (ALP/HT ctl/status) */
#define SBSDIO_FUNC1_CHIPCLKCSR 0x1000E
/* Force ALP request to backplane */
#define SBSDIO_FORCE_ALP 0x01
/* Force HT request to backplane */
#define SBSDIO_FORCE_HT 0x02
/* Force ILP request to backplane */
#define SBSDIO_FORCE_ILP 0x04
/* Make ALP ready (power up xtal) */
#define SBSDIO_ALP_AVAIL_REQ 0x08
/* Make HT ready (power up PLL) */
#define SBSDIO_HT_AVAIL_REQ 0x10
/* Squelch clock requests from HW */
#define SBSDIO_FORCE_HW_CLKREQ_OFF 0x20
/* Status: ALP is ready */
#define SBSDIO_ALP_AVAIL 0x40
/* Status: HT is ready */
#define SBSDIO_HT_AVAIL 0x80
/* SdioPullUp (on cmd, d0-d2) */
#define SBSDIO_FUNC1_SDIOPULLUP 0x1000F
/* Write Frame Byte Count Low */
#define SBSDIO_FUNC1_WFRAMEBCLO 0x10019
/* Write Frame Byte Count High */
#define SBSDIO_FUNC1_WFRAMEBCHI 0x1001A
/* Read Frame Byte Count Low */
#define SBSDIO_FUNC1_RFRAMEBCLO 0x1001B
/* Read Frame Byte Count High */
#define SBSDIO_FUNC1_RFRAMEBCHI 0x1001C
/* MesBusyCtl (rev 11) */
#define SBSDIO_FUNC1_MESBUSYCTRL 0x1001D
/* Sdio Core Rev 12 */
#define SBSDIO_FUNC1_WAKEUPCTRL 0x1001E
#define SBSDIO_FUNC1_WCTRL_ALPWAIT_MASK 0x1
#define SBSDIO_FUNC1_WCTRL_ALPWAIT_SHIFT 0
#define SBSDIO_FUNC1_WCTRL_HTWAIT_MASK 0x2
#define SBSDIO_FUNC1_WCTRL_HTWAIT_SHIFT 1
#define SBSDIO_FUNC1_SLEEPCSR 0x1001F
#define SBSDIO_FUNC1_SLEEPCSR_KSO_MASK 0x1
#define SBSDIO_FUNC1_SLEEPCSR_KSO_SHIFT 0
#define SBSDIO_FUNC1_SLEEPCSR_KSO_EN 1
#define SBSDIO_FUNC1_SLEEPCSR_DEVON_MASK 0x2
#define SBSDIO_FUNC1_SLEEPCSR_DEVON_SHIFT 1
#define SBSDIO_AVBITS (SBSDIO_HT_AVAIL | SBSDIO_ALP_AVAIL)
#define SBSDIO_ALPAV(regval) ((regval) & SBSDIO_AVBITS)
#define SBSDIO_HTAV(regval) (((regval) & SBSDIO_AVBITS) == SBSDIO_AVBITS)
#define SBSDIO_ALPONLY(regval) (SBSDIO_ALPAV(regval) && !SBSDIO_HTAV(regval))
#define SBSDIO_CLKAV(regval, alponly) \
(SBSDIO_ALPAV(regval) && (alponly ? 1 : SBSDIO_HTAV(regval)))
#define SBSDIO_FUNC1_MISC_REG_START 0x10000 /* f1 misc register start */
#define SBSDIO_FUNC1_MISC_REG_LIMIT 0x1001F /* f1 misc register end */
/* function 1 OCP space */
/* sb offset addr is <= 15 bits, 32k */
#define SBSDIO_SB_OFT_ADDR_MASK 0x07FFF
#define SBSDIO_SB_OFT_ADDR_LIMIT 0x08000
/* with b15, maps to 32-bit SB access */
#define SBSDIO_SB_ACCESS_2_4B_FLAG 0x08000
/* valid bits in SBSDIO_FUNC1_SBADDRxxx regs */
#define SBSDIO_SBADDRLOW_MASK 0x80 /* Valid bits in SBADDRLOW */
#define SBSDIO_SBADDRMID_MASK 0xff /* Valid bits in SBADDRMID */
#define SBSDIO_SBADDRHIGH_MASK 0xffU /* Valid bits in SBADDRHIGH */
/* Address bits from SBADDR regs */
#define SBSDIO_SBWINDOW_MASK 0xffff8000
/* Packet alignment for most efficient SDIO (can change based on platform) */
#define BRCMF_SDALIGN (1 << 6)
/* watchdog polling interval in ms */
#define BRCMF_WD_POLL_MS 10
#endif /* _BRCM_SDH_H_ */

View File

@ -0,0 +1,653 @@
/****************************************************************************
* drivers/wireless/ieee80211/bcmf_sdpcm.c
*
* Copyright (C) 2017 Gregory Nutt. All rights reserved.
* Author: Simon Piriou <spiriou31@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/compiler.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/arch.h>
#include <stddef.h>
#include <string.h>
#include <queue.h>
#include <semaphore.h>
#include "bcmf_sdio.h"
#include "bcmf_core.h"
#include "bcmf_sdpcm.h"
#include "bcmf_ioctl.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* SDA_FRAMECTRL */
#define SFC_RF_TERM (1 << 0) /* Read Frame Terminate */
#define SFC_WF_TERM (1 << 1) /* Write Frame Terminate */
#define SFC_CRC4WOOS (1 << 2) /* CRC error for write out of sync */
#define SFC_ABORTALL (1 << 3) /* Abort all in-progress frames */
/* tosbmailbox bits corresponding to intstatus bits */
#define SMB_NAK (1 << 0) /* Frame NAK */
#define SMB_INT_ACK (1 << 1) /* Host Interrupt ACK */
#define SMB_USE_OOB (1 << 2) /* Use OOB Wakeup */
#define SMB_DEV_INT (1 << 3) /* Miscellaneous Interrupt */
/* CDC flag definitions */
#define CDC_DCMD_ERROR 0x01 /* 1=cmd failed */
#define CDC_DCMD_SET 0x02 /* 0=get, 1=set cmd */
#define CDC_DCMD_IF_MASK 0xF000 /* I/F index */
#define CDC_DCMD_IF_SHIFT 12
#define CDC_DCMD_ID_MASK 0xFFFF0000 /* id an cmd pairing */
#define CDC_DCMD_ID_SHIFT 16 /* ID Mask shift bits */
#define CDC_DCMD_ID(flags) \
(((flags) & CDC_DCMD_ID_MASK) >> CDC_DCMD_ID_SHIFT)
#define SDPCM_CONTROL_CHANNEL 0 /* Control */
#define SDPCM_EVENT_CHANNEL 1 /* Asyc Event Indication */
#define SDPCM_DATA_CHANNEL 2 /* Data Xmit/Recv */
#define SDPCM_GLOM_CHANNEL 3 /* Coalesced packets */
#define SDPCM_TEST_CHANNEL 15 /* Test/debug packets */
#define SDPCM_CONTROL_TIMEOUT_MS 1000
// TODO remove
void bcmf_hexdump(uint8_t *data, unsigned int len, unsigned long offset);
/****************************************************************************
* Private Types
****************************************************************************/
struct bcmf_sdpcm_header {
uint16_t size;
uint16_t checksum;
uint8_t sequence;
uint8_t channel;
uint8_t next_length;
uint8_t data_offset;
uint8_t flow_control;
uint8_t credit;
uint16_t padding;
};
struct bcmf_sdpcm_cdc_header {
uint32_t cmd; /* dongle command value */
uint32_t len; /* lower 16: output buflen;
* upper 16: input buflen (excludes header) */
uint32_t flags; /* flag defns given below */
uint32_t status; /* status code returned from the device */
};
struct bcmf_sdpcm_cdc_dcmd {
struct bcmf_sdpcm_header header;
struct bcmf_sdpcm_cdc_header cdc_header;
uint8_t data[0];
};
struct bcmf_sdpcm_frame {
dq_entry_t list_entry;
struct bcmf_sdpcm_header header;
uint8_t data[0];
};
struct bcmf_sdpcm_cdc_frame {
dq_entry_t list_entry;
struct bcmf_sdpcm_header header;
struct bcmf_sdpcm_cdc_header cdc_header;
uint8_t data[0];
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int bcmf_sdpcm_rxfail(FAR struct bcmf_dev_s *priv, bool retry);
static int bcmf_sdpcm_process_header(FAR struct bcmf_dev_s *priv,
struct bcmf_sdpcm_header *header);
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
int bcmf_sdpcm_rxfail(FAR struct bcmf_dev_s *priv, bool retry)
{
/* issue abort command for F2 through F0 */
bcmf_write_reg(priv, 0, SDIO_CCCR_IOABORT, 2);
bcmf_write_reg(priv, 1, SBSDIO_FUNC1_FRAMECTRL, SFC_RF_TERM);
/* TODO Wait until the packet has been flushed (device/FIFO stable) */
/* Send NAK to retry to read frame */
if (retry)
{
bcmf_write_sbregb(priv,
CORE_BUS_REG(priv->get_core_base_address(SDIOD_CORE_ID),
tosbmailbox), SMB_NAK);
}
return 0;
}
int bcmf_sdpcm_process_header(FAR struct bcmf_dev_s *priv,
struct bcmf_sdpcm_header *header)
{
if (header->data_offset < sizeof(struct bcmf_sdpcm_header) ||
header->data_offset > header->size)
{
_err("Invalid data offset\n");
bcmf_sdpcm_rxfail(priv, false);
return -ENXIO;
}
/* Update tx credits */
_info("update credit %x %x %x\n", header->credit,
priv->tx_seq, priv->max_seq);
if (header->credit - priv->tx_seq > 0x40)
{
_err("seq %d: max tx seq number error\n", priv->tx_seq);
priv->max_seq = priv->tx_seq + 2;
}
else
{
priv->max_seq = header->credit;
}
return OK;
}
/****************************************************************************
* Public Functions
****************************************************************************/
// FIXME remove
uint8_t tmp_buffer[1024];
uint8_t tmp_buffer_ctl[1024];
int bcmf_sdpcm_readframe(FAR struct bcmf_dev_s *priv)
{
int ret;
uint16_t len, checksum;
struct bcmf_sdpcm_header *header = (struct bcmf_sdpcm_header*)tmp_buffer;
/* Read header */
ret = bcmf_transfer_bytes(priv, false, 2, 0, (uint8_t*)header, 4);
if (ret != OK)
{
_info("failread size\n");
ret = -EIO;
goto exit_abort;
}
len = header->size;
checksum = header->checksum;
/* All zero means no more to read */
if (!(len | checksum))
{
return -ENODATA;
}
if (((~len & 0xffff) ^ checksum) || len < sizeof(struct bcmf_sdpcm_header))
{
_err("Invalid header checksum or len %x %x\n", len, checksum);
ret = -EINVAL;
goto exit_abort;
}
// FIXME define for size
if (len > sizeof(tmp_buffer))
{
_err("Frame is too large, cancel %d\n", len);
ret = -ENOMEM;
goto exit_abort;
}
/* Read remaining frame data */
// TODO allocate buffer
ret = bcmf_transfer_bytes(priv, false, 2, 0, (uint8_t*)header+4, len - 4);
if (ret != OK)
{
ret = -EIO;
goto exit_free_abort;
}
// _info("Receive frame\n");
// bcmf_hexdump((uint8_t*)header, len, (unsigned int)header);
/* Process and validate header */
ret = bcmf_sdpcm_process_header(priv, header);
if (ret != OK)
{
_err("Error while processing header %d\n", ret);
ret = -EINVAL;
goto exit_free_frame;
}
/* Process received frame content */
switch (header->channel & 0x0f)
{
case SDPCM_CONTROL_CHANNEL:
// _info("Control message\n");
/* Check frame */
if (header->size < sizeof(struct bcmf_sdpcm_header) +
sizeof(struct bcmf_sdpcm_cdc_header))
{
_err("Control frame too small\n");
ret = -EINVAL;
goto exit_free_frame;
}
struct bcmf_sdpcm_cdc_header *cdc_header =
(struct bcmf_sdpcm_cdc_header*)&header[1];
if (header->size < sizeof(struct bcmf_sdpcm_header) +
sizeof(struct bcmf_sdpcm_cdc_header) +
cdc_header->len ||
cdc_header->len > sizeof(tmp_buffer) -
sizeof(struct bcmf_sdpcm_header) -
sizeof(struct bcmf_sdpcm_cdc_header))
{
_err("Invalid control frame size\n");
ret = -EINVAL;
goto exit_free_frame;
}
// TODO check interface ?
if (cdc_header->flags >> CDC_DCMD_ID_SHIFT == priv->control_reqid)
{
/* Expected frame received, send it back to user */
// TODO allocate real buffer
memcpy(tmp_buffer_ctl, tmp_buffer, header->size);
priv->control_rxframe = tmp_buffer_ctl;
// priv->control_rxframe = (uint8_t*)header;
sem_post(&priv->control_timeout);
return OK;
}
else
{
_info("Got unexpected control frame\n");
ret = -EINVAL;
goto exit_free_frame;
}
break;
case SDPCM_EVENT_CHANNEL:
_info("Event message\n");
bcmf_hexdump((uint8_t*)header, header->size, (unsigned long)header);
ret = OK;
break;
case SDPCM_DATA_CHANNEL:
_info("Data message\n");
ret = OK;
break;
default:
_err("Got unexpected message type %d\n", header->channel);
ret = OK;
}
exit_free_frame:
// TODO free frame buffer
return ret;
exit_free_abort:
// TODO free frame buffer
exit_abort:
bcmf_sdpcm_rxfail(priv, false);
return ret;
}
int bcmf_sdpcm_sendframe(FAR struct bcmf_dev_s *priv)
{
int ret;
struct bcmf_sdpcm_frame *frame;
if (priv->tx_queue.tail == NULL)
{
/* No more frames to send */
return -ENODATA;
}
if (priv->tx_seq == priv->max_seq)
{
// TODO handle this case
_err("No credit to send frame\n");
return -EAGAIN;
}
if ((ret = sem_wait(&priv->tx_queue_mutex)) != OK)
{
return ret;
}
frame = (struct bcmf_sdpcm_frame*)priv->tx_queue.tail;
/* Set frame sequence id */
frame->header.sequence = priv->tx_seq++;
// _info("Send frame\n");
// bcmf_hexdump((uint8_t*)&frame->header, frame->header.size,
// (unsigned int)&frame->header);
ret = bcmf_transfer_bytes(priv, true, 2, 0, (uint8_t*)&frame->header,
frame->header.size);
if (ret != OK)
{
_info("fail send frame %d\n", ret);
ret = -EIO;
goto exit_abort;
// TODO handle retry count and remove frame from queue + abort TX
}
/* Frame sent, remove it from queue */
if (priv->tx_queue.head == &frame->list_entry)
{
/* List is empty */
priv->tx_queue.head = NULL;
priv->tx_queue.tail = NULL;
}
else
{
priv->tx_queue.tail = frame->list_entry.blink;
frame->list_entry.blink->flink = priv->tx_queue.head;
}
/* TODO free frame buffer */
goto exit_post_sem;
exit_abort:
// bcmf_sdpcm_txfail(priv, false);
exit_post_sem:
sem_post(&priv->tx_queue_mutex);
return ret;
}
// FIXME remove
uint8_t tmp_buffer2[512];
uint8_t* bcmf_sdpcm_allocate_iovar(FAR struct bcmf_dev_s *priv, char *name,
uint8_t *data, uint32_t *len)
{
uint32_t data_len;
uint16_t name_len;
if (name)
{
name_len = strlen(name) + 1;
}
else
{
name_len = 0;
}
if (data)
{
data_len = *len;
}
else
{
data_len = 0;
}
*len = 0;
// FIXME allocate buffer and use max_size instead of 512
if (data_len > 512-sizeof(struct bcmf_sdpcm_cdc_frame) ||
(data_len + name_len) > 512-sizeof(struct bcmf_sdpcm_cdc_frame))
{
return NULL;
}
// TODO allocate buffer len + sizeof(struct bcmf_sdpcm_cdc_frame)
/* Copy name string and data */
memcpy(tmp_buffer2+sizeof(struct bcmf_sdpcm_cdc_frame), name, name_len);
memcpy(tmp_buffer2+sizeof(struct bcmf_sdpcm_cdc_frame)+name_len,
data, data_len);
*len = sizeof(struct bcmf_sdpcm_cdc_frame)+name_len+data_len;
return tmp_buffer2;
}
int bcmf_sdpcm_queue_frame(FAR struct bcmf_dev_s *priv, uint8_t channel,
uint8_t *data, uint32_t len)
{
int ret;
struct bcmf_sdpcm_frame *frame = (struct bcmf_sdpcm_frame*)data;
uint16_t frame_size = len - sizeof(frame->list_entry);
/* Prepare sw header */
memset(&frame->header, 0, sizeof(struct bcmf_sdpcm_header));
frame->header.size = frame_size;
frame->header.checksum = ~frame_size;
frame->header.channel = channel;
frame->header.data_offset = sizeof(struct bcmf_sdpcm_header);
/* Add frame in tx queue */
if ((ret = sem_wait(&priv->tx_queue_mutex)) != OK)
{
return ret;
}
if (priv->tx_queue.head == NULL)
{
/* List is empty */
priv->tx_queue.head = &frame->list_entry;
priv->tx_queue.tail = &frame->list_entry;
frame->list_entry.flink = &frame->list_entry;
frame->list_entry.blink = &frame->list_entry;
}
else
{
/* Insert entry at list head */
frame->list_entry.flink = priv->tx_queue.head;
frame->list_entry.blink = priv->tx_queue.tail;
priv->tx_queue.head->blink = &frame->list_entry;
priv->tx_queue.head = &frame->list_entry;
}
sem_post(&priv->tx_queue_mutex);
/* Notify bcmf thread tx frame is ready */
sem_post(&priv->thread_signal);
return OK;
}
int bcmf_sdpcm_send_cdc_frame(FAR struct bcmf_dev_s *priv, uint32_t cmd,
int ifidx, bool set, uint8_t *data, uint32_t len)
{
struct bcmf_sdpcm_cdc_frame *msg = (struct bcmf_sdpcm_cdc_frame*)data;
/* Setup cdc_dcmd header */
msg->cdc_header.cmd = cmd;
msg->cdc_header.len = len-sizeof(struct bcmf_sdpcm_cdc_frame);
msg->cdc_header.status = 0;
msg->cdc_header.flags = ++priv->control_reqid << CDC_DCMD_ID_SHIFT;
msg->cdc_header.flags |= ifidx << CDC_DCMD_IF_SHIFT;
if (set)
{
msg->cdc_header.flags |= CDC_DCMD_SET;
}
/* Queue frame */
return bcmf_sdpcm_queue_frame(priv, SDPCM_CONTROL_CHANNEL, data, len);
}
int bcmf_sdpcm_control_request(FAR struct bcmf_dev_s *priv,
uint32_t ifidx, bool set, uint32_t cmd,
char *name, uint8_t *data, uint32_t *len)
{
int ret;
uint8_t *iovar_buf;
uint32_t out_len = *len;
uint32_t iovar_buf_len = *len;
*len = 0;
/* Take device control mutex */
if ((ret = sem_wait(&priv->control_mutex)) !=OK)
{
return ret;
}
/* Prepare control frame */
iovar_buf = bcmf_sdpcm_allocate_iovar(priv, name, data, &iovar_buf_len);
if (!iovar_buf)
{
_err("Cannot allocate iovar buf\n");
ret = -ENOMEM;
goto exit_sem_post;
}
/* Send control frame. iovar buffer is freed when sent */
ret = bcmf_sdpcm_send_cdc_frame(priv, cmd,
ifidx, set, iovar_buf, iovar_buf_len);
if (ret != OK)
{
// TODO free allocated iovar buffer here
goto exit_sem_post;
}
/* Wait for response */
priv->control_rxframe = NULL;
ret = bcmf_sem_wait(&priv->control_timeout, SDPCM_CONTROL_TIMEOUT_MS);
if (ret != OK)
{
_err("Error while waiting for control response %d\n", ret);
goto exit_sem_post;
}
/* Check frame status */
struct bcmf_sdpcm_cdc_dcmd *rxframe =
(struct bcmf_sdpcm_cdc_dcmd*)priv->control_rxframe;
priv->control_status = rxframe->cdc_header.status;
if (priv->control_status != 0)
{
_err("Invalid cdc status 0x%x\n", priv->control_status);
ret = -EIO;
goto exit_free_rx_frame;
}
if (!set)
{
/* Request sent, copy received data back */
if (rxframe->cdc_header.len > out_len)
{
_err("RX frame too big %d %d\n", rxframe->cdc_header.len, out_len);
memcpy(data, rxframe->data, out_len);
*len = out_len;
}
else
{
memcpy(data, rxframe->data, rxframe->cdc_header.len);
*len = rxframe->cdc_header.len;
}
}
exit_free_rx_frame:
// TODO free rxframe buffer */
priv->control_rxframe = NULL;
exit_sem_post:
sem_post(&priv->control_mutex);
return ret;
}
int bcmf_sdpcm_iovar_request(FAR struct bcmf_dev_s *priv,
uint32_t ifidx, bool set, char *name,
uint8_t *data, uint32_t *len)
{
return bcmf_sdpcm_control_request(priv, ifidx, set,
set ? WLC_SET_VAR : WLC_GET_VAR, name,
data, len);
}
int bcmf_sdpcm_ioctl(FAR struct bcmf_dev_s *priv,
uint32_t ifidx, bool set, uint32_t cmd,
uint8_t *data, uint32_t *len)
{
return bcmf_sdpcm_control_request(priv, ifidx, set, cmd, NULL, data, len);
}

View File

@ -0,0 +1,18 @@
#ifndef __DRIVERS_WIRELESS_IEEE80211_BCMF_SDPCM_H
#define __DRIVERS_WIRELESS_IEEE80211_BCMF_SDPCM_H
#include "bcmf_driver.h"
int bcmf_sdpcm_readframe(FAR struct bcmf_dev_s *priv);
int bcmf_sdpcm_sendframe(FAR struct bcmf_dev_s *priv);
int bcmf_sdpcm_iovar_request(FAR struct bcmf_dev_s *priv,
uint32_t ifidx, bool set, char *name,
uint8_t *data, uint32_t *len);
int bcmf_sdpcm_ioctl(FAR struct bcmf_dev_s *priv,
uint32_t ifidx, bool set, uint32_t cmd,
uint8_t *data, uint32_t *len);
#endif /* __DRIVERS_WIRELESS_IEEE80211_BCMF_SDPCM_H */

View File

@ -0,0 +1,382 @@
#include <nuttx/wireless/ieee80211/mmc_sdio.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/arch.h>
#define SDIO_CMD53_TIMEOUT_MS 100
#define SDIO_IDLE_DELAY_MS 50
struct __attribute__((packed)) sdio_cmd52 {
uint32_t write_data : 8;
uint32_t reserved_8 : 1;
uint32_t register_address : 17;
uint32_t reserved_26 : 1;
uint32_t raw_flag : 1;
uint32_t function_number : 3;
uint32_t rw_flag : 1;
};
struct __attribute__((packed)) sdio_cmd53 {
uint32_t byte_block_count : 9;
uint32_t register_address : 17;
uint32_t op_code : 1;
uint32_t block_mode : 1;
uint32_t function_number : 3;
uint32_t rw_flag : 1;
};
struct __attribute__((packed)) sdio_resp_R5 {
uint32_t data : 8;
struct {
uint32_t out_of_range : 1;
uint32_t function_number : 1;
uint32_t rfu : 1;
uint32_t error : 1;
uint32_t io_current_state : 2;
uint32_t illegal_command : 1;
uint32_t com_crc_error : 1;
} flags;
uint32_t reserved_16 : 16;
};
union sdio_cmd5x {
uint32_t value;
struct sdio_cmd52 cmd52;
struct sdio_cmd53 cmd53;
};
int sdio_sendcmdpoll(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t arg)
{
int ret;
/* Send the command */
ret = SDIO_SENDCMD(dev, cmd, arg);
if (ret == OK)
{
/* Then poll-wait until the response is available */
ret = SDIO_WAITRESPONSE(dev, cmd);
if (ret != OK)
{
_err("ERROR: Wait for response to cmd: %08x failed: %d\n",
cmd, ret);
}
}
return ret;
}
int sdio_io_rw_direct(FAR struct sdio_dev_s *dev, bool write,
uint8_t function, uint32_t address,
uint8_t inb, uint8_t* outb)
{
union sdio_cmd5x arg;
struct sdio_resp_R5 resp;
int ret;
/* Setup CMD52 argument */
if (write)
{
arg.cmd52.write_data = inb;
}
else
{
arg.cmd52.write_data = 0;
}
arg.cmd52.register_address = address & 0x1ffff;
arg.cmd52.raw_flag = (write && outb);
arg.cmd52.function_number = function & 7;
arg.cmd52.rw_flag = write;
/* Send CMD52 command */
sdio_sendcmdpoll(dev, SDIO_ACMD52, arg.value);
ret = SDIO_RECVR5(dev, SDIO_ACMD52, (uint32_t*)&resp);
if (ret != OK)
{
_err("ERROR: SDIO_RECVR5 failed %d\n", ret);
return ret;
}
/* Check for errors */
if (resp.flags.error)
{
return -EIO;
}
if (resp.flags.function_number || resp.flags.out_of_range)
{
return -EINVAL;
}
/* Write output byte */
if (outb)
{
*outb = resp.data & 0xff;
}
return OK;
}
int sdio_io_rw_extended(FAR struct sdio_dev_s *dev, bool write,
uint8_t function, uint32_t address,
bool inc_addr, uint8_t *buf,
unsigned int blocklen, unsigned int nblocks)
{
union sdio_cmd5x arg;
struct sdio_resp_R5 resp;
int ret;
sdio_eventset_t wkupevent;
/* Setup CMD53 argument */
arg.cmd53.register_address = address & 0x1ffff;
arg.cmd53.op_code = inc_addr;
arg.cmd53.function_number = function & 7;
arg.cmd53.rw_flag = write;
if (nblocks == 0 && blocklen < 512)
{
/* Use byte mode */
// _info("byte mode\n");
arg.cmd53.block_mode = 0;
arg.cmd53.byte_block_count = blocklen;
nblocks = 1;
}
else
{
/* Use block mode */
arg.cmd53.block_mode = 1;
arg.cmd53.byte_block_count = nblocks;
}
/* Send CMD53 command */
SDIO_BLOCKSETUP(dev, blocklen, nblocks);
SDIO_WAITENABLE(dev,
SDIOWAIT_TRANSFERDONE | SDIOWAIT_TIMEOUT | SDIOWAIT_ERROR);
if (write)
{
// _info("prep write %d %d\n", blocklen, nblocks);
sdio_sendcmdpoll(dev, SDIO_ACMD53, (uint32_t)arg.value);
ret = SDIO_RECVR5(dev, SDIO_ACMD53, (uint32_t*)&resp);
SDIO_SENDSETUP(dev, buf, blocklen * nblocks);
wkupevent = SDIO_EVENTWAIT(dev, SDIO_CMD53_TIMEOUT_MS);
}
else
{
// _info("prep read %d\n", blocklen * nblocks);
SDIO_RECVSETUP(dev, buf, blocklen * nblocks);
SDIO_SENDCMD(dev, SDIO_ACMD53, (uint32_t)arg.value);
wkupevent = SDIO_EVENTWAIT(dev, SDIO_CMD53_TIMEOUT_MS);
ret = SDIO_RECVR5(dev, SDIO_ACMD53, (uint32_t*)&resp);
}
if (ret != OK)
{
_err("ERROR: SDIO_RECVR5 failed %d\n", ret);
return ret;
}
/* Check for errors */
if (wkupevent & SDIOWAIT_TIMEOUT)
{
_err("timeout\n");
return -ETIMEDOUT;
}
if (resp.flags.error || (wkupevent & SDIOWAIT_ERROR))
{
_err("error 1\n");
return -EIO;
}
if (resp.flags.function_number || resp.flags.out_of_range)
{
_err("error 2\n");
return -EINVAL;
}
return OK;
}
int sdio_set_wide_bus(struct sdio_dev_s *dev)
{
int ret;
uint8_t value;
/* Read Bus Interface Control register */
ret = sdio_io_rw_direct(dev, false, 0, SDIO_CCCR_BUS_IF, 0, &value);
if (ret != OK)
{
return ret;
}
/* Set 4 bits bus width setting */
value &= ~SDIO_CCCR_BUS_IF_WIDTH_MASK;
value |= SDIO_CCCR_BUS_IF_4_BITS;
ret = sdio_io_rw_direct(dev, true, 0, SDIO_CCCR_BUS_IF, value, NULL);
if (ret != OK)
{
return ret;
}
SDIO_WIDEBUS(dev, true);
return OK;
}
int sdio_probe(FAR struct sdio_dev_s *dev)
{
int ret;
uint32_t data = 0;
/* Set device state from reset to idle */
sdio_sendcmdpoll(dev, MMCSD_CMD0, 0);
up_mdelay(SDIO_IDLE_DELAY_MS);
/* Device is SDIO card compatible so we can send CMD5 instead of ACMD41 */
sdio_sendcmdpoll(dev, SDIO_CMD5, 0);
/* Receive R4 response */
ret = SDIO_RECVR4(dev, SDIO_CMD5, &data);
if (ret != OK)
{
return ret;
}
/* Device is in Card Identification Mode, request device RCA */
sdio_sendcmdpoll(dev, SD_CMD3, 0);
ret = SDIO_RECVR6(dev, SD_CMD3, &data);
if (ret != OK)
{
_err("ERROR: RCA request failed: %d\n", ret);
return ret;
}
_info("rca is %x\n", data >> 16);
/* Send CMD7 with the argument == RCA in order to select the card
* and put it in Transfer State */
sdio_sendcmdpoll(dev, MMCSD_CMD7S, data & 0xffff0000);
ret = SDIO_RECVR1(dev, MMCSD_CMD7S, &data);
if (ret != OK)
{
_err("ERROR: card selection failed: %d\n", ret);
return ret;
}
/* Configure 4 bits bus width */
ret = sdio_set_wide_bus(dev);
if (ret != OK)
{
return ret;
}
return OK;
}
int sdio_set_blocksize(FAR struct sdio_dev_s *dev, uint8_t function,
uint16_t blocksize)
{
int ret;
ret = sdio_io_rw_direct(dev, true, 0,
(function << SDIO_FBR_SHIFT) + SDIO_CCCR_FN0_BLKSIZE_0,
blocksize & 0xff, NULL);
if (ret != OK)
{
return ret;
}
ret = sdio_io_rw_direct(dev, true, 0,
(function << SDIO_FBR_SHIFT) + SDIO_CCCR_FN0_BLKSIZE_1,
(blocksize >> 8), NULL);
if (ret != OK)
{
return ret;
}
return OK;
}
int sdio_enable_function(FAR struct sdio_dev_s *dev, uint8_t function)
{
int ret;
uint8_t value;
/* Read current I/O Enable register */
ret = sdio_io_rw_direct(dev, false, 0, SDIO_CCCR_IOEN, 0, &value);
if (ret != OK)
{
return ret;
}
ret = sdio_io_rw_direct(dev, true, 0, SDIO_CCCR_IOEN, value | (1 << function), NULL);
if (ret != OK)
{
return ret;
}
/* Wait 10ms for function to be enabled */
int loops = 10;
while (loops-- > 0)
{
up_mdelay(1);
ret = sdio_io_rw_direct(dev, false, 0, SDIO_CCCR_IOEN, 0, &value);
if (ret != OK)
{
return ret;
}
if (value & (1 << function))
{
/* Function enabled */
_info("Function %d enabled\n", function);
return OK;
}
}
return -ETIMEDOUT;
}
int sdio_enable_interrupt(FAR struct sdio_dev_s *dev, uint8_t function)
{
int ret;
uint8_t value;
/* Read current Int Enable register */
ret = sdio_io_rw_direct(dev, false, 0, SDIO_CCCR_INTEN, 0, &value);
if (ret != OK)
{
return ret;
}
return sdio_io_rw_direct(dev, true, 0, SDIO_CCCR_INTEN, value | (1 << function), NULL);
}

View File

@ -328,6 +328,44 @@
#define SDIO_ACMD52 (SDIO_ACMDIDX52|MMCSD_R5_RESPONSE |MMCSD_NODATAXFR)
#define SDIO_ACMD53 (SDIO_ACMDIDX53|MMCSD_R5_RESPONSE |MMCSD_NODATAXFR)
/* SDIO Card Common Control Registers definitions
* see https://www.sdcard.org/developers/overview/sdio/
* sdio_spec/Simplified_SDIO_Card_Spec.pdf */
#define SDIO_CCCR_REV 0x00 /* CCCR/SDIO Revision */
#define SDIO_CCCR_SD_SPEC_REV 0x01 /* SD Specification Revision */
#define SDIO_CCCR_IOEN 0x02 /* I/O Enable */
#define SDIO_CCCR_IORDY 0x03 /* I/O Ready */
#define SDIO_CCCR_INTEN 0x04 /* Int Enable */
#define SDIO_CCCR_INTPEND 0x05 /* Int Pending */
#define SDIO_CCCR_IOABORT 0x06 /* I/O Abort */
#define SDIO_CCCR_BUS_IF 0x07 /* Bus Interface Control */
#define SDIO_CCCR_CARD_CAP 0x08 /* Card Capabilitiy */
#define SDIO_CCCR_CCP 0x09 /* Common CIS Pointer */
#define SDIO_CCCR_BUS_SUSP 0x0C /* Bus Suspend */
#define SDIO_CCCR_FUNCSEL 0x0D /* Function Select */
#define SDIO_CCCR_EXEC_FLAGS 0x0E /* Exec Flags */
#define SDIO_CCCR_RDY_FLAGS 0x0F /* Ready Flags */
#define SDIO_CCCR_FN0_BLKSIZE_0 0x10 /* FN0 Block Size */
#define SDIO_CCCR_FN0_BLKSIZE_1 0x11 /* FN0 Block Size */
#define SDIO_CCCR_POWER 0x12 /* Power Control */
#define SDIO_CCCR_HIGHSPEED 0x13 /* High-Speed */
#define SDIO_CCCR_RFU 0x14 /* Reserved for future use */
#define SDIO_CCCR_VENDOR 0xF0 /* Reserved for Vendors */
#define SDIO_CCCR_BUS_IF_WIDTH_MASK 0x03 /* Bus width configuration */
#define SDIO_CCCR_BUS_IF_1_BIT 0x01 /* 1 bit bus width setting */
#define SDIO_CCCR_BUS_IF_4_BITS 0x02 /* 4 bits bus width setting */
#define SDIO_FBR_SHIFT 8 /* FBR bit shift */
#define SDIO_FN1_BR_BASE (1 << SDIO_FBR_SHIFT) /* Func 1 registers base */
#define SDIO_FN2_BR_BASE (2 << SDIO_FBR_SHIFT) /* Func 2 registers base */
#define SDIO_FN3_BR_BASE (3 << SDIO_FBR_SHIFT) /* Func 3 registers base */
#define SDIO_FN4_BR_BASE (4 << SDIO_FBR_SHIFT) /* Func 4 registers base */
#define SDIO_FN5_BR_BASE (5 << SDIO_FBR_SHIFT) /* Func 5 registers base */
#define SDIO_FN6_BR_BASE (6 << SDIO_FBR_SHIFT) /* Func 6 registers base */
#define SDIO_FN7_BR_BASE (7 << SDIO_FBR_SHIFT) /* Func 7 registers base */
/****************************************************************************
* Name: SDIO_LOCK
*

View File

@ -102,6 +102,22 @@ void bcmf_board_power(int minor, bool power);
void bcmf_board_reset(int minor, bool reset);
/************************************************************************************
* Function: bcmf_board_setup_oob_irq
*
* Description:
* Board specific function called from Broadcom FullMAC driver
* that must be implemented to use WLAN chip interrupt signal
*
* Parameters:
* minor - zero based minor device number which is unique
* for each wlan device.
* func - WLAN chip callback function that must be called on gpio event
* arg - WLAN chip internal structure that must be passed to callback
************************************************************************************/
void bcmf_board_setup_oob_irq(int minor, xcpt_t func, void *arg);
#undef EXTERN
#ifdef __cplusplus
}

View File

@ -0,0 +1,28 @@
#ifndef caca
#define caca
#include <stdint.h>
#include <nuttx/sdio.h>
int sdio_probe(FAR struct sdio_dev_s *dev);
int sdio_set_wide_bus(struct sdio_dev_s *dev);
int sdio_set_blocksize(FAR struct sdio_dev_s *dev, uint8_t function,
uint16_t blocksize);
int sdio_enable_function(FAR struct sdio_dev_s *dev, uint8_t function);
int sdio_enable_interrupt(FAR struct sdio_dev_s *dev, uint8_t function);
int sdio_sendcmdpoll(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t arg);
int sdio_io_rw_direct(FAR struct sdio_dev_s *dev, bool write,
uint8_t function, uint32_t address,
uint8_t inb, uint8_t* outb);
int sdio_io_rw_extended(FAR struct sdio_dev_s *dev, bool write,
uint8_t function, uint32_t address,
bool inc_addr, uint8_t *buf,
unsigned int blocklen, unsigned int nblocks);
#endif