9
0
Fork 0

HACK: Wrapper around Osmocom-BB driver for SPI and sercomm

Brings up a console and power control but has issues and should be replaced.
This commit is contained in:
Stefan Richter 2011-06-22 23:24:42 +02:00
parent 6c7abe3781
commit d1cd20aa54
11 changed files with 1076 additions and 1 deletions

View File

@ -50,4 +50,4 @@ CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
CHIP_ASRCS = calypso_lowputc.S
CHIP_CSRCS = calypso_irq.c calypso_timer.c calypso_heap.c calypso_armio.c \
calypso_keypad.c clock.c
calypso_keypad.c calypso_serial.c calypso_spi.c clock.c

View File

@ -0,0 +1,141 @@
/****************************************************************************
* calypso_spi.c
* SPI driver for TI Calypso
*
* Copyright (C) 2011 Stefan Richter <ichgeh@l--putt.de>
*
* 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 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.
*
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/spi.h>
#warning "MOST OF SPI API IS INCOMPLETE! (Wrapper around Osmocom driver)"
extern void spi_init(void);
extern int spi_xfer(uint8_t dev_idx, uint8_t bitlen, const void *dout, void *din);
#ifndef CONFIG_SPI_EXCHANGE
#error "Calypso HW only supports exchange. Enable CONFIG_SPI_EXCHANGE!"
#endif
struct calypso_spidev_s
{
struct spi_dev_s spidev; /* External driver interface */
int nbits; /* Number of transfered bits */
#ifndef CONFIG_SPI_OWNBUS
sem_t exclsem; /* Mutual exclusion of devices */
#endif
};
/* STUBS! */
#ifndef CONFIG_SPI_OWNBUS
static int spi_lock(FAR struct spi_dev_s *dev, bool lock);
#endif
static void spi_select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
bool selected)
{
}
static uint32_t spi_setfrequency(FAR struct spi_dev_s *dev, uint32_t frequency)
{
return frequency;
}
static void spi_setmode(FAR struct spi_dev_s *dev, enum spi_mode_e mode)
{
}
/* Osmocom wrapper */
static void spi_setbits(FAR struct spi_dev_s *dev, int nbits)
{
((FAR struct calypso_spidev_s *)dev)->nbits = nbits;
}
static void spi_exchange(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
FAR void *rxbuffer, size_t nwords)
{
FAR struct calypso_spidev_s *priv = (FAR struct calypso_spidev_s *)dev;
size_t i;
for(i=0; i<nwords; i++)
spi_xfer(0, priv->nbits, txbuffer+i, rxbuffer+i);
}
static uint16_t spi_send(FAR struct spi_dev_s *dev, uint16_t wd)
{
uint16_t buf = wd;
spi_exchange(dev, &buf, &buf, 1);
return buf;
}
static const struct spi_ops_s g_spiops =
{
#ifndef CONFIG_SPI_OWNBUS
.lock = spi_lock,
#endif
.select = spi_select,
.setfrequency = spi_setfrequency,
.setmode = spi_setmode,
.setbits = spi_setbits,
.status = 0,
#ifdef CONFIG_SPI_CMDDATA
.cmddata = ,
#endif
.send = spi_send,
#ifdef CONFIG_SPI_EXCHANGE
.exchange = spi_exchange,
#else
.sndblock = spi_sndblock,
.recvblock = spi_recvblock,
#endif
.registercallback = 0,
};
static struct calypso_spidev_s g_spidev =
{
.spidev = { &g_spiops },
.nbits = 0,
};
FAR struct spi_dev_s *up_spiinitialize(int port)
{
switch(port) {
case 0: /* SPI master device */
spi_init();
return (FAR struct spi_dev_s *)&g_spidev;
case 1: /* uWire device */
return NULL;
default:
return NULL;
}
}

View File

@ -48,6 +48,7 @@ VPATH = .
# files to the source file list, add its DEPPATH info, and will add
# the appropriate paths to the VPATH variable
include sercomm/Make.defs
include serial/Make.defs
include net/Make.defs
include pipes/Make.defs

