Check return from nxsem_wait_uninterruptible()
Resolution of Issue 619 will require multiple steps, this part of the first step in that resolution: Every call to nxsem_wait_uninterruptible() must handle the return value from nxsem_wait_uninterruptible properly. This commit is for all HCI UART drivers under arch/.
This commit is contained in:
parent
8f85a57941
commit
d95d641597
@ -1,35 +1,20 @@
|
||||
/****************************************************************************
|
||||
* arch/arm/src/stm32/stm32_hciuart.c
|
||||
*
|
||||
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* 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.
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
@ -305,8 +290,8 @@ struct hciuart_config_s
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
static inline uint32_t hciuart_getreg32(const struct hciuart_config_s *config,
|
||||
unsigned int offset);
|
||||
static inline uint32_t hciuart_getreg32(
|
||||
const struct hciuart_config_s *config, unsigned int offset);
|
||||
static inline void hciuart_putreg32(const struct hciuart_config_s *config,
|
||||
unsigned int offset, uint32_t value);
|
||||
static void hciuart_enableints(const struct hciuart_config_s *config,
|
||||
@ -324,8 +309,9 @@ static uint16_t hciuart_rxinuse(const struct hciuart_config_s *config);
|
||||
static void hciuart_rxflow_enable(const struct hciuart_config_s *config);
|
||||
static void hciuart_rxflow_disable(const struct hciuart_config_s *config);
|
||||
static ssize_t hciuart_copytorxbuffer(const struct hciuart_config_s *config);
|
||||
static ssize_t hciuart_copyfromrxbuffer(const struct hciuart_config_s *config,
|
||||
uint8_t *dest, size_t destlen);
|
||||
static ssize_t hciuart_copyfromrxbuffer(
|
||||
const struct hciuart_config_s *config, uint8_t *dest,
|
||||
size_t destlen);
|
||||
static ssize_t hciuart_copytotxfifo(const struct hciuart_config_s *config);
|
||||
static void hciuart_line_configure(const struct hciuart_config_s *config);
|
||||
static void hciuart_apbclock_enable(const struct hciuart_config_s *config);
|
||||
@ -744,8 +730,9 @@ static struct pm_callback_s g_serialcb =
|
||||
* Name: hciuart_getreg32
|
||||
****************************************************************************/
|
||||
|
||||
static inline uint32_t hciuart_getreg32(const struct hciuart_config_s *config,
|
||||
unsigned int offset)
|
||||
static inline uint32_t
|
||||
hciuart_getreg32(const struct hciuart_config_s *config,
|
||||
unsigned int offset)
|
||||
{
|
||||
return getreg32(config->usartbase + offset);
|
||||
}
|
||||
@ -1112,7 +1099,8 @@ static ssize_t hciuart_copytorxbuffer(const struct hciuart_config_s *config)
|
||||
{
|
||||
/* Is there data available in the Rx FIFO? */
|
||||
|
||||
while ((hciuart_getreg32(config, STM32_USART_SR_OFFSET) & USART_SR_RXNE) != 0)
|
||||
while ((hciuart_getreg32(config, STM32_USART_SR_OFFSET) &
|
||||
USART_SR_RXNE) != 0)
|
||||
{
|
||||
/* Compare the Rx buffer head and tail indices. If the
|
||||
* incremented tail index would make the Rx buffer appear empty,
|
||||
@ -1177,8 +1165,9 @@ static ssize_t hciuart_copytorxbuffer(const struct hciuart_config_s *config)
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static ssize_t hciuart_copyfromrxbuffer(const struct hciuart_config_s *config,
|
||||
uint8_t *dest, size_t destlen)
|
||||
static ssize_t
|
||||
hciuart_copyfromrxbuffer(const struct hciuart_config_s *config,
|
||||
uint8_t *dest, size_t destlen)
|
||||
{
|
||||
struct hciuart_state_s *state;
|
||||
ssize_t nbytes;
|
||||
@ -1261,11 +1250,12 @@ static ssize_t hciuart_copytotxfifo(const struct hciuart_config_s *config)
|
||||
/* Is the transmit data register empty?
|
||||
*
|
||||
* TXE: Transmit data register empty
|
||||
* This bit is set by hardware when the content of the TDR register has
|
||||
* been transferred into the shift register.
|
||||
* This bit is set by hardware when the content of the TDR register
|
||||
* has been transferred into the shift register.
|
||||
*/
|
||||
|
||||
if ((hciuart_getreg32(config, STM32_USART_SR_OFFSET) & USART_SR_TXE) == 0)
|
||||
if ((hciuart_getreg32(config, STM32_USART_SR_OFFSET) &
|
||||
USART_SR_TXE) == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
@ -1389,11 +1379,11 @@ static void hciuart_line_configure(const struct hciuart_config_s *config)
|
||||
* baud = fCK / (16 * usartdiv)
|
||||
* usartdiv = fCK / (16 * baud)
|
||||
*
|
||||
* Where fCK is the input clock to the peripheral (PCLK1 for USART2, 3, 4, 5
|
||||
* or PCLK2 for USART1)
|
||||
* Where fCK is the input clock to the peripheral (PCLK1 for USART2, 3,
|
||||
* 4, 5 or PCLK2 for USART1)
|
||||
*
|
||||
* First calculate (NOTE: all stand baud values are even so dividing by two
|
||||
* does not lose precision):
|
||||
* First calculate (NOTE: all stand baud values are even so dividing by
|
||||
* two does not lose precision):
|
||||
*
|
||||
* usartdiv32 = 32 * usartdiv = fCK / (baud/2)
|
||||
*/
|
||||
@ -1633,7 +1623,8 @@ static int hciuart_configure(const struct hciuart_config_s *config)
|
||||
/* Clear CTSE, RTSE, and all interrupt enable bits */
|
||||
|
||||
regval = hciuart_getreg32(config, STM32_USART_CR3_OFFSET);
|
||||
regval &= ~(USART_CR3_CTSIE | USART_CR3_CTSE | USART_CR3_RTSE | USART_CR3_EIE);
|
||||
regval &= ~(USART_CR3_CTSIE | USART_CR3_CTSE | USART_CR3_RTSE |
|
||||
USART_CR3_EIE);
|
||||
|
||||
hciuart_putreg32(config, STM32_USART_CR3_OFFSET, regval);
|
||||
|
||||
@ -1699,7 +1690,8 @@ static int hciuart_configure(const struct hciuart_config_s *config)
|
||||
* interrupt received on the 'irq' It should call uart_transmitchars or
|
||||
* uart_receivechar to perform the appropriate data transfers. The
|
||||
* interrupt handling logic must be able to map the 'irq' number into the
|
||||
* appropriate btuart_lowerhalf_s structure in order to call these functions.
|
||||
* appropriate btuart_lowerhalf_s structure in order to call these
|
||||
* functions.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
@ -1737,24 +1729,27 @@ static int hciuart_interrupt(int irq, void *context, void *arg)
|
||||
|
||||
/* USART interrupts:
|
||||
*
|
||||
* Enable Status Meaning Usage
|
||||
* ------------------ --------------- ------------------------------- ----------
|
||||
* USART_CR1_IDLEIE USART_SR_IDLE Idle Line Detected (not used)
|
||||
* USART_CR1_RXNEIE USART_SR_RXNE Received Data Ready to be Read
|
||||
* " " USART_SR_ORE Overrun Error Detected
|
||||
* USART_CR1_TCIE USART_SR_TC Transmission Complete (only for RS-485)
|
||||
* USART_CR1_TXEIE USART_SR_TXE Transmit Data Register Empty
|
||||
* USART_CR1_PEIE USART_SR_PE Parity Error (No parity)
|
||||
* Enable Status Meaning Usage
|
||||
* ---------------- ------------- ----------------------- ----------
|
||||
* USART_CR1_IDLEIE USART_SR_IDLE Idle Line Detected (not used)
|
||||
* USART_CR1_RXNEIE USART_SR_RXNE Received Data Ready to
|
||||
* be Read
|
||||
* " " USART_SR_ORE Overrun Error Detected
|
||||
* USART_CR1_TCIE USART_SR_TC Transmission Complete (only for
|
||||
* RS-485)
|
||||
* USART_CR1_TXEIE USART_SR_TXE Transmit Data Register
|
||||
* Empty
|
||||
* USART_CR1_PEIE USART_SR_PE Parity Error (No parity)
|
||||
*
|
||||
* USART_CR2_LBDIE USART_SR_LBD Break Flag (not used)
|
||||
* USART_CR3_EIE USART_SR_FE Framing Error
|
||||
* " " USART_SR_NE Noise Error
|
||||
* " " USART_SR_ORE Overrun Error Detected
|
||||
* USART_CR3_CTSIE USART_SR_CTS CTS flag (not used)
|
||||
* USART_CR2_LBDIE USART_SR_LBD Break Flag (not used)
|
||||
* USART_CR3_EIE USART_SR_FE Framing Error
|
||||
* " " USART_SR_NE Noise Error
|
||||
* " " USART_SR_ORE Overrun Error Detected
|
||||
* USART_CR3_CTSIE USART_SR_CTS CTS flag (not used)
|
||||
*
|
||||
* NOTE: Some of these status bits must be cleared by explicitly writing zero
|
||||
* to the SR register: USART_SR_CTS, USART_SR_LBD. Note of those are currently
|
||||
* being used.
|
||||
* NOTE: Some of these status bits must be cleared by explicitly
|
||||
* writing zero to the SR register: USART_SR_CTS, USART_SR_LBD. Note
|
||||
* of those are currently being used.
|
||||
*/
|
||||
|
||||
/* Handle incoming, receive bytes (non-DMA only) */
|
||||
@ -1811,8 +1806,8 @@ static int hciuart_interrupt(int irq, void *context, void *arg)
|
||||
/* Handle outgoing, transmit bytes
|
||||
*
|
||||
* TXE: Transmit data register empty
|
||||
* This bit is set by hardware when the content of the TDR register has
|
||||
* been transferred into the shift register.
|
||||
* This bit is set by hardware when the content of the TDR register
|
||||
* has been transferred into the shift register.
|
||||
*/
|
||||
|
||||
if ((status & USART_SR_TXE) != 0 &&
|
||||
@ -1828,13 +1823,14 @@ static int hciuart_interrupt(int irq, void *context, void *arg)
|
||||
nbytes = hciuart_copytotxfifo(config);
|
||||
UNUSED(nbytes);
|
||||
|
||||
/* If the Tx buffer is now empty, then disable further Tx interrupts.
|
||||
* Tx interrupts will only be enabled in the following circumstances:
|
||||
/* If the Tx buffer is now empty, then disable further Tx
|
||||
* interrupts. Tx interrupts will only be enabled in the
|
||||
* following circumstances:
|
||||
*
|
||||
* 1. The user is waiting in hciuart_write() for space to become
|
||||
* available in the Tx FIFO.
|
||||
* 2. The full, outgoing message has been placed into the Tx buffer
|
||||
* by hciuart_write().
|
||||
* 2. The full, outgoing message has been placed into the Tx
|
||||
* buffer by hciuart_write().
|
||||
*
|
||||
* In either case, no more Tx interrupts will be needed until more
|
||||
* data is added to the Tx buffer.
|
||||
@ -1954,10 +1950,10 @@ static void hciuart_rxenable(const struct btuart_lowerhalf_s *lower,
|
||||
|
||||
/* En/disable DMA reception.
|
||||
*
|
||||
* Note that it is not safe to check for available bytes and immediately
|
||||
* pass them to uart_recvchars as that could potentially recurse back to
|
||||
* us again. Instead, bytes must wait until the next up_dma_poll or DMA
|
||||
* event.
|
||||
* Note that it is not safe to check for available bytes and
|
||||
* immediately pass them to uart_recvchars as that could potentially
|
||||
* recurse back to us again. Instead, bytes must wait until the next
|
||||
* up_dma_poll or DMA event.
|
||||
*/
|
||||
|
||||
state->rxenable = enable;
|
||||
@ -1972,17 +1968,18 @@ static void hciuart_rxenable(const struct btuart_lowerhalf_s *lower,
|
||||
|
||||
/* USART receive interrupts:
|
||||
*
|
||||
* Enable Status Meaning Usage
|
||||
* ------------------ --------------- ------------------------------- ----------
|
||||
* USART_CR1_IDLEIE USART_SR_IDLE Idle Line Detected (not used)
|
||||
* USART_CR1_RXNEIE USART_SR_RXNE Received Data Ready to be Read
|
||||
* " " USART_SR_ORE Overrun Error Detected
|
||||
* USART_CR1_PEIE USART_SR_PE Parity Error (No parity)
|
||||
* Enable Status Meaning Usage
|
||||
* ---------------- ------------- ----------------------- ----------
|
||||
* USART_CR1_IDLEIE USART_SR_IDLE Idle Line Detected (not used)
|
||||
* USART_CR1_RXNEIE USART_SR_RXNE Received Data Ready to
|
||||
* be Read
|
||||
* " " USART_SR_ORE Overrun Error Detected
|
||||
* USART_CR1_PEIE USART_SR_PE Parity Error (No parity)
|
||||
*
|
||||
* USART_CR2_LBDIE USART_SR_LBD Break Flag (not used)
|
||||
* USART_CR3_EIE USART_SR_FE Framing Error
|
||||
* " " USART_SR_NE Noise Error
|
||||
* " " USART_SR_ORE Overrun Error Detected
|
||||
* USART_CR2_LBDIE USART_SR_LBD Break Flag (not used)
|
||||
* USART_CR3_EIE USART_SR_FE Framing Error
|
||||
* " " USART_SR_NE Noise Error
|
||||
* " " USART_SR_ORE Overrun Error Detected
|
||||
*/
|
||||
|
||||
flags = spin_lock_irqsave();
|
||||
@ -2053,9 +2050,10 @@ static ssize_t hciuart_read(const struct btuart_lowerhalf_s *lower,
|
||||
struct hciuart_state_s *state;
|
||||
uint8_t *dest;
|
||||
size_t remaining;
|
||||
size_t ntotal;
|
||||
ssize_t ntotal;
|
||||
ssize_t nbytes;
|
||||
bool rxenable;
|
||||
int ret;
|
||||
|
||||
wlinfo("config %p buffer %p buflen %lu\n",
|
||||
config, buffer, (unsigned long)buflen);
|
||||
@ -2100,7 +2098,12 @@ static ssize_t hciuart_read(const struct btuart_lowerhalf_s *lower,
|
||||
state->rxwaiting = true;
|
||||
do
|
||||
{
|
||||
nxsem_wait_uninterruptible(&state->rxwait);
|
||||
ret = nxsem_wait_uninterruptible(&state->rxwait);
|
||||
if (ret < 0)
|
||||
{
|
||||
ntotal == (ssize_t)ret;
|
||||
break;
|
||||
}
|
||||
}
|
||||
while (state->rxwaiting);
|
||||
}
|
||||
@ -2169,8 +2172,9 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
uint16_t txhead;
|
||||
uint16_t txtail;
|
||||
uint16_t txnext;
|
||||
size_t remaining;
|
||||
ssize_t ntotal;
|
||||
irqstate_t flags;
|
||||
int ret;
|
||||
|
||||
wlinfo("config %p buffer %p buflen %lu\n",
|
||||
config, buffer, (unsigned long)buflen);
|
||||
@ -2186,10 +2190,11 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
* USART transmit interrupts:
|
||||
*
|
||||
* Enable Status Meaning Usage
|
||||
* ---------------- ------------- ---------------------------- ----------
|
||||
* USART_CR1_TCIE USART_SR_TC Transmission Complete (only for RS-485)
|
||||
* USART_CR1_TXEIE USART_SR_TXE Transmit Data Register Empty
|
||||
* USART_CR3_CTSIE USART_SR_CTS CTS flag (not used)
|
||||
* ---------------- ------------- ---------------------- ----------
|
||||
* USART_CR1_TCIE USART_SR_TC Transmission Complete (only for RS-485)
|
||||
* USART_CR1_TXEIE USART_SR_TXE Transmit Data Register
|
||||
* Empty
|
||||
* USART_CR3_CTSIE USART_SR_CTS CTS flag (not used)
|
||||
*/
|
||||
|
||||
flags = spin_lock_irqsave();
|
||||
@ -2198,10 +2203,10 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
|
||||
/* Loop until all of the user data have been moved to the Tx buffer */
|
||||
|
||||
src = buffer;
|
||||
remaining = buflen;
|
||||
src = buffer;
|
||||
ntotal = 0;
|
||||
|
||||
while (remaining > 0)
|
||||
while (ntotal < (ssize_t)buflen)
|
||||
{
|
||||
/* Copy bytes to the tail of the Tx buffer */
|
||||
|
||||
@ -2220,7 +2225,7 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
* copy?
|
||||
*/
|
||||
|
||||
while (txhead != txnext && remaining > 0)
|
||||
while (txhead != txnext && ntotal < (ssize_t)buflen)
|
||||
{
|
||||
/* Yes.. copy one byte to the Tx buffer */
|
||||
|
||||
@ -2232,7 +2237,7 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
txnext = 0;
|
||||
}
|
||||
|
||||
remaining--;
|
||||
ntotal++;
|
||||
}
|
||||
|
||||
/* Save the updated Tx buffer tail index */
|
||||
@ -2248,7 +2253,7 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
* space in the Tx* buffer then try again.
|
||||
*/
|
||||
|
||||
if (nbytes <= 0 && remaining > 0)
|
||||
if (nbytes <= 0 && ntotal < (ssize_t)buflen)
|
||||
{
|
||||
DEBUGASSERT(nbytes == 0);
|
||||
|
||||
@ -2263,7 +2268,16 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
state->txwaiting = true;
|
||||
do
|
||||
{
|
||||
nxsem_wait_uninterruptible(&state->txwait);
|
||||
ret = nxsem_wait_uninterruptible(&state->txwait);
|
||||
if (ret < 0)
|
||||
{
|
||||
if (ntotal == 0)
|
||||
{
|
||||
ntotal = (ssize_t)ret;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
while (state->txwaiting);
|
||||
|
||||
@ -2283,7 +2297,7 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
spin_unlock_irqrestore(flags);
|
||||
}
|
||||
|
||||
return buflen;
|
||||
return ntotal;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
@ -2438,6 +2452,7 @@ static void hciuart_pm_notify(struct pm_callback_s *cb, int domain,
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
/* Should not get here */
|
||||
|
||||
break;
|
||||
@ -2509,7 +2524,8 @@ static int hciuart_pm_prepare(struct pm_callback_s *cb, int domain,
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
const struct btuart_lowerhalf_s *hciuart_instantiate(enum hciuart_devno_e uart)
|
||||
const struct btuart_lowerhalf_s *
|
||||
hciuart_instantiate(enum hciuart_devno_e uart)
|
||||
{
|
||||
const struct hciuart_config_s *config;
|
||||
#ifdef CONFIG_PM
|
||||
@ -2586,8 +2602,8 @@ void hciuart_initialize(void)
|
||||
ret = irq_attach(config->irq, hciuart_interrupt, (void *)config);
|
||||
if (ret == OK)
|
||||
{
|
||||
/* Enable the interrupt (RX and TX interrupts are still disabled
|
||||
* in the USART)
|
||||
/* Enable the interrupt (RX and TX interrupts are still
|
||||
* disabled in the USART)
|
||||
*/
|
||||
|
||||
up_enable_irq(config->irq);
|
||||
|
@ -1,35 +1,20 @@
|
||||
/****************************************************************************
|
||||
* arch/arm/src/tiva/tiva_hciuart.c
|
||||
*
|
||||
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* 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.
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
@ -144,8 +129,8 @@ struct hciuart_config_s
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
static inline uint32_t hciuart_getreg32(const struct hciuart_config_s *config,
|
||||
unsigned int offset);
|
||||
static inline uint32_t hciuart_getreg32(
|
||||
const struct hciuart_config_s *config, unsigned int offset);
|
||||
static inline void hciuart_putreg32(const struct hciuart_config_s *config,
|
||||
unsigned int offset, uint32_t value);
|
||||
static void hciuart_enableints(const struct hciuart_config_s *config,
|
||||
@ -158,8 +143,9 @@ static inline bool hciuart_rxenabled(const struct hciuart_config_s *config);
|
||||
|
||||
static uint16_t hciuart_rxinuse(const struct hciuart_config_s *config);
|
||||
static ssize_t hciuart_copytorxbuffer(const struct hciuart_config_s *config);
|
||||
static ssize_t hciuart_copyfromrxbuffer(const struct hciuart_config_s *config,
|
||||
uint8_t *dest, size_t destlen);
|
||||
static ssize_t hciuart_copyfromrxbuffer(
|
||||
const struct hciuart_config_s *config, uint8_t *dest,
|
||||
size_t destlen);
|
||||
static ssize_t hciuart_copytotxfifo(const struct hciuart_config_s *config);
|
||||
static void hciuart_line_configure(const struct hciuart_config_s *config);
|
||||
static int hciuart_configure(const struct hciuart_config_s *config);
|
||||
@ -603,8 +589,9 @@ static struct pm_callback_s g_serialcb =
|
||||
* Name: hciuart_getreg32
|
||||
****************************************************************************/
|
||||
|
||||
static inline uint32_t hciuart_getreg32(const struct hciuart_config_s *config,
|
||||
unsigned int offset)
|
||||
static inline uint32_t
|
||||
hciuart_getreg32(const struct hciuart_config_s *config,
|
||||
unsigned int offset)
|
||||
{
|
||||
return getreg32(config->uartbase + offset);
|
||||
}
|
||||
@ -828,8 +815,9 @@ static ssize_t hciuart_copytorxbuffer(const struct hciuart_config_s *config)
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static ssize_t hciuart_copyfromrxbuffer(const struct hciuart_config_s *config,
|
||||
uint8_t *dest, size_t destlen)
|
||||
static ssize_t
|
||||
hciuart_copyfromrxbuffer(const struct hciuart_config_s *config,
|
||||
uint8_t *dest, size_t destlen)
|
||||
{
|
||||
struct hciuart_state_s *state;
|
||||
ssize_t nbytes;
|
||||
@ -919,7 +907,8 @@ static ssize_t hciuart_copytotxfifo(const struct hciuart_config_s *config)
|
||||
* register has been transferred into the shift register.
|
||||
*/
|
||||
|
||||
if ((hciuart_getreg32(config, TIVA_UART_FR_OFFSET) & UART_FR_TXFF) != 0)
|
||||
if ((hciuart_getreg32(config, TIVA_UART_FR_OFFSET) &
|
||||
UART_FR_TXFF) != 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
@ -1051,7 +1040,7 @@ static int hciuart_configure(const struct hciuart_config_s *config)
|
||||
config->state->im = hciuart_getreg32(config, TIVA_UART_IM_OFFSET);
|
||||
|
||||
hciuart_putreg32(config, TIVA_UART_IFLS_OFFSET,
|
||||
UART_IFLS_TXIFLSEL_18th | UART_IFLS_RXIFLSEL_78th);
|
||||
UART_IFLS_TXIFLSEL_18th | UART_IFLS_RXIFLSEL_78th);
|
||||
|
||||
hciuart_putreg32(config, TIVA_UART_IM_OFFSET, UART_IM_RXIM | UART_IM_RTIM);
|
||||
|
||||
@ -1370,9 +1359,10 @@ static ssize_t hciuart_read(const struct btuart_lowerhalf_s *lower,
|
||||
struct hciuart_state_s *state;
|
||||
uint8_t *dest;
|
||||
size_t remaining;
|
||||
size_t ntotal;
|
||||
ssize_t ntotal;
|
||||
ssize_t nbytes;
|
||||
bool rxenable;
|
||||
int ret;
|
||||
|
||||
wlinfo("config %p buffer %p buflen %lu\n",
|
||||
config, buffer, (unsigned long)buflen);
|
||||
@ -1417,7 +1407,12 @@ static ssize_t hciuart_read(const struct btuart_lowerhalf_s *lower,
|
||||
state->rxwaiting = true;
|
||||
do
|
||||
{
|
||||
nxsem_wait_uninterruptible(&state->rxwait);
|
||||
ret = nxsem_wait_uninterruptible(&state->rxwait);
|
||||
if (ret < 0)
|
||||
{
|
||||
ntotal = (ssize_t)ret;
|
||||
break;
|
||||
}
|
||||
}
|
||||
while (state->rxwaiting);
|
||||
}
|
||||
@ -1486,8 +1481,9 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
uint16_t txhead;
|
||||
uint16_t txtail;
|
||||
uint16_t txnext;
|
||||
size_t remaining;
|
||||
ssize_t ntotal;
|
||||
irqstate_t flags;
|
||||
int ret;
|
||||
|
||||
DEBUGASSERT(config != NULL && config->state != NULL);
|
||||
state = config->state;
|
||||
@ -1504,10 +1500,10 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
|
||||
/* Loop until all of the user data have been moved to the Tx buffer */
|
||||
|
||||
src = buffer;
|
||||
remaining = buflen;
|
||||
src = buffer;
|
||||
ntotal = 0;
|
||||
|
||||
while (remaining > 0)
|
||||
while (ntotal < (ssize_t)buflen)
|
||||
{
|
||||
/* Copy bytes to the tail of the Tx buffer */
|
||||
|
||||
@ -1526,7 +1522,7 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
* copy?
|
||||
*/
|
||||
|
||||
while (txhead != txnext && remaining > 0)
|
||||
while (txhead != txnext && ntotal < (ssize_t)buflen)
|
||||
{
|
||||
/* Yes.. copy one byte to the Tx buffer */
|
||||
|
||||
@ -1538,7 +1534,7 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
txnext = 0;
|
||||
}
|
||||
|
||||
remaining--;
|
||||
ntotal++;
|
||||
}
|
||||
|
||||
/* Save the updated Tx buffer tail index */
|
||||
@ -1554,7 +1550,7 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
* space in the Tx* buffer then try again.
|
||||
*/
|
||||
|
||||
if (nbytes <= 0 && remaining > 0)
|
||||
if (nbytes <= 0 && ntotal < buflen)
|
||||
{
|
||||
DEBUGASSERT(nbytes == 0);
|
||||
|
||||
@ -1569,7 +1565,16 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
state->txwaiting = true;
|
||||
do
|
||||
{
|
||||
nxsem_wait_uninterruptible(&state->txwait);
|
||||
ret = nxsem_wait_uninterruptible(&state->txwait);
|
||||
if (ret < 0)
|
||||
{
|
||||
if (ntotal == 0)
|
||||
{
|
||||
ntotal = (ssize_t)ret;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
while (state->txwaiting);
|
||||
|
||||
@ -1589,7 +1594,7 @@ static ssize_t hciuart_write(const struct btuart_lowerhalf_s *lower,
|
||||
spin_unlock_irqrestore(flags);
|
||||
}
|
||||
|
||||
return buflen;
|
||||
return ntotal;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
@ -1853,8 +1858,8 @@ void hciuart_initialize(void)
|
||||
ret = irq_attach(config->irq, hciuart_interrupt, (void *)config);
|
||||
if (ret == OK)
|
||||
{
|
||||
/* Enable the interrupt (RX and TX interrupts are still disabled
|
||||
* in the UART)
|
||||
/* Enable the interrupt (RX and TX interrupts are still
|
||||
* disabled in the UART)
|
||||
*/
|
||||
|
||||
up_enable_irq(config->irq);
|
||||
|
Loading…
Reference in New Issue
Block a user