adding source files

This commit is contained in:
vuko 2016-11-28 16:47:59 +01:00
commit 738dbffd78
8 changed files with 831 additions and 0 deletions

32
src/Makefile Normal file
View file

@ -0,0 +1,32 @@
##
## This file is part of the libopencm3 project.
##
## Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>
##
## This library is free software: you can redistribute it and/or modify
## it under the terms of the GNU Lesser General Public License as published by
## the Free Software Foundation, either version 3 of the License, or
## (at your option) any later version.
##
## This library 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 Lesser General Public License for more details.
##
## You should have received a copy of the GNU Lesser General Public License
## along with this library. If not, see <http://www.gnu.org/licenses/>.
##
## Modified by Jan Wiśniewski <vuko@hackerspace.pl>
#CFLAGS += -DSTDIO_UART
CFLAGS += -DSTDIO_USB
BINARY = main
OBJS += fifo.o iobuffers.o usb.o
OOCD_BOARD = ../target/stm32f1x_stlink
OOCD_INTERFACE = stlink-v2
LDSCRIPT = ../makefiles/stm32f103x8.ld
OPENCM3_DIR = ../libopencm3
include ../makefiles/stm32f1.include

50
src/fifo.c Normal file
View file

@ -0,0 +1,50 @@
/* Copyright (c) 2016, Jan Wiśniewski <vuko@hackerspace.pl>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*/
#include"fifo.h"
void fifoInit(fifo *f, uint8_t *addr, uint16_t size){
f->first = 0;
f->last = 0;
f->size = size;
f->data = addr;
};
inline uint8_t fifoEmpty(fifo *f){
return f->first == f->last;
}
inline uint8_t fifoFull(fifo *f){
return f->first == (f->last+1)%f->size;
}
void fifoPush(fifo *f, uint8_t d){
if(!fifoFull(f)){
f->data[f->last] = d;
f->last = (f->last + 1)%f->size;
}
}
uint8_t fifoPop(fifo *f){
uint16_t first = f->first;
if(!fifoEmpty(f)){
f->first = (f->first + 1)%f->size;
return f->data[first];
} else {
return 0x00;
}
}

37
src/fifo.h Normal file
View file

@ -0,0 +1,37 @@
/* Copyright (c) 2016, Jan Wiśniewski <vuko@hackerspace.pl>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*/
#ifndef MFIFO_H
#define MFIFO_H
#include<stdint.h>
#include<stdio.h>
typedef struct fifo {
uint16_t first;
uint16_t last;
uint8_t *data;
uint16_t size;
} fifo;
void fifoInit(fifo*,uint8_t*,uint16_t);
inline uint8_t fifoEmpty(fifo*);
inline uint8_t fifoFull(fifo*);
void fifoPush(fifo*,uint8_t);
uint8_t fifoPop(fifo*);
#endif

109
src/iobuffers.c Normal file
View file