View File

@ -0,0 +1,46 @@
############################################################################
# drivers/serial/Make.defs
#
# Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
#
# 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.
#
############################################################################
ifneq ($(CONFIG_NFILE_DESCRIPTORS),0)
# Include serial drivers
CSRCS += console.c uart.c
# Include sercomm build support
DEPPATH += --dep-path sercomm
VPATH += :sercomm
endif

View File

@ -0,0 +1,152 @@
/****************************************************************************
* arch/arm/src/calypso/calypso_serial.c
*
* (C) 2011 Stefan Richter <ichgeh@l--putt.de>
* All Rights Reserved
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <nuttx/fs.h>
#include <nuttx/serial.h>
#include <errno.h>
#include <debug.h>
#include <string.h>
#include "uart.h"
#include <sercomm/sercomm.h>
/* stubs to make serial driver happy */
void sercomm_recvchars(void *a) { }
void sercomm_xmitchars(void *a) { }
/* Stubs to make memory allocator happy */
void cons_puts(void *foo){}
void delay_ms(int ms){}
/************************************************************************************
* Fileops Prototypes and Structures
************************************************************************************/
typedef FAR struct file file_t;
static ssize_t sc_console_read(file_t *filep, FAR char *buffer, size_t buflen);
static ssize_t sc_console_write(file_t *filep, FAR const char *buffer, size_t buflen);
static int sc_console_ioctl(file_t *filep, int cmd, unsigned long arg);
#ifndef CONFIG_DISABLE_POLL
static int sc_console_poll(file_t *filep, FAR struct pollfd *fds, bool setup);
#endif
static const struct file_operations g_sercom_console_ops =
{
0, /* open, always opened */
0, /* close, stays open */
sc_console_read, /* read */
sc_console_write, /* write */
0, /* seek, not supported */
sc_console_ioctl, /* ioctl */
#ifndef CONFIG_DISABLE_POLL
sc_console_poll /* poll */
#endif
};
/****************************************************************************
* Helper functions
****************************************************************************/
static FAR uart_dev_t *readdev = NULL;
static struct msgb *recvmsg = NULL;
static void recv_cb(uint8_t dlci, struct msgb *msg)
{
sem_post(&readdev->recvsem);
recvmsg = msg;
}
/****************************************************************************
* Fileops
****************************************************************************/
/* XXX: recvmsg is overwritten when multiple msg arrive! */
static ssize_t sc_console_read(file_t *filep, FAR char *buffer, size_t buflen)
{
size_t len;
struct msgb *tmp;
/* Wait until data is received */
while(recvmsg == NULL) {
sem_wait(&readdev->recvsem);
}
len = recvmsg->len > buflen ? buflen : recvmsg->len;
memcpy(buffer, msgb_get(recvmsg, len), len);
if(recvmsg->len == 0) {
/* prevent inconsistent msg by first invalidating it, then free it */
tmp = recvmsg;
recvmsg = NULL;
msgb_free(tmp);
}
return len;
}
/* XXX: redirect to old Osmocom-BB comm/sercomm_cons.c -> 2 buffers */
extern int sercomm_write(void *file, const char *s, const int len);
static ssize_t sc_console_write(file_t *filep, FAR const char *buffer, size_t buflen)
{
int ret = sercomm_write(filep, buffer, buflen);
if(ret < 0)
return ret;
else
return buflen;
}
/* Forward ioctl to uart driver */
static int sc_console_ioctl(struct file *filep, int cmd, unsigned long arg)
{
FAR struct inode *inode = filep->f_inode;
FAR uart_dev_t *dev = inode->i_private;
return dev->ops->ioctl(filep, cmd, arg);
}
/************************************************************************************
* Public Functions
************************************************************************************/
/* Use sercomm on uart driver, register console driver */
int sercomm_register(FAR const char *path, FAR uart_dev_t *dev)
{
/* XXX: initialize MODEMUART to be used for sercomm*/
uart_init(SERCOMM_UART_NR, 1);
uart_baudrate(SERCOMM_UART_NR, UART_115200);
readdev = dev;
sercomm_register_rx_cb(SC_DLCI_LOADER, &recv_cb);
sem_init(&dev->xmit.sem, 0, 1);
sem_init(&dev->recv.sem, 0, 1);
sem_init(&dev->closesem, 0, 1);
sem_init(&dev->xmitsem, 0, 0);
sem_init(&dev->recvsem, 0, 0);
#ifndef CONFIG_DISABLE_POLL
sem_init(&dev->pollsem, 0, 1);
#endif
dbg("Registering %s\n", path);
return register_driver(path, &g_sercom_console_ops, 0666, NULL);
}

