aboutsummaryrefslogtreecommitdiff
path: root/src/platforms/tm4c
diff options
context:
space:
mode:
authorNicolas Schodet2015-05-11 11:44:13 +0200
committerNicolas Schodet2015-06-16 17:00:31 +0200
commitb877293fd8603270854876f8c7e6a6d46fb49c2c (patch)
tree580838207cb8a0b5d980a6b1d2cc5b3d5420fb90 /src/platforms/tm4c
parent21b3aba640e492b49140cb9c994f97a61ca51d29 (diff)
parent482070c91b0cc5a5f16c02a30e26e306685566bb (diff)
Merge remote-tracking branch 'github/master' into dev2
Conflicts: src/main.c
Diffstat (limited to 'src/platforms/tm4c')
-rw-r--r--src/platforms/tm4c/gdb_if.c102
-rw-r--r--src/platforms/tm4c/jtagtap.c59
-rw-r--r--src/platforms/tm4c/swdptap.c124
-rw-r--r--src/platforms/tm4c/tm4c.ld29
-rw-r--r--src/platforms/tm4c/traceswo.c161
-rw-r--r--src/platforms/tm4c/usbuart.c182
6 files changed, 657 insertions, 0 deletions
diff --git a/src/platforms/tm4c/gdb_if.c b/src/platforms/tm4c/gdb_if.c
new file mode 100644
index 0000000..7119638
--- /dev/null
+++ b/src/platforms/tm4c/gdb_if.c
@@ -0,0 +1,102 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2011 Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * 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 3 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, see <http://www.gnu.org/licenses/>.
+ */
+
+/* This file implements a transparent channel over which the GDB Remote
+ * Serial Debugging protocol is implemented. This implementation for STM32
+ * uses the USB CDC-ACM device bulk endpoints to implement the channel.
+ */
+
+#include "general.h"
+#include "gdb_if.h"
+#include "cdcacm.h"
+
+#include <libopencm3/usb/usbd.h>
+
+static volatile uint32_t head_out, tail_out;
+static volatile uint32_t count_in;
+static volatile uint8_t buffer_out[16*CDCACM_PACKET_SIZE];
+static volatile uint8_t buffer_in[CDCACM_PACKET_SIZE];
+
+void gdb_if_putchar(unsigned char c, int flush)
+{
+ buffer_in[count_in++] = c;
+ if(flush || (count_in == CDCACM_PACKET_SIZE)) {
+ /* Refuse to send if USB isn't configured, and
+ * don't bother if nobody's listening */
+ if((cdcacm_get_config() != 1) || !cdcacm_get_dtr()) {
+ count_in = 0;
+ return;
+ }
+ while(usbd_ep_write_packet(usbdev, CDCACM_GDB_ENDPOINT,
+ (uint8_t *)buffer_in, count_in) <= 0);
+ count_in = 0;
+ }
+}
+
+void gdb_usb_out_cb(usbd_device *dev, uint8_t ep)
+{
+ (void)ep;
+ static uint8_t buf[CDCACM_PACKET_SIZE];
+
+ usbd_ep_nak_set(dev, CDCACM_GDB_ENDPOINT, 1);
+ uint32_t count = usbd_ep_read_packet(dev, CDCACM_GDB_ENDPOINT,
+ (uint8_t *)buf, CDCACM_PACKET_SIZE);
+
+
+ uint32_t idx;
+ for (idx=0; idx<count; idx++) {
+ buffer_out[head_out++ % sizeof(buffer_out)] = buf[idx];
+ }
+
+ usbd_ep_nak_set(dev, CDCACM_GDB_ENDPOINT, 0);
+}
+
+unsigned char gdb_if_getchar(void)
+{
+
+ while(tail_out == head_out) {
+ /* Detach if port closed */
+ if(!cdcacm_get_dtr())
+ return 0x04;
+
+ while(cdcacm_get_config() != 1);
+ }
+
+ return buffer_out[tail_out++ % sizeof(buffer_out)];
+}
+
+unsigned char gdb_if_getchar_to(int timeout)
+{
+ timeout_counter = timeout/100;
+
+ if(head_out == tail_out) do {
+ /* Detach if port closed */
+ if(!cdcacm_get_dtr())
+ return 0x04;
+
+ while(cdcacm_get_config() != 1);
+ } while(timeout_counter && head_out == tail_out);
+
+ if(head_out != tail_out)
+ return gdb_if_getchar();
+
+ return -1;
+}
+
diff --git a/src/platforms/tm4c/jtagtap.c b/src/platforms/tm4c/jtagtap.c
new file mode 100644
index 0000000..e0bdd68
--- /dev/null
+++ b/src/platforms/tm4c/jtagtap.c
@@ -0,0 +1,59 @@
+#include "general.h"
+#include "jtagtap.h"
+
+int
+jtagtap_init(void)
+{
+ TMS_SET_MODE();
+
+ for(int i = 0; i <= 50; i++) jtagtap_next(1,0);
+ jtagtap_tms_seq(0xE73C, 16);
+ jtagtap_soft_reset();
+
+ return 0;
+}
+
+void
+jtagtap_reset(void)
+{
+#ifdef TRST_PORT
+ volatile int i;
+ gpio_clear(TRST_PORT, TRST_PIN);
+ for(i = 0; i < 10000; i++) asm("nop");
+ gpio_set(TRST_PORT, TRST_PIN);
+#endif
+ jtagtap_soft_reset();
+}
+
+void
+jtagtap_srst(bool assert)
+{
+ volatile int i;
+ if (assert) {
+ gpio_clear(SRST_PORT, SRST_PIN);
+ for(i = 0; i < 10000; i++) asm("nop");
+ } else {
+ gpio_set(SRST_PORT, SRST_PIN);
+ }
+}
+
+uint8_t
+jtagtap_next(const uint8_t dTMS, const uint8_t dTDO)
+{
+ uint16_t ret;
+
+ gpio_set_val(TMS_PORT, TMS_PIN, dTMS);
+ gpio_set_val(TDI_PORT, TDI_PIN, dTDO);
+ gpio_set(TCK_PORT, TCK_PIN);
+ ret = gpio_get(TDO_PORT, TDO_PIN);
+ gpio_clear(TCK_PORT, TCK_PIN);
+
+ DEBUG("jtagtap_next(TMS = %d, TDO = %d) = %d\n", dTMS, dTDO, ret);
+
+ return ret != 0;
+}
+
+#define PROVIDE_GENERIC_JTAGTAP_TMS_SEQ
+#define PROVIDE_GENERIC_JTAGTAP_TDI_TDO_SEQ
+#define PROVIDE_GENERIC_JTAGTAP_TDI_SEQ
+#include "jtagtap_generic.c"
diff --git a/src/platforms/tm4c/swdptap.c b/src/platforms/tm4c/swdptap.c
new file mode 100644
index 0000000..058f6ba
--- /dev/null
+++ b/src/platforms/tm4c/swdptap.c
@@ -0,0 +1,124 @@
+#include "general.h"
+#include "swdptap.h"
+
+static void swdptap_turnaround(uint8_t dir)
+{
+ static uint8_t olddir = 0;
+
+ DEBUG("%s", dir ? "\n-> ":"\n<- ");
+
+ /* Don't turnaround if direction not changing */
+ if(dir == olddir) return;
+ olddir = dir;
+
+ if(dir)
+ SWDIO_MODE_FLOAT();
+ gpio_set(SWCLK_PORT, SWCLK_PIN);
+ gpio_clear(SWCLK_PORT, SWCLK_PIN);
+ if(!dir)
+ SWDIO_MODE_DRIVE();
+}
+
+static uint8_t swdptap_bit_in(void)
+{
+ uint16_t ret;
+
+ ret = gpio_get(SWDIO_PORT, SWDIO_PIN);
+ gpio_set(SWCLK_PORT, SWCLK_PIN);
+ gpio_clear(SWCLK_PORT, SWCLK_PIN);
+
+ DEBUG("%d", ret?1:0);
+
+ return ret != 0;
+}
+
+static void swdptap_bit_out(uint8_t val)
+{
+ DEBUG("%d", val);
+
+ gpio_set_val(SWDIO_PORT, SWDIO_PIN, val);
+ gpio_set(SWCLK_PORT, SWCLK_PIN);
+ gpio_clear(SWCLK_PORT, SWCLK_PIN);
+}
+
+int
+swdptap_init(void)
+{
+ swdptap_reset();
+ swdptap_seq_out(0xE79E, 16); /* 0b0111100111100111 */
+ swdptap_reset();
+ swdptap_seq_out(0, 16);
+
+ return 0;
+}
+
+void
+swdptap_reset(void)
+{
+ swdptap_turnaround(0);
+ /* 50 clocks with TMS high */
+ for(int i = 0; i < 50; i++) swdptap_bit_out(1);
+}
+
+uint32_t
+swdptap_seq_in(int ticks)
+{
+ uint32_t index = 1;
+ uint32_t ret = 0;
+
+ swdptap_turnaround(1);
+
+ while(ticks--) {
+ if(swdptap_bit_in()) ret |= index;
+ index <<= 1;
+ }
+
+ return ret;
+}
+
+uint8_t
+swdptap_seq_in_parity(uint32_t *ret, int ticks)
+{
+ uint32_t index = 1;
+ uint8_t parity = 0;
+ *ret = 0;
+
+ swdptap_turnaround(1);
+
+ while(ticks--) {
+ if(swdptap_bit_in()) {
+ *ret |= index;
+ parity ^= 1;
+ }
+ index <<= 1;
+ }
+ if(swdptap_bit_in()) parity ^= 1;
+
+ return parity;
+}
+
+void
+swdptap_seq_out(uint32_t MS, int ticks)
+{
+ swdptap_turnaround(0);
+
+ while(ticks--) {
+ swdptap_bit_out(MS & 1);
+ MS >>= 1;
+ }
+}
+
+void
+swdptap_seq_out_parity(uint32_t MS, int ticks)
+{
+ uint8_t parity = 0;
+
+ swdptap_turnaround(0);
+
+ while(ticks--) {
+ swdptap_bit_out(MS & 1);
+ parity ^= MS;
+ MS >>= 1;
+ }
+ swdptap_bit_out(parity & 1);
+}
diff --git a/src/platforms/tm4c/tm4c.ld b/src/platforms/tm4c/tm4c.ld
new file mode 100644
index 0000000..8fe93a4
--- /dev/null
+++ b/src/platforms/tm4c/tm4c.ld
@@ -0,0 +1,29 @@
+/*
+ * This file is part of the libopenstm32 project.
+ *
+ * Copyright (C) 2010 Thomas Otto <tommi@viadmin.org>
+ *
+ * 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 3 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, see <http://www.gnu.org/licenses/>.
+ */
+
+/* Define memory regions. */
+MEMORY
+{
+ rom (rx) : ORIGIN = 0x00000000, LENGTH = 256K
+ ram (rwx) : ORIGIN = 0x20000000, LENGTH = 32K
+}
+
+/* Include the common ld script from libopenstm32. */
+INCLUDE libopencm3_lm4f.ld
+
diff --git a/src/platforms/tm4c/traceswo.c b/src/platforms/tm4c/traceswo.c
new file mode 100644
index 0000000..3e43a3b
--- /dev/null
+++ b/src/platforms/tm4c/traceswo.c
@@ -0,0 +1,161 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2012 Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * Copyright (C) 2014 Fredrik Ahlberg <fredrik@z80.se>
+ *
+ * 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 3 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, see <http://www.gnu.org/licenses/>.
+ */
+
+/* This file implements capture of the TRACESWO output.
+ *
+ * ARM DDI 0403D - ARMv7M Architecture Reference Manual
+ * ARM DDI 0337I - Cortex-M3 Technical Reference Manual
+ * ARM DDI 0314H - CoreSight Components Technical Reference Manual
+ */
+
+#include "general.h"
+#include "cdcacm.h"
+
+#include <libopencm3/cm3/nvic.h>
+#include <libopencm3/lm4f/rcc.h>
+#include <libopencm3/lm4f/nvic.h>
+#include <libopencm3/lm4f/uart.h>
+#include <libopencm3/usb/usbd.h>
+
+void traceswo_init(void)
+{
+ periph_clock_enable(RCC_GPIOD);
+ periph_clock_enable(TRACEUART_CLK);
+ __asm__("nop"); __asm__("nop"); __asm__("nop");
+
+ gpio_mode_setup(SWO_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, SWO_PIN);
+ gpio_set_af(SWO_PORT, 1, SWO_PIN); /* U2RX */
+
+ uart_disable(TRACEUART);
+
+ /* Setup UART parameters. */
+ uart_clock_from_sysclk(TRACEUART);
+ uart_set_baudrate(TRACEUART, 800000);
+ uart_set_databits(TRACEUART, 8);
+ uart_set_stopbits(TRACEUART, 1);
+ uart_set_parity(TRACEUART, UART_PARITY_NONE);
+
+ // Enable FIFO
+ uart_enable_fifo(TRACEUART);
+
+ // Set FIFO interrupt trigger levels to 4/8 full for RX buffer and
+ // 7/8 empty (1/8 full) for TX buffer
+ uart_set_fifo_trigger_levels(TRACEUART, UART_FIFO_RX_TRIG_1_2, UART_FIFO_TX_TRIG_7_8);
+
+ uart_clear_interrupt_flag(TRACEUART, UART_INT_RX | UART_INT_RT);
+
+ /* Enable interrupts */
+ uart_enable_interrupts(TRACEUART, UART_INT_RX | UART_INT_RT);
+
+ /* Finally enable the USART. */
+ uart_enable(TRACEUART);
+
+ nvic_set_priority(TRACEUART_IRQ, 0);
+ nvic_enable_irq(TRACEUART_IRQ);
+
+ /* Un-stall USB endpoint */
+ usbd_ep_stall_set(usbdev, 0x85, 0);
+
+ gpio_mode_setup(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO3);
+}
+
+void traceswo_baud(unsigned int baud)
+{
+ uart_set_baudrate(TRACEUART, baud);
+ uart_set_databits(TRACEUART, 8);
+}
+
+#define FIFO_SIZE 256
+
+/* RX Fifo buffer */
+static volatile uint8_t buf_rx[FIFO_SIZE];
+/* Fifo in pointer, writes assumed to be atomic, should be only incremented within RX ISR */
+static volatile uint32_t buf_rx_in = 0;
+/* Fifo out pointer, writes assumed to be atomic, should be only incremented outside RX ISR */
+static volatile uint32_t buf_rx_out = 0;
+
+void trace_buf_push(void)
+{
+ size_t len;
+
+ if (buf_rx_in == buf_rx_out) {
+ return;
+ } else if (buf_rx_in > buf_rx_out) {
+ len = buf_rx_in - buf_rx_out;
+ } else {
+ len = FIFO_SIZE - buf_rx_out;
+ }
+
+ if (len > 64) {
+ len = 64;
+ }
+
+ if (usbd_ep_write_packet(usbdev, 0x85, (uint8_t *)&buf_rx[buf_rx_out], len) == len) {
+ buf_rx_out += len;
+ buf_rx_out %= FIFO_SIZE;
+ }
+}
+
+void trace_buf_drain(usbd_device *dev, uint8_t ep)
+{
+ (void) dev;
+ (void) ep;
+ trace_buf_push();
+}
+
+void trace_tick(void)
+{
+ trace_buf_push();
+}
+
+void TRACEUART_ISR(void)
+{
+ uint32_t flush = uart_is_interrupt_source(TRACEUART, UART_INT_RT);
+
+ while (!uart_is_rx_fifo_empty(TRACEUART)) {
+ uint32_t c = uart_recv(TRACEUART);
+
+ /* If the next increment of rx_in would put it at the same point
+ * as rx_out, the FIFO is considered full.
+ */
+ if (((buf_rx_in + 1) % FIFO_SIZE) != buf_rx_out)
+ {
+ /* insert into FIFO */
+ buf_rx[buf_rx_in++] = c;
+
+ /* wrap out pointer */
+ if (buf_rx_in >= FIFO_SIZE)
+ {
+ buf_rx_in = 0;
+ }
+ } else {
+ flush = 1;
+ break;
+ }
+ }
+
+ if (flush) {
+ /* advance fifo out pointer by amount written */
+ trace_buf_push();
+ }
+}
+
diff --git a/src/platforms/tm4c/usbuart.c b/src/platforms/tm4c/usbuart.c
new file mode 100644
index 0000000..da82198
--- /dev/null
+++ b/src/platforms/tm4c/usbuart.c
@@ -0,0 +1,182 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2012 Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * Copyright (C) 2014 Fredrik Ahlberg <fredrik@z80.se>
+ *
+ * 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 3 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, see <http://www.gnu.org/licenses/>.
+ */
+#include "general.h"
+#include "cdcacm.h"
+
+#include <libopencm3/cm3/nvic.h>
+#include <libopencm3/cm3/scs.h>
+#include <libopencm3/usb/usbd.h>
+#include <libopencm3/usb/cdc.h>
+#include <libopencm3/lm4f/rcc.h>
+#include <libopencm3/lm4f/uart.h>
+
+#define FIFO_SIZE 128
+
+/* RX Fifo buffer */
+static uint8_t buf_rx[FIFO_SIZE];
+/* Fifo in pointer, writes assumed to be atomic, should be only incremented within RX ISR */
+static uint8_t buf_rx_in;
+/* Fifo out pointer, writes assumed to be atomic, should be only incremented outside RX ISR */
+static uint8_t buf_rx_out;
+
+void usbuart_init(void)
+{
+ UART_PIN_SETUP();
+
+ periph_clock_enable(USBUART_CLK);
+ __asm__("nop"); __asm__("nop"); __asm__("nop");
+
+ uart_disable(USBUART);
+
+ /* Setup UART parameters. */
+ uart_clock_from_sysclk(USBUART);
+ uart_set_baudrate(USBUART, 38400);
+ uart_set_databits(USBUART, 8);
+ uart_set_stopbits(USBUART, 1);
+ uart_set_parity(USBUART, UART_PARITY_NONE);
+
+ // Enable FIFO
+ uart_enable_fifo(USBUART);
+
+ // Set FIFO interrupt trigger levels to 1/8 full for RX buffer and
+ // 7/8 empty (1/8 full) for TX buffer
+ uart_set_fifo_trigger_levels(USBUART, UART_FIFO_RX_TRIG_1_8, UART_FIFO_TX_TRIG_7_8);
+
+ uart_clear_interrupt_flag(USBUART, UART_INT_RX | UART_INT_RT);
+
+ /* Enable interrupts */
+ uart_enable_interrupts(UART0, UART_INT_RX| UART_INT_RT);
+
+ /* Finally enable the USART. */
+ uart_enable(USBUART);
+
+ //nvic_set_priority(USBUSART_IRQ, IRQ_PRI_USBUSART);
+ nvic_enable_irq(USBUART_IRQ);
+}
+
+void usbuart_set_line_coding(struct usb_cdc_line_coding *coding)
+{
+ uart_set_baudrate(USBUART, coding->dwDTERate);
+ uart_set_databits(USBUART, coding->bDataBits);
+ switch(coding->bCharFormat) {
+ case 0:
+ case 1:
+ uart_set_stopbits(USBUART, 1);
+ break;
+ case 2:
+ uart_set_stopbits(USBUART, 2);
+ break;
+ }
+ switch(coding->bParityType) {
+ case 0:
+ uart_set_parity(USBUART, UART_PARITY_NONE);
+ break;
+ case 1:
+ uart_set_parity(USBUART, UART_PARITY_ODD);
+ break;
+ case 2:
+ uart_set_parity(USBUART, UART_PARITY_EVEN);
+ break;
+ }
+}
+
+void usbuart_usb_out_cb(usbd_device *dev, uint8_t ep)
+{
+ (void)ep;
+
+ char buf[CDCACM_PACKET_SIZE];
+ int len = usbd_ep_read_packet(dev, CDCACM_UART_ENDPOINT,
+ buf, CDCACM_PACKET_SIZE);
+
+ for(int i = 0; i < len; i++)
+ uart_send_blocking(USBUART, buf[i]);
+}
+
+
+void usbuart_usb_in_cb(usbd_device *dev, uint8_t ep)
+{
+ (void) dev;
+ (void) ep;
+}
+
+/*
+ * Read a character from the UART RX and stuff it in a software FIFO.
+ * Allowed to read from FIFO out pointer, but not write to it.
+ * Allowed to write to FIFO in pointer.
+ */
+void USBUART_ISR(void)
+{
+ int flush = uart_is_interrupt_source(USBUART, UART_INT_RT);
+
+ while (!uart_is_rx_fifo_empty(USBUART)) {
+ char c = uart_recv(USBUART);
+
+ /* If the next increment of rx_in would put it at the same point
+ * as rx_out, the FIFO is considered full.
+ */
+ if (((buf_rx_in + 1) % FIFO_SIZE) != buf_rx_out)
+ {
+ /* insert into FIFO */
+ buf_rx[buf_rx_in++] = c;
+
+ /* wrap out pointer */
+ if (buf_rx_in >= FIFO_SIZE)
+ {
+ buf_rx_in = 0;
+ }
+ } else {
+ flush = 1;
+ }
+ }
+
+ if (flush) {
+ /* forcibly empty fifo if no USB endpoint */
+ if (cdcacm_get_config() != 1)
+ {
+ buf_rx_out = buf_rx_in;
+ return;
+ }
+
+ uint8_t packet_buf[CDCACM_PACKET_SIZE];
+ uint8_t packet_size = 0;
+ uint8_t buf_out = buf_rx_out;
+
+ /* copy from uart FIFO into local usb packet buffer */
+ while (buf_rx_in != buf_out && packet_size < CDCACM_PACKET_SIZE)
+ {
+ packet_buf[packet_size++] = buf_rx[buf_out++];
+
+ /* wrap out pointer */
+ if (buf_out >= FIFO_SIZE)
+ {
+ buf_out = 0;
+ }
+
+ }
+
+ /* advance fifo out pointer by amount written */
+ buf_rx_out += usbd_ep_write_packet(usbdev,
+ CDCACM_UART_ENDPOINT, packet_buf, packet_size);
+ buf_rx_out %= FIFO_SIZE;
+ }
+}
+