@ -0,0 +1,109 @@
/* Copyright (c) 2016, vuko@hackerspace.pl
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*/
#include"iobuffers.h"
#include <libopencm3/cm3/nvic.h>
#include<libopencmsis/core_cm3.h>
#include <libopencm3/stm32/usart.h>
#include <libopencm3/usb/usbd.h>
#include "usb.h"
int _write(int file, char *ptr, int len){
(void)file;
int no = 0;
__disable_irq();
while(no<len && !fifoFull(&tx_fifo)){
fifoPush(&tx_fifo, ptr[no]);
++no;
}
#ifdef STDIO_UART
USART_CR1(USART1) |= USART_CR1_TXEIE;
__enable_irq();
#endif
#ifdef STDIO_USB
if(usb_tx_in_progress == 0){
usb_tx_in_progress = 1;
__enable_irq();
cdcacm_data_tx_cb(usbd_dev,0x82);
}
else
__enable_irq();
#endif
return no;
}
int _write_block(int file, char *ptr, int len){
(void)file;
int no = 0;
__disable_irq();
while(no<len){
while(fifoFull(&tx_fifo)){
__WFI();
__enable_irq();
__asm("nop"); // TODO remove
__disable_irq();
}
fifoPush(&tx_fifo, ptr[no]);
++no;
}
#ifdef STDIO_UART
USART_CR1(USART1) |= USART_CR1_TXEIE;
__enable_irq();
return no;
#endif
#ifdef STDIO_USB
if(usb_tx_in_progress == 0){
usb_tx_in_progress = 1;
__enable_irq();
cdcacm_data_tx_cb(usbd_dev,0x82);
}
else
__enable_irq();
return no;
#endif
}
int _read(int file, char *ptr, int len){
(void)file;
int no = 0;
__disable_irq();
while(no<len && !fifoEmpty(&rx_fifo)){
ptr[no] = fifoPop(&rx_fifo);
++no;
}
__enable_irq();
return no;
}
int _read_block(int file, char *ptr, int len){
(void)file;
int no = 0;
__disable_irq();
while(no<len){
while(fifoEmpty(&rx_fifo)){
__WFI();
__enable_irq();
__asm("nop");
__disable_irq();
}
ptr[no] = fifoPop(&rx_fifo);
++no;
}
__enable_irq();
return no;
}

29
src/iobuffers.h Normal file
View file

@ -0,0 +1,29 @@
/* Copyright (c) 2016, vuko@hackerspace.pl
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*/
#ifndef IOBUFFERS_H
#define IOBUFFERS_H
#include"fifo.h"
extern fifo tx_fifo;
extern fifo rx_fifo;
int _write(int, char*, int);
int _read(int, char*, int);
int _write_block(int, char*, int);
int _read_block(int, char*, int);
#endif

272
src/main.c Normal file
View file