View File

@ -0,0 +1,19 @@
#!/usr/bin/python
from socket import *
import time
SOCKET_NAME = '/tmp/osmocom_loader'
s = socket(AF_UNIX, SOCK_STREAM)
s.connect(SOCKET_NAME)
while 1:
try:
x = raw_input(">")
y = len(x) + 1
s.send(chr(y>>8) + chr(y&255) + x + "\n")
except:
print ''
break
s.close()

View File

@ -0,0 +1,456 @@
/* Calypso DBB internal UART Driver */
/* (C) 2010 by Harald Welte <laforge@gnumonks.org>
* (C) 2010 by Ingo Albrecht <prom@berlin.ccc.de>
*
* All Rights Reserved
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*
*/
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <nuttx/config.h>
#include <nuttx/irq.h>
#include <nuttx/arch.h>
#include <arch/calypso/memory.h>
#include <arch/calypso/debug.h>
#include <arch/calypso/defines.h>
//#include <arch/calypso/console.h>
#include <sercomm/sercomm.h>
#include "uart.h"
#define BASE_ADDR_UART_MODEM 0xffff5000
#define OFFSET_IRDA 0x800
#define UART_REG(n,m) (BASE_ADDR_UART_MODEM + ((n)*OFFSET_IRDA)+(m))
#define LCR7BIT 0x80
#define LCRBFBIT 0x40
#define MCR6BIT 0x20
#define REG_OFFS(m) ((m) & ~(LCR7BIT|LCRBFBIT|MCR6BIT))
/* read access LCR[7] = 0 */
enum uart_reg {
RHR = 0,
IER = 1,
IIR = 2,
LCR = 3,
MCR = 4,
LSR = 5,
MSR = 6,
SPR = 7,
MDR1 = 8,
DMR2 = 9,
SFLSR = 0x0a,
RESUME = 0x0b,
SFREGL = 0x0c,
SFREGH = 0x0d,
BLR = 0x0e,
ACREG = 0x0f,
SCR = 0x10,
SSR = 0x11,
EBLR = 0x12,
/* read access LCR[7] = 1 */
DLL = RHR | LCR7BIT,
DLH = IER | LCR7BIT,
DIV1_6 = ACREG | LCR7BIT,
/* read/write access LCR[7:0] = 0xbf */
EFR = IIR | LCRBFBIT,
XON1 = MCR | LCRBFBIT,
XON2 = LSR | LCRBFBIT,
XOFF1 = MSR | LCRBFBIT,
XOFF2 = SPR | LCRBFBIT,
/* read/write access if EFR[4] = 1 and MCR[6] = 1 */
TCR = MSR | MCR6BIT,
TLR = SPR | MCR6BIT,
};
/* write access LCR[7] = 0 */
#define THR RHR
#define FCR IIR /* only if EFR[4] = 1 */
#define TXFLL SFLSR
#define TXFLH RESUME
#define RXFLL SFREGL
#define RXFLH SFREGH
enum fcr_bits {
FIFO_EN = (1 << 0),
RX_FIFO_CLEAR = (1 << 1),
TX_FIFO_CLEAR = (1 << 2),
DMA_MODE = (1 << 3),
};
#define TX_FIFO_TRIG_SHIFT 4
#define RX_FIFO_TRIG_SHIFT 6
enum iir_bits {
IIR_INT_PENDING = 0x01,
IIR_INT_TYPE = 0x3E,
IIR_INT_TYPE_RX_STATUS_ERROR = 0x06,
IIR_INT_TYPE_RX_TIMEOUT = 0x0C,
IIR_INT_TYPE_RHR = 0x04,
IIR_INT_TYPE_THR = 0x02,
IIR_INT_TYPE_MSR = 0x00,
IIR_INT_TYPE_XOFF = 0x10,
IIR_INT_TYPE_FLOW = 0x20,
IIR_FCR0_MIRROR = 0xC0,
};
#define UART_REG_UIR 0xffff6000
/* enable or disable the divisor latch for access to DLL, DLH */
static void uart_set_lcr7bit(int uart, int on)
{
uint8_t reg;
reg = readb(UART_REG(uart, LCR));
if (on)
reg |= (1 << 7);
else
reg &= ~(1 << 7);
writeb(reg, UART_REG(uart, LCR));
}
static uint8_t old_lcr;
static void uart_set_lcr_bf(int uart, int on)
{
if (on) {
old_lcr = readb(UART_REG(uart, LCR));
writeb(0xBF, UART_REG(uart, LCR));
} else {
writeb(old_lcr, UART_REG(uart, LCR));
}
}
/* Enable or disable the TCR_TLR latch bit in MCR[6] */
static void uart_set_mcr6bit(int uart, int on)
{
uint8_t mcr;
/* we assume EFR[4] is always set to 1 */
mcr = readb(UART_REG(uart, MCR));
if (on)
mcr |= (1 << 6);
else
mcr &= ~(1 << 6);
writeb(mcr, UART_REG(uart, MCR));
}
static void uart_reg_write(int uart, enum uart_reg reg, uint8_t val)
{
if (reg & LCRBFBIT)
uart_set_lcr_bf(uart, 1);
else if (reg & LCR7BIT)
uart_set_lcr7bit(uart, 1);
else if (reg & MCR6BIT)
uart_set_mcr6bit(uart, 1);
writeb(val, UART_REG(uart, REG_OFFS(reg)));
if (reg & LCRBFBIT)
uart_set_lcr_bf(uart, 0);
else if (reg & LCR7BIT)
uart_set_lcr7bit(uart, 0);
else if (reg & MCR6BIT)
uart_set_mcr6bit(uart, 0);
}
/* read from a UART register, applying any required latch bits */
static uint8_t uart_reg_read(int uart, enum uart_reg reg)
{
uint8_t ret;
if (reg & LCRBFBIT)
uart_set_lcr_bf(uart, 1);
else if (reg & LCR7BIT)
uart_set_lcr7bit(uart, 1);
else if (reg & MCR6BIT)
uart_set_mcr6bit(uart, 1);
ret = readb(UART_REG(uart, REG_OFFS(reg)));
if (reg & LCRBFBIT)
uart_set_lcr_bf(uart, 0);
else if (reg & LCR7BIT)
uart_set_lcr7bit(uart, 0);
else if (reg & MCR6BIT)
uart_set_mcr6bit(uart, 0);
return ret;
}
#if 0
static void uart_irq_handler_cons(__unused enum irq_nr irqnr)
{
const uint8_t uart = CONS_UART_NR;
uint8_t iir;
//uart_putchar_nb(uart, 'U');
iir = uart_reg_read(uart, IIR);
if (iir & IIR_INT_PENDING)
return;
switch (iir & IIR_INT_TYPE) {
case IIR_INT_TYPE_RHR:
break;
case IIR_INT_TYPE_THR:
if (cons_rb_flush() == 1) {
/* everything was flushed, disable THR IRQ */
uint8_t ier = uart_reg_read(uart, IER);
ier &= ~(1 << 1);
uart_reg_write(uart, IER, ier);
}
break;
case IIR_INT_TYPE_MSR:
break;
case IIR_INT_TYPE_RX_STATUS_ERROR:
break;
case IIR_INT_TYPE_RX_TIMEOUT:
break;
case IIR_INT_TYPE_XOFF:
break;
}
}
#endif
static void uart_irq_handler_sercomm(__unused enum irq_nr irqnr, __unused void *context)
{
const uint8_t uart = SERCOMM_UART_NR;
uint8_t iir, ch;
//uart_putchar_nb(uart, 'U');
iir = uart_reg_read(uart, IIR);
if (iir & IIR_INT_PENDING)
return;
switch (iir & IIR_INT_TYPE) {
case IIR_INT_TYPE_RX_TIMEOUT:
case IIR_INT_TYPE_RHR:
/* as long as we have rx data available */
while (uart_getchar_nb(uart, &ch)) {
if (sercomm_drv_rx_char(ch) < 0) {
/* sercomm cannot receive more data right now */
uart_irq_enable(uart, UART_IRQ_RX_CHAR, 0);
}
}
break;
case IIR_INT_TYPE_THR:
/* as long as we have space in the FIFO */
while (!uart_tx_busy(uart)) {
/* get a byte from sercomm */
if (!sercomm_drv_pull(&ch)) {
/* no more bytes in sercomm, stop TX interrupts */
uart_irq_enable(uart, UART_IRQ_TX_EMPTY, 0);
break;
}
/* write the byte into the TX FIFO */
uart_putchar_nb(uart, ch);
}
break;
case IIR_INT_TYPE_MSR:
printf("UART IRQ MSR\n");
break;
case IIR_INT_TYPE_RX_STATUS_ERROR:
printf("UART IRQ RX_SE\n");
break;
case IIR_INT_TYPE_XOFF:
printf("UART IRQXOFF\n");
break;
}
}
static const uint8_t uart2irq[] = {
[0] = IRQ_UART_IRDA,
[1] = IRQ_UART_MODEM,
};
void uart_init(uint8_t uart, uint8_t interrupts)
{
uint8_t irq = uart2irq[uart];
uart_reg_write(uart, IER, 0x00);
if (uart == SERCOMM_UART_NR) {
sercomm_init();
irq_attach(IRQ_UART_MODEM, (xcpt_t)uart_irq_handler_sercomm);
up_enable_irq(IRQ_UART_MODEM);
uart_irq_enable(uart, UART_IRQ_RX_CHAR, 1);
}
#if 0
if (uart == CONS_UART_NR) {
cons_init();
if(interrupts) {
irq_register_handler(irq, &uart_irq_handler_cons);
irq_config(irq, 0, 0, 0xff);
irq_enable(irq);
}
} else {
sercomm_init();
if(interrupts) {
irq_register_handler(irq, &uart_irq_handler_sercomm);
irq_config(irq, 0, 0, 0xff);
irq_enable(irq);
}
uart_irq_enable(uart, UART_IRQ_RX_CHAR, 1);
}
#endif
#if 0
if (uart == 1) {
/* assign UART to MCU and unmask interrupts*/
writeb(UART_REG_UIR, 0x00);
}
#endif
/* if we don't initialize these, we get strange corruptions in the
received data... :-( */
uart_reg_write(uart, MDR1, 0x07); /* turn off UART */
uart_reg_write(uart, XON1, 0x00); /* Xon1/Addr Register */
uart_reg_write(uart, XON2, 0x00); /* Xon2/Addr Register */
uart_reg_write(uart, XOFF1, 0x00); /* Xoff1 Register */
uart_reg_write(uart, XOFF2, 0x00); /* Xoff2 Register */
uart_reg_write(uart, EFR, 0x00); /* Enhanced Features Register */
/* select UART mode */
uart_reg_write(uart, MDR1, 0);
/* no XON/XOFF flow control, ENHANCED_EN, no auto-RTS/CTS */
uart_reg_write(uart, EFR, (1 << 4));
/* enable Tx/Rx FIFO, Tx trigger at 56 spaces, Rx trigger at 60 chars */
uart_reg_write(uart, FCR, FIFO_EN | RX_FIFO_CLEAR | TX_FIFO_CLEAR |
(3 << TX_FIFO_TRIG_SHIFT) | (3 << RX_FIFO_TRIG_SHIFT));
/* THR interrupt only when TX FIFO and TX shift register are empty */
uart_reg_write(uart, SCR, (1 << 0));// | (1 << 3));
/* 8 bit, 1 stop bit, no parity, no break */
uart_reg_write(uart, LCR, 0x03);
uart_set_lcr7bit(uart, 0);
}
void uart_poll(uint8_t uart) {
/* if(uart == CONS_UART_NR) {
uart_irq_handler_cons(0);
} else
*/ {
uart_irq_handler_sercomm(0, NULL);
}
}
void uart_irq_enable(uint8_t uart, enum uart_irq irq, int on)
{
uint8_t ier = uart_reg_read(uart, IER);
uint8_t mask = 0;
switch (irq) {
case UART_IRQ_TX_EMPTY:
mask = (1 << 1);
break;
case UART_IRQ_RX_CHAR:
mask = (1 << 0);
break;
}
if (on)
ier |= mask;
else
ier &= ~mask;
uart_reg_write(uart, IER, ier);
}
void uart_putchar_wait(uint8_t uart, int c)
{
/* wait while TX FIFO indicates full */
while (readb(UART_REG(uart, SSR)) & 0x01) { }
/* put character in TX FIFO */
writeb(c, UART_REG(uart, THR));
}
int uart_putchar_nb(uint8_t uart, int c)
{
/* if TX FIFO indicates full, abort */
if (readb(UART_REG(uart, SSR)) & 0x01)
return 0;
writeb(c, UART_REG(uart, THR));
return 1;
}
int uart_getchar_nb(uint8_t uart, uint8_t *ch)
{
uint8_t lsr;
lsr = readb(UART_REG(uart, LSR));
/* something strange happened */
if (lsr & 0x02)
printf("LSR RX_OE\n");
if (lsr & 0x04)
printf("LSR RX_PE\n");
if (lsr & 0x08)
printf("LSR RX_FE\n");
if (lsr & 0x10)
printf("LSR RX_BI\n");
if (lsr & 0x80)
printf("LSR RX_FIFO_STS\n");
/* is the Rx FIFO empty? */
if (!(lsr & 0x01))
return 0;
*ch = readb(UART_REG(uart, RHR));
//printf("getchar_nb(%u) = %02x\n", uart, *ch);
return 1;
}
int uart_tx_busy(uint8_t uart)
{
if (readb(UART_REG(uart, SSR)) & 0x01)
return 1;
return 0;
}
static const uint16_t divider[] = {
[UART_38400] = 21, /* 38,690 */
[UART_57600] = 14, /* 58,035 */
[UART_115200] = 7, /* 116,071 */
[UART_230400] = 4, /* 203,125! (-3% would be 223,488) */
[UART_460800] = 2, /* 406,250! (-3% would be 446,976) */
[UART_921600] = 1, /* 812,500! (-3% would be 893,952) */
};
int uart_baudrate(uint8_t uart, enum uart_baudrate bdrt)
{
uint16_t div;
if (bdrt > ARRAY_SIZE(divider))
return -1;
div = divider[bdrt];
uart_set_lcr7bit(uart, 1);
writeb(div & 0xff, UART_REG(uart, DLL));
writeb(div >> 8, UART_REG(uart, DLH));
uart_set_lcr7bit(uart, 0);
return 0;
}

View File

@ -0,0 +1,32 @@
#ifndef _UART_H
#define _UART_H
#include <stdint.h>
enum uart_baudrate {
UART_38400,
UART_57600,
UART_115200,
UART_230400,
UART_460800,
UART_614400,
UART_921600,
};
void uart_init(uint8_t uart, uint8_t interrupts);
void uart_putchar_wait(uint8_t uart, int c);
int uart_putchar_nb(uint8_t uart, int c);
int uart_getchar_nb(uint8_t uart, uint8_t *ch);
int uart_tx_busy(uint8_t uart);
int uart_baudrate(uint8_t uart, enum uart_baudrate bdrt);
enum uart_irq {
UART_IRQ_TX_EMPTY,
UART_IRQ_RX_CHAR,
};
void uart_irq_enable(uint8_t uart, enum uart_irq irq, int on);
void uart_poll(uint8_t uart);
#endif /* _UART_H */

View File

@ -0,0 +1,161 @@
#ifndef _MSGB_H
#define _MSGB_H
/* (C) 2008-2010 by Harald Welte <laforge@gnumonks.org>
* All Rights Reserved
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*
*/
#include <osmocom/core/linuxlist.h>
#include <console.h>
struct msgb {
struct llist_head list;
/* the layer 1 header, if any */
unsigned char *l1h;
/* the A-bis layer 2 header: OML, RSL(RLL), NS */
unsigned char *l2h;
/* the layer 3 header. For OML: FOM; RSL: 04.08; GPRS: BSSGP */
unsigned char *l3h;
uint16_t data_len;
uint16_t len;
unsigned char *head; /* start of buffer */
unsigned char *tail; /* end of message */
unsigned char *data; /* start of message */
unsigned char _data[0];
};
extern struct msgb *msgb_alloc(uint16_t size, const char *name);
extern void msgb_free(struct msgb *m);
extern void msgb_enqueue(struct llist_head *queue, struct msgb *msg);
extern struct msgb *msgb_dequeue(struct llist_head *queue);
extern void msgb_reset(struct msgb *m);
#define msgb_l1(m) ((void *)(m->l1h))
#define msgb_l2(m) ((void *)(m->l2h))
#define msgb_l3(m) ((void *)(m->l3h))
static inline unsigned int msgb_l1len(const struct msgb *msgb)
{
return msgb->tail - (uint8_t *)msgb_l1(msgb);
}
static inline unsigned int msgb_l2len(const struct msgb *msgb)
{
return msgb->tail - (uint8_t *)msgb_l2(msgb);
}
static inline unsigned int msgb_l3len(const struct msgb *msgb)
{
return msgb->tail - (uint8_t *)msgb_l3(msgb);
}
static inline unsigned int msgb_headlen(const struct msgb *msgb)
{
return msgb->len - msgb->data_len;
}
static inline int msgb_tailroom(const struct msgb *msgb)
{
return (msgb->head + msgb->data_len) - msgb->tail;
}
static inline unsigned char *msgb_put(struct msgb *msgb, unsigned int len)
{
unsigned char *tmp = msgb->tail;
/* we intentionally call cons_puts() here to display an allocation
* failure on the _other_ serial port (i.e. the one that doesn't
* have the HDLC layer on it */
if (msgb_tailroom(msgb) < len)
cons_puts("msgb_tailroom insufficient!\n");
msgb->tail += len;
msgb->len += len;
return tmp;
}
static inline void msgb_put_u8(struct msgb *msgb, uint8_t word)
{
uint8_t *space = msgb_put(msgb, 1);
space[0] = word & 0xFF;
}
static inline void msgb_put_u16(struct msgb *msgb, uint16_t word)
{
uint8_t *space = msgb_put(msgb, 2);
space[0] = word >> 8 & 0xFF;
space[1] = word & 0xFF;
}
static inline void msgb_put_u32(struct msgb *msgb, uint32_t word)
{
uint8_t *space = msgb_put(msgb, 4);
space[0] = word >> 24 & 0xFF;
space[1] = word >> 16 & 0xFF;
space[2] = word >> 8 & 0xFF;
space[3] = word & 0xFF;
}
static inline unsigned char *msgb_get(struct msgb *msgb, unsigned int len)
{
unsigned char *tmp = msgb->data;
msgb->data += len;
msgb->len -= len;
return tmp;
}
static inline uint8_t msgb_get_u8(struct msgb *msgb)
{
uint8_t *space = msgb_get(msgb, 1);
return space[0];
}
static inline uint16_t msgb_get_u16(struct msgb *msgb)
{
uint8_t *space = msgb_get(msgb, 2);
return space[0] << 8 | space[1];
}
static inline uint32_t msgb_get_u32(struct msgb *msgb)
{
uint8_t *space = msgb_get(msgb, 4);
return space[0] << 24 | space[1] << 16 | space[2] << 8 | space[3];
}
static inline unsigned char *msgb_push(struct msgb *msgb, unsigned int len)
{
msgb->data -= len;
msgb->len += len;
return msgb->data;
}
static inline unsigned char *msgb_pull(struct msgb *msgb, unsigned int len)
{
msgb->len -= len;
return msgb->data += len;
}
/* increase the headroom of an empty msgb, reducing the tailroom */
static inline void msgb_reserve(struct msgb *msg, int len)
{
msg->data += len;
msg->tail += len;
}
static inline struct msgb *msgb_alloc_headroom(int size, int headroom,
const char *name)
{
struct msgb *msg = msgb_alloc(size, name);
if (msg)
msgb_reserve(msg, headroom);
return msg;
}
#endif /* _MSGB_H */