@ -0,0 +1,272 @@
/* Copyright (c) 2016, Jan Wiśniewski <vuko@hackerspace.pl>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
*/
#include <libopencm3/cm3/nvic.h>
#include <libopencm3/stm32/dbgmcu.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/timer.h>
#include <libopencm3/stm32/usart.h>
#include <libopencm3/usb/cdc.h>
#include <libopencm3/usb/usbd.h>
#include <libopencmsis/core_cm3.h>
#include <stdint.h>
#include <stdio.h>
#include "fifo.h"
#include "iobuffers.h"
#include "usb.h"
/* connected to white cables from flow meters
* 1300 impulses (LO/HI cycle) per liter (TODO make sure)
* external pull-ups are needed
*/
#define INPUT1_PIN GPIO8
#define INPUT2_PIN GPIO9
#define INPUTS_PORT GPIOB
/* buzzer output pin
* HIGH - buzzer on
* LOW - buzzer off
*/
#define BUZZER_PIN GPIO12
#define BUZZER_PORT GPIOB
uint8_t tx_buffer[1024];
uint8_t rx_buffer[1024];
fifo tx_fifo;
fifo rx_fifo;
int main(void);
#ifdef STDIO_USB
usbd_device* usbd_dev;
void usb_hp_can_tx_isr(void){
usbd_poll(usbd_dev);
}
void usb_lp_can_rx0_isr(void){
usbd_poll(usbd_dev);
}
#endif
#ifdef STDIO_UART
void usart1_isr(void) {
uint8_t data;
/* RXNE interrupt handling */
if (((USART_CR1(USART1) & USART_CR1_RXNEIE) != 0) &&
((USART_SR(USART1) & USART_SR_RXNE) != 0)) {
data = usart_recv(USART1);
if(!fifoFull(&rx_fifo))
fifoPush(&rx_fifo, data);
}
/* TXE interrupt handling */
if (((USART_CR1(USART1) & USART_CR1_TXEIE) != 0) &&
((USART_SR(USART1) & USART_SR_TXE) != 0)) {
if(!fifoEmpty(&tx_fifo)){
data = fifoPop(&tx_fifo);
usart_send(USART1, data);
} else {
/* disable TXE interrupt if buffer empty */
USART_CR1(USART1) &= ~USART_CR1_TXEIE;
}
}
}
#endif
/* main routine */
void tim2_isr(void)
{
enum flow_state_t {
INIT, // initial state emit four 1 second beeps
NORMAL, // flow is ok
WARNING, // low flow for less than 4s, emit 0.5 second beeps
ERROR // low flow for more than 4s, emit continuous beep
};
static enum flow_state_t flow_state = INIT;
static uint32_t cycles = 0; // cycles from last second
static uint16_t seconds = 0; // seconds from last state change
static uint8_t previous[2]; // input values from last cycle
static uint16_t impulses[2]; // number of impulses from last second
uint8_t alarm; // should buzzer be on?
const uint32_t minimal_value = 5; // minimal number of impulses per second
const uint16_t cycles_per_second = 1000;
uint8_t good = (impulses[0] >= minimal_value) &&
(impulses[1] > minimal_value);
cycles = cycles + 1;
if (cycles >= cycles_per_second){
cycles = 0;
seconds = seconds + 1;
}
if(cycles == 0){
printf("impulses %u %u\r\n",impulses[0],impulses[1]);
switch(flow_state){
case INIT:
if(seconds >= 4){
flow_state = NORMAL;
seconds = 0;
}
break;
case NORMAL:
if(!good){
flow_state = WARNING;
seconds = 0;
}
break;
case WARNING:
if(good){
flow_state = NORMAL;
seconds = 0;
}
else if( seconds >= 4){
flow_state = ERROR;
seconds = 0;
}
break;
case ERROR:
if(good){
flow_state = NORMAL;
seconds = 0;
}
break;
}
impulses[0] = 0;
impulses[1] = 0;
}
alarm = 0;
switch(flow_state){
case INIT:
if(seconds % 2 == 0) alarm = 1;
break;
case NORMAL:
alarm = 0;
break;
case WARNING:
if(cycles > (cycles_per_second/2)) alarm = 1;
break;
case ERROR:
alarm = 1;
break;
}
uint16_t port_in;
uint8_t current[2];
port_in = gpio_port_read(INPUTS_PORT);
current[0] = (port_in & INPUT1_PIN) != 0;
current[1] = (port_in & INPUT2_PIN) != 0;
/* detect rising edge */
uint8_t input_no;
for (input_no=0; input_no<2; input_no+=1){
if (current[input_no] && !previous[input_no]){
impulses[input_no] += 1;
}
previous[input_no] = current[input_no];
}
if(alarm){
gpio_set(BUZZER_PORT, BUZZER_PIN);
}
else {
gpio_clear(BUZZER_PORT, BUZZER_PIN);
}
TIM_SR(TIM2) &= ~TIM_SR_UIF; /* Clear interrrupt flag. */
}
int main(){
// enable dubbuger during sleep modes
//DBGMCU_CR |= DBGMCU_CR_SLEEP | DBGMCU_CR_STOP | DBGMCU_CR_STANDBY;
/* clocks init */
rcc_clock_setup_in_hse_8mhz_out_72mhz();
//rcc_periph_clock_enable(RCC_GPIOA);
rcc_periph_clock_enable(RCC_GPIOB);
rcc_periph_clock_enable(RCC_AFIO);
rcc_periph_clock_enable(RCC_TIM2);
/* init data structures */
fifoInit(&tx_fifo, tx_buffer, 1024);
fifoInit(&rx_fifo, rx_buffer, 1024);
#ifdef STDIO_UART
/* usart configuration */
rcc_periph_clock_enable(RCC_USART1);
gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART1_TX);
gpio_set_mode(GPIOA, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART1_RX);
usart_set_baudrate(USART1, 115200);
usart_set_databits(USART1, 8);
usart_set_stopbits(USART1, USART_STOPBITS_1);
usart_set_parity(USART1, USART_PARITY_NONE);
usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE);
usart_set_mode(USART1, USART_MODE_TX_RX);
USART_CR1(USART1) |= USART_CR1_RXNEIE; // enable interrupt
usart_enable(USART1);
#endif
#ifdef STDIO_USB
/* usb configuration */
rcc_set_usbpre(RCC_CFGR_USBPRE_PLL_CLK_DIV1_5);
usbd_dev = usb_init();
usbd_poll(usbd_dev);
nvic_enable_irq(NVIC_USB_HP_CAN_TX_IRQ);
nvic_enable_irq(NVIC_USB_LP_CAN_RX0_IRQ);
nvic_set_priority(NVIC_USB_HP_CAN_TX_IRQ, 1);
nvic_set_priority(NVIC_USB_LP_CAN_RX0_IRQ, 1);
#endif
/* main timer configuration */
nvic_enable_irq(NVIC_TIM2_IRQ);
nvic_set_priority(NVIC_TIM2_IRQ, 1);
nvic_enable_irq(NVIC_USART1_IRQ);
TIM_CNT(TIM2) = 0; // timer start value
TIM_PSC(TIM2) = 71; // timer prescaller
TIM_ARR(TIM2) = 1000; // timer TOP
TIM_DIER(TIM2) |= TIM_DIER_UIE; // enable interrupt
TIM_CR1(TIM2) |= TIM_CR1_CEN; // start timer
gpio_set_mode(INPUTS_PORT, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN,
INPUT1_PIN | INPUT2_PIN);
gpio_set(INPUTS_PORT, INPUT1_PIN | INPUT2_PIN);
gpio_set_mode(BUZZER_PORT, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_PUSHPULL, BUZZER_PIN);
gpio_set(BUZZER_PORT, BUZZER_PIN);
SCB_SCR |= SCB_SCR_SLEEPONEXIT | SCB_SCR_SEVEONPEND;
while(1){
__WFI();
}
return 0;
}

295
src/usb.c Normal file
View file

@ -0,0 +1,295 @@
/*
* This file is part of the libopencm3 project.
*
* Copyright (C) 2010 Gareth McMullin <gareth@blacksphere.co.nz>
*
* This library is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This library 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this library. If not, see <http://www.gnu.org/licenses/>.
*
* Modified by Jan Wiśniewski <vuko@hackerspace.pl>
*/
#include <stdlib.h>
#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/spi.h>
#include <libopencm3/usb/usbd.h>
#include <libopencm3/usb/cdc.h>
#include<libopencmsis/core_cm3.h>
#include "usb.h"
#include "fifo.h"
#include "iobuffers.h"
uint8_t usb_tx_in_progress = 0;
uint8_t usb_enabled = 0;
static const struct usb_device_descriptor dev = {
.bLength = USB_DT_DEVICE_SIZE,
.bDescriptorType = USB_DT_DEVICE,
.bcdUSB = 0x0200,
.bDeviceClass = USB_CLASS_CDC,
.bDeviceSubClass = 0,
.bDeviceProtocol = 0,
.bMaxPacketSize0 = 64,
.idVendor = 0x0f11,
.idProduct = 0x1100,
.bcdDevice = 0x0200,
.iManufacturer = 1,
.iProduct = 2,
.iSerialNumber = 3,
.bNumConfigurations = 1,
};
/*
* This notification endpoint isn't implemented. According to CDC spec its
* optional, but its absence causes a NULL pointer dereference in Linux
* cdc_acm driver.
*/
static const struct usb_endpoint_descriptor comm_endp[] = {{
.bLength = USB_DT_ENDPOINT_SIZE,
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = 0x83,
.bmAttributes = USB_ENDPOINT_ATTR_INTERRUPT,
.wMaxPacketSize = 16,
.bInterval = 255,
}};
static const struct usb_endpoint_descriptor data_endp[] = {{
.bLength = USB_DT_ENDPOINT_SIZE,
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = 0x01,
.bmAttributes = USB_ENDPOINT_ATTR_BULK,
.wMaxPacketSize = 64,
.bInterval = 1,
}, {
.bLength = USB_DT_ENDPOINT_SIZE,
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = 0x82,
.bmAttributes = USB_ENDPOINT_ATTR_BULK,
.wMaxPacketSize = 64,
.bInterval = 1,
}};
static const struct {
struct usb_cdc_header_descriptor header;
struct usb_cdc_call_management_descriptor call_mgmt;
struct usb_cdc_acm_descriptor acm;
struct usb_cdc_union_descriptor cdc_union;
} __attribute__((packed)) cdcacm_functional_descriptors = {
.header = {
.bFunctionLength = sizeof(struct usb_cdc_header_descriptor),
.bDescriptorType = CS_INTERFACE,
.bDescriptorSubtype = USB_CDC_TYPE_HEADER,
.bcdCDC = 0x0110,
},
.call_mgmt = {
.bFunctionLength =
sizeof(struct usb_cdc_call_management_descriptor),
.bDescriptorType = CS_INTERFACE,
.bDescriptorSubtype = USB_CDC_TYPE_CALL_MANAGEMENT,
.bmCapabilities = 0,
.bDataInterface = 1,
},
.acm = {
.bFunctionLength = sizeof(struct usb_cdc_acm_descriptor),
.bDescriptorType = CS_INTERFACE,
.bDescriptorSubtype = USB_CDC_TYPE_ACM,
.bmCapabilities = 0,
},
.cdc_union = {
.bFunctionLength = sizeof(struct usb_cdc_union_descriptor),
.bDescriptorType = CS_INTERFACE,
.bDescriptorSubtype = USB_CDC_TYPE_UNION,
.bControlInterface = 0,
.bSubordinateInterface0 = 1,
}
};
static const struct usb_interface_descriptor comm_iface[] = {{
.bLength = USB_DT_INTERFACE_SIZE,
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 0,
.bAlternateSetting = 0,
.bNumEndpoints = 1,
.bInterfaceClass = USB_CLASS_CDC,
.bInterfaceSubClass = USB_CDC_SUBCLASS_ACM,
.bInterfaceProtocol = USB_CDC_PROTOCOL_AT,
.iInterface = 0,
.endpoint = comm_endp,
.extra = &cdcacm_functional_descriptors,
.extralen = sizeof(cdcacm_functional_descriptors)
}};
static const struct usb_interface_descriptor data_iface[] = {{
.bLength = USB_DT_INTERFACE_SIZE,
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 1,
.bAlternateSetting = 0,
.bNumEndpoints = 2,
.bInterfaceClass = USB_CLASS_DATA,
.bInterfaceSubClass = 0,
.bInterfaceProtocol = 0,
.iInterface = 0,
.endpoint = data_endp,
}};
static const struct usb_interface ifaces[] = {{
.num_altsetting = 1,
.altsetting = comm_iface,
}, {
.num_altsetting = 1,
.altsetting = data_iface,
}};
static const struct usb_config_descriptor config = {
.bLength = USB_DT_CONFIGURATION_SIZE,
.bDescriptorType = USB_DT_CONFIGURATION,
.wTotalLength = 0,
.bNumInterfaces = 2,
.bConfigurationValue = 1,
.iConfiguration = 0,
.bmAttributes = 0x80,
.bMaxPower = 0x32,
.interface = ifaces,
};
static const char *usb_strings[] = {
"vuko@hackerspace.pl",
"flowMeter",
"00001",
};
/* Buffer to be used for control requests. */
uint8_t usbd_control_buffer[128];
static int cdcacm_control_request(usbd_device *usbd_dev, struct usb_setup_data *req, uint8_t **buf,
uint16_t *len, void (**complete)(usbd_device *usbd_dev, struct usb_setup_data *req))
{
(void)complete;
(void)buf;
(void)usbd_dev;
switch(req->bRequest) {
case USB_CDC_REQ_SET_CONTROL_LINE_STATE: {
/*
* This Linux cdc_acm driver requires this to be implemented
* even though it's optional in the CDC spec, and we don't
* advertise it in the ACM functional descriptor.
*/
char local_buf[10];
struct usb_cdc_notification *notif = (void *)local_buf;
/* We echo signals back to host as notification. */
notif->bmRequestType = 0xA1;
notif->bNotification = USB_CDC_NOTIFY_SERIAL_STATE;
notif->wValue = 0;
notif->wIndex = 0;
notif->wLength = 2;
local_buf[8] = req->wValue & 3;
local_buf[9] = 0;
//usbd_ep_write_packet(usbd_dev, 0x83, local_buf, 10);
return 1;
}
case USB_CDC_REQ_SET_LINE_CODING:
if(*len < sizeof(struct usb_cdc_line_coding))
return 0;
return 1;
}
return 0;
}
static void cdcacm_data_rx_cb(usbd_device *usbd_dev, uint8_t ep)
{
(void)ep;
char buf[64];
int len = usbd_ep_read_packet(usbd_dev, 0x01, buf, 64);
int i;
for(i = 0;i<len;++i){
__disable_irq();
if(!fifoFull(&rx_fifo))
fifoPush(&rx_fifo, buf[i]);
__enable_irq();
}
}
void cdcacm_data_tx_cb(usbd_device *usbd_dev, uint8_t ep)
{
(void)ep;
char buf[64];
uint8_t len = 0;
usb_tx_in_progress = 1;
uint8_t i;
for(i = 0;i<64;++i){
__disable_irq();
if(!fifoEmpty(&tx_fifo)){
buf[i] = fifoPop(&tx_fifo);
++len;
} else {
__enable_irq();
break;
}
__enable_irq();
}
if(len>0){
if(usb_enabled == 1){ // TODO what happens after reset
usbd_ep_write_packet(usbd_dev, 0x82, buf, len);
}
else
usb_tx_in_progress = 0;
}
else {
__disable_irq();
if(fifoEmpty(&tx_fifo)){
usb_tx_in_progress = 0;
__enable_irq();
}
else {
__enable_irq();
cdcacm_data_tx_cb(usbd_dev,ep);
}
}
}
static void cdcacm_set_config(usbd_device *usbd_dev, uint16_t wValue)
{
(void)wValue;
usbd_ep_setup(usbd_dev, 0x01, USB_ENDPOINT_ATTR_BULK, 64, cdcacm_data_rx_cb);
usbd_ep_setup(usbd_dev, 0x82, USB_ENDPOINT_ATTR_BULK, 64, cdcacm_data_tx_cb);
usbd_ep_setup(usbd_dev, 0x83, USB_ENDPOINT_ATTR_INTERRUPT, 16, NULL);
usbd_register_control_callback(
usbd_dev,
USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE,
USB_REQ_TYPE_TYPE | USB_REQ_TYPE_RECIPIENT,
cdcacm_control_request);
usb_enabled = 1;
}
usbd_device * usb_init(void){
usbd_device * usbd_dev;
usbd_dev = usbd_init(&stm32f103_usb_driver, &dev, &config, usb_strings, 3, usbd_control_buffer, sizeof(usbd_control_buffer));
usbd_register_set_config_callback(usbd_dev, cdcacm_set_config);
return usbd_dev;
}

7
src/usb.h Normal file
View file

@ -0,0 +1,7 @@
#include <libopencm3/usb/usbd.h>
#include <libopencm3/usb/cdc.h>
usbd_device * usb_init(void);
extern uint8_t usb_tx_in_progress;
extern uint8_t usb_active;
extern usbd_device * usbd_dev;
void cdcacm_data_tx_cb(usbd_device*, uint8_t);