View File

@ -0,0 +1,57 @@
#ifndef _SERCOMM_H
#define _SERCOMM_H
/* SERCOMM layer on UART1 (modem UART) */
#include <osmocom/core/msgb.h>
#define SERCOMM_UART_NR 1
#define HDLC_FLAG 0x7E
#define HDLC_ESCAPE 0x7D
#define HDLC_C_UI 0x03
#define HDLC_C_P_BIT (1 << 4)
#define HDLC_C_F_BIT (1 << 4)
/* a low sercomm_dlci means high priority. A high DLCI means low priority */
enum sercomm_dlci {
SC_DLCI_HIGHEST = 0,
SC_DLCI_DEBUG = 4,
SC_DLCI_L1A_L23 = 5,
SC_DLCI_LOADER = 9,
SC_DLCI_CONSOLE = 10,
SC_DLCI_ECHO = 128,
_SC_DLCI_MAX
};
void sercomm_init(void);
int sercomm_initialized(void);
/* User Interface: Tx */
/* user interface for transmitting messages for a given DLCI */
void sercomm_sendmsg(uint8_t dlci, struct msgb *msg);
/* how deep is the Tx queue for a given DLCI */
unsigned int sercomm_tx_queue_depth(uint8_t dlci);
/* User Interface: Rx */
/* receiving messages for a given DLCI */
typedef void (*dlci_cb_t)(uint8_t dlci, struct msgb *msg);
int sercomm_register_rx_cb(uint8_t dlci, dlci_cb_t cb);
/* Driver Interface */
/* fetch one octet of to-be-transmitted serial data. returns 0 if no more data */
int sercomm_drv_pull(uint8_t *ch);
/* the driver has received one byte, pass it into sercomm layer.
returns 1 in case of success, 0 in case of unrecognized char */
int sercomm_drv_rx_char(uint8_t ch);
static inline struct msgb *sercomm_alloc_msgb(unsigned int len)
{
return msgb_alloc_headroom(len+4, 4, "sercomm_tx");
}
#endif /* _SERCOMM_H */

View File

@ -0,0 +1,10 @@
#ifndef _SERCOMM_CONS_H
#define _SERCOMM_CONS_H
/* how large buffers do we allocate? */
#define SERCOMM_CONS_ALLOC 256
int sercomm_puts(const char *s);
int sercomm_putchar(int c);
#endif /* _SERCOMM_CONS_H */