int ubertooth_bulk_receive(ubertooth_t* ut, rx_callback cb, void* cb_args)
{
int i;
usb_pkt_rx* rx;
if (ut->usb_really_full) {
/* process each received block */
for (i = 0; i < PKTS_PER_XFER; i++) {
rx = (usb_pkt_rx *)(ut->full_usb_buf + PKT_LEN * i);
if(rx->pkt_type != KEEP_ALIVE) {
ringbuffer_add(ut->packets, rx);
(*cb)(ut, cb_args);
}
if(ut->stop_ubertooth) {
if(ut->rx_xfer)
libusb_cancel_transfer(ut->rx_xfer);
return 1;
}
}
ut->usb_really_full = 0;
fflush(stderr);
return 0;
} else {
return -1;
}
}
int main(void)
{
char *status = "I am written to the UART every 2 seconds\n";
char buf[128];
int res;
msg_t msg;
main_pid = thread_getpid();
printf("Main thread pid %i \n", main_pid);
printf("Testing interrupt driven mode of UART driver\n\n");
printf("Setting up buffers...\n");
ringbuffer_init(&rx_buf, rx_mem, 128);
ringbuffer_init(&tx_buf, tx_mem, 128);
printf("Initializing UART @ %i", BAUD);
if (uart_init(DEV, BAUD, rx, NULL, tx, NULL) >= 0) {
printf(" ...done\n");
}
else {
printf(" ...failed\n");
return 1;
}
ringbuffer_add(&tx_buf, status, strlen(status));
uart_tx_begin(DEV);
while (1) {
printf("Going into receive message state\n");
//msg_receive(&msg);
if (status) {
printf("INPUT: ");
res = ringbuffer_get(&rx_buf, buf, rx_buf.avail);
buf[res] = '\0';
printf("%s", buf);
status = 0;
}
/* printf("got message");
if (msg.type == MSG_LINE_RDY) {
printf("INPUT: ");
res = ringbuffer_get(&rx_buf, buf, rx_buf.avail);
buf[res] = '\0';
printf("%s", buf);
}
*/
}
return 0;
}
void ICACHE_FLASH_ATTR com_send(const void *data, const uint8_t length, const int8_t cid) {
if(cid == -2) { // UART
tfp_handle_packet(data, length);
} else { // WIFI
// If ringbuffer not empty we add data to ringbuffer (we want to keep order)
// Else if we can't immediately send to master brick we also add to ringbuffer
if(!ringbuffer_is_empty(&com_out_rb) || tfp_send_w_cid(data, length, cid) == 0) {
if(ringbuffer_get_free(&com_out_rb) > (length + 2 + 1)) {
ringbuffer_add(&com_out_rb, cid);
for(uint8_t i = 0; i < length; i++) {
if(!ringbuffer_add(&com_out_rb, ((uint8_t*)data)[i])) {
// Should be unreachable
loge("Ringbuffer (com out) full!\n");
}
}
} else {
logw("Message does not fit in Buffer (com out): %d < %d\n", ringbuffer_get_free(&com_out_rb), length + 2);
}
}
}
}
void ICACHE_FLASH_ATTR tfp_handle_packet(const uint8_t *data, const uint8_t length) {
// If ringbuffer not empty we add data to ringbuffer (we want to keep order)
// Else if we can't immediately send to master brick we also add to ringbuffer
if(!ringbuffer_is_empty(&tfp_rb) || uart_con_send(data, length) == 0) {
if(ringbuffer_get_free(&tfp_rb) > (length + 2)) {
for(uint8_t i = 0; i < length; i++) {
if(!ringbuffer_add(&tfp_rb, data[i])) {
// Should be unreachable
loge("Ringbuffer full!\n");
}
}
} else {
logw("Message does not fit in Buffer: %d < %d\n", ringbuffer_get_free(&tfp_rb), length + 2);
}
}
}
void usbdev_acm_evt_out(usbdev_t *dev)
{
usbdev_ops_t *driver = dev->driver;
size_t l = driver->read_ep(ENDPOINT_ADDR_OUT(USBDEV_ACM_EP_BULKOUT),
rec_buffer, USBDEV_ACM_EP_PACKET_SIZE);
ringbuffer_add(&cdcacm_rx_rb, (char*)rec_buffer, l);
if (ucb_config.rx_cb != NULL) {
int retval = ringbuffer_get_one(&cdcacm_rx_rb);
while (retval != -1) {
ucb_config.rx_cb(ucb_config.arg, (char)retval);
retval = ringbuffer_get_one(&cdcacm_rx_rb);
}
}
}
bool ICACHE_FLASH_ATTR tfp_send_w_cid(const uint8_t *data, const uint8_t length, const uint8_t cid) {
uint8_t i = 0;
if(!configuration_current.mesh_enable) {
/*
* FIXME: Shouldn't the buffering while sending mechanism be also used for
* non-mesh case? As it is documented that packets should be sent from the
* sent callback of the previous packet.
*/
if(tfp_cons[cid].state == TFP_CON_STATE_OPEN) {
tfp_cons[cid].state = TFP_CON_STATE_SENDING;
os_memcpy(tfp_cons[cid].send_buffer, data, length);
espconn_send(tfp_cons[cid].con, (uint8_t*)data, length);
return true;
}
return false;
}
else {
if(tfp_cons[cid].state == TFP_CON_STATE_OPEN) {
os_memcpy(tfp_cons[cid].send_buffer, data, length);
tfp_mesh_send(tfp_cons[cid].con, (uint8_t*)data, length);
return true;
}
else {
// Check if there is enough space in the send buffer.
if(tfp_mesh_send_check_buffer(length)) {
// Store the packet in the TFP mesh send buffer.
for(i = 0; i < length; i++) {
if(!ringbuffer_add(&tfp_mesh_send_rb, data[i])) {
return false;
}
}
return true;
}
else {
return false;
}
}
}
}
/* file should be in full USB packet format (ubertooth-dump -f) */
int stream_rx_file(FILE* fp, rx_callback cb, void* cb_args)
{
uint8_t buf[BUFFER_SIZE];
size_t nitems;
ubertooth_t* ut = ubertooth_init();
if (ut == NULL)
return -1;
while(1) {
uint32_t systime_be;
nitems = fread(&systime_be, sizeof(systime_be), 1, fp);
if (nitems != 1)
return 0;
systime = (time_t)be32toh(systime_be);
nitems = fread(buf, sizeof(buf[0]), PKT_LEN, fp);
if (nitems != PKT_LEN)
return 0;
ringbuffer_add(ut->packets, (usb_pkt_rx*)buf);
(*cb)(ut, cb_args);
}
}
bool ICACHE_FLASH_ATTR tfp_send(const uint8_t *data, const uint8_t length) {
uint8_t i = 0;
// TODO: Sanity check length again?
// TODO: Are we sure that data is always a full TFP packet?
// cid == -2 => send back via UART
if(com_handle_message(data, length, -2)) {
return true;
}
// We only peak at the routing table here (and delete the route manually if
// we can fit the data in our buffer). It would be very expensive to first
// peak and then discover the route again.
BrickdRouting *match = NULL;
int8_t cid = brickd_route_to_peak(data, &match);
if(!brickd_check_auth((const MessageHeader*)data, cid)) {
return true;
}
/*
* First let's check if everything fits in the buffers. This is only done if
* mesh isn't enabled. When mesh is enabled, dedicated buffer is used for
* sending.
*/
if(!configuration_current.mesh_enable) {
if(!tfp_send_check_buffer(cid)) {
return false;
}
}
// Add websocket header if necessary
uint8_t data_with_websocket_header[TFP_SEND_BUFFER_SIZE + sizeof(WebsocketFrameClientToServer)];
int16_t length_with_websocket_header = tfpw_insert_websocket_header(cid, data_with_websocket_header, data, length);
if(length_with_websocket_header == -1) { // -1 = We use websocket but state is not OK for sending
return false;
}
// Remove match from brickd routing table only if we now that we can fit
// the data in the buffer
if(match != NULL) {
match->uid = 0;
match->func_id = 0;
match->sequence_number = 0;
match->cid = -1;
}
/*
* FIXME: Shouldn't the buffering while sending mechanism be also used for
* non-mesh case? As it is documented that packets should be sent from the
* sent callback of the previous packet.
*/
// Broadcast.
if(cid == -1) {
/*
* Broadcast is handled differently when mesh is enabled as then there is
* only one socket connection invovled.
*/
if (!configuration_current.mesh_enable) {
for(uint8_t i = 0; i < TFP_MAX_CONNECTIONS; i++) {
if(tfp_cons[i].state == TFP_CON_STATE_OPEN) {
// TODO: sent data (return value)
tfp_cons[i].state = TFP_CON_STATE_SENDING;
uint8_t length_to_send = length;
if(tfp_cons[i].websocket_state == WEBSOCKET_STATE_NO_WEBSOCKET) {
os_memcpy(tfp_cons[i].send_buffer, data, length);
}
else {
os_memcpy(tfp_cons[i].send_buffer, data_with_websocket_header, length_with_websocket_header);
length_to_send = length_with_websocket_header;
}
espconn_send(tfp_cons[i].con, tfp_cons[i].send_buffer, length_to_send);
}
}
}
else {
os_memcpy(tfp_cons[0].send_buffer, data, length);
// Check if the socket is in a state to be able to send.
if(tfp_cons[0].state == TFP_CON_STATE_OPEN) {
tfp_mesh_send(tfp_cons[0].con, tfp_cons[0].send_buffer, length);
}
/*
* If the socket can't send at the moment buffer the packet in TFP mesh
* send buffer for sending in future.
*/
else {
if(tfp_mesh_send_check_buffer(length)) {
for(i = 0; i < length; i++) {
if(!ringbuffer_add(&tfp_mesh_send_rb, data[i])) {
return false;
}
}
}
else {
return false;
}
}
}
}
// Unicast.
else {
uint8_t length_to_send = length;
if(!configuration_current.mesh_enable) {
// When mesh mode is enabled this socket state is updated from tfp_mesh_send().
tfp_cons[cid].state = TFP_CON_STATE_SENDING;
}
if(tfp_cons[cid].websocket_state == WEBSOCKET_STATE_NO_WEBSOCKET) {
os_memcpy(tfp_cons[cid].send_buffer, data, length);
}
else {
os_memcpy(tfp_cons[cid].send_buffer, data_with_websocket_header, length_with_websocket_header);
length_to_send = length_with_websocket_header;
}
if(configuration_current.mesh_enable) {
// Check if the socket is in a state to be able to send.
if(tfp_cons[0].state == TFP_CON_STATE_OPEN) {
tfp_mesh_send(tfp_cons[0].con, tfp_cons[0].send_buffer, length_to_send);
}
/*
* If the socket can't send at the moment buffer the packet in TFP mesh
* send buffer for sending in future.
*/
else {
if(tfp_mesh_send_check_buffer(length_to_send)) {
for(i = 0; i < length_to_send; i++) {
if(!ringbuffer_add(&tfp_mesh_send_rb, data[i])) {
return false;
}
}
}
else {
return false;
}
}
}
else {
espconn_send(tfp_cons[cid].con, tfp_cons[cid].send_buffer, length_to_send);
}
}
if(ringbuffer_get_free(&tfp_rb) > (6*MTU_LENGTH + 2)) {
tfp_recv_unhold();
}
return true;
}
void acm_uart_write(uart_t uart, const uint8_t *data, size_t len)
{
mutex_lock(&tx_rb_lock);
ringbuffer_add(&cdcacm_tx_rb, (char*)data, len);
mutex_unlock(&tx_rb_lock);
}
static void bricklet_stack_transceive(BrickletStack *bricklet_stack) {
// If we have not seen any data from the Bricklet we increase a counter.
// If the counter reaches BRICKLET_STACK_FIRST_MESSAGE_TRIES we assume that
// there is no Bricklet and we stop trying to send to initial message (if a
// Bricklet is hotplugged it will send a enumerate itself).
if(!bricklet_stack->data_seen) {
if(bricklet_stack->first_message_tries < BRICKLET_STACK_FIRST_MESSAGE_TRIES) {
bricklet_stack->first_message_tries++;
} else {
bricklet_stack->buffer_send_length = 0;
}
}
const uint16_t length_read = bricklet_stack_check_missing_length(bricklet_stack);
if(bricklet_stack->buffer_send_length == 0) {
// If buffer is empty we try to send request from the queue.
bricklet_stack_check_request_queue(bricklet_stack);
if((bricklet_stack->buffer_send_length == 0) && (bricklet_stack->ack_to_send)) {
// If there is no request in the queue (buffer still empty)
// and we have to send an ACK still, we send the ACK.
bricklet_stack_send_ack(bricklet_stack);
}
}
uint16_t length_write = bricklet_stack->wait_for_ack ? 0 : bricklet_stack->buffer_send_length;
uint16_t length = MAX(MAX(length_read, length_write), 1);
uint8_t rx[SPITFP_MAX_TFP_MESSAGE_LENGTH] = {0};
uint8_t tx[SPITFP_MAX_TFP_MESSAGE_LENGTH] = {0};
if((length == 1) || (!bricklet_stack->data_seen)) {
// If there is nothing to read or to write, we give the Bricklet some breathing
// room before we start polling again.
// If we have nothing to send and we are currently not awaiting data from the Bricklet, we will
// poll every 200 us.
uint32_t sleep_us = 200;
if(!bricklet_stack->data_seen) {
// If we have never seen any data, we will first poll every 1ms with the StackEnumerate message
// and switch to polling every 500ms after we tried BRICKLET_STACK_FIRST_MESSAGE_TRIES times.
// In this case there is likely no Bricklet connected. If a Bricklet is hotpluged "data_seen"
// will be true and we will switch to polling every 200us immediately.
if(bricklet_stack->first_message_tries < BRICKLET_STACK_FIRST_MESSAGE_TRIES) {
sleep_us = 1*1000;
} else {
sleep_us = 500*1000;
}
}
struct timespec t;
t.tv_sec = 0;
t.tv_nsec = 1000*sleep_us;
clock_nanosleep(CLOCK_MONOTONIC, 0, &t, NULL);
}
memcpy(tx, bricklet_stack->buffer_send, length_write);
struct spi_ioc_transfer spi_transfer = {
.tx_buf = (unsigned long)tx,
.rx_buf = (unsigned long)rx,
.len = length,
};
// Make sure that we only access SPI once at a time
mutex_lock(bricklet_stack->config.mutex);
// Do chip select by hand if necessary
if(bricklet_stack->config.chip_select_driver == CHIP_SELECT_GPIO) {
if(gpio_sysfs_set_output(&bricklet_stack->config.chip_select_gpio_sysfs, GPIO_SYSFS_VALUE_LOW) < 0) {
log_error("Could not enable chip select");
return;
}
}
int rc = ioctl(bricklet_stack->spi_fd, SPI_IOC_MESSAGE(1), &spi_transfer);
// If the length is 1 (i.e. we wanted to see if the SPI slave has data for us)
// and he does have data for us, we will immediately retrieve the data without
// giving back the mutex.
if((length == 1) && (rx[0] != 0) && (rc == length) && (length_write == 0)) {
// First add the one byte of already received data to the ringbuffer
ringbuffer_add(&bricklet_stack->ringbuffer_recv, rx[0]);
// Set rc to 0, so if there is no more data to read, we don't get the
// "unexpected result" error
rc = 0;
// Get length for rest of message
length = bricklet_stack_check_missing_length(bricklet_stack);
if(length != 0) {
// Set first byte back to 0 and the new length, the rest was not touched
// and we don't need to reinizialize it.
rx[0] = 0;
spi_transfer.len = length;
rc = ioctl(bricklet_stack->spi_fd, SPI_IOC_MESSAGE(1), &spi_transfer);
}
}
// Do chip deselect by hand if necessary
if(bricklet_stack->config.chip_select_driver == CHIP_SELECT_GPIO) {
if(gpio_sysfs_set_output(&bricklet_stack->config.chip_select_gpio_sysfs, GPIO_SYSFS_VALUE_HIGH) < 0) {
log_error("Could not disable chip select");
return;
}
}
mutex_unlock(bricklet_stack->config.mutex);
if (rc < 0) {
log_error("ioctl failed: %s (%d)", get_errno_name(errno), errno);
return;
}
if (rc != length) {
log_error("ioctl has unexpected result (actual: %d != expected: %d)", rc, length);
return;
}
// We don't expect an ACK to be acked, so we can set the length to 0 here
if(bricklet_stack->buffer_send_length == SPITFP_PROTOCOL_OVERHEAD) {
bricklet_stack->buffer_send_length = 0;
}
if(bricklet_stack->buffer_send_length >= SPITFP_MIN_TFP_MESSAGE_LENGTH) {
bricklet_stack->wait_for_ack = true;
}
for(uint16_t i = 0; i < length; i++) {
ringbuffer_add(&bricklet_stack->ringbuffer_recv, rx[i]);
}
}
static void bricklet_stack_spi_thread(void *opaque) {
BrickletStack *bricklet_stack = (BrickletStack*)opaque;
bricklet_stack->spi_thread_running = true;
// Depending on the configuration we wait on startup for
// other Bricklets to identify themself first.
struct timespec t = {
.tv_sec = bricklet_stack->config.startup_wait_time,
};
clock_nanosleep(CLOCK_MONOTONIC, 0, &t, NULL);
// Pre-fill the send buffer with the "StackEnumerate"-Packet.
// This packet will trigger an initial enumeration in the Bricklet.
// If the Brick Daemon is restarted, we need to
// trigger the initial enumeration, since the Bricklet does not know
// that it has to enumerate itself again.
PacketHeader header = {
.uid = 0,
.length = sizeof(PacketHeader),
.function_id = FUNCTION_STACK_ENUMERATE,
.sequence_number_and_options = 0x08, // return expected
.error_code_and_future_use = 0
};
bricklet_stack_send_ack_and_message(bricklet_stack, (uint8_t*)&header, sizeof(PacketHeader));
while (bricklet_stack->spi_thread_running) {
bricklet_stack_transceive(bricklet_stack);
bricklet_stack_check_message(bricklet_stack);
}
}
static int bricklet_stack_init_spi(BrickletStack *bricklet_stack) {
// Use hw chip select if it is done by SPI hardware unit, otherwise set SPI_NO_CS flag.
const int mode = BRICKLET_STACK_SPI_CONFIG_MODE | (bricklet_stack->config.chip_select_driver == CHIP_SELECT_HARDWARE ? 0 : SPI_NO_CS);
const int lsb_first = BRICKLET_STACK_SPI_CONFIG_LSB_FIRST;
const int bits_per_word = BRICKLET_STACK_SPI_CONFIG_BITS_PER_WORD;
const int max_speed_hz = BRICKLET_STACK_SPI_CONFIG_MAX_SPEED_HZ;
// Open spidev
bricklet_stack->spi_fd = open(bricklet_stack->config.spi_device, O_RDWR);
if (bricklet_stack->spi_fd < 0) {
log_error("Could not open %s: : %s (%d)",
bricklet_stack->config.spi_device, get_errno_name(errno), errno);
return -1;
}
if (ioctl(bricklet_stack->spi_fd, SPI_IOC_WR_MODE, &mode) < 0) {
log_error("Could not configure SPI mode: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
if (ioctl(bricklet_stack->spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &max_speed_hz) < 0) {
log_error("Could not configure SPI max speed: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
if (ioctl(bricklet_stack->spi_fd, SPI_IOC_WR_BITS_PER_WORD, &bits_per_word) < 0) {
log_error("Could not configure SPI bits per word: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
if (ioctl(bricklet_stack->spi_fd, SPI_IOC_WR_LSB_FIRST, &lsb_first) < 0) {
log_error("Could not configure SPI lsb first: %s (%d)",
get_errno_name(errno), errno);
return -1;
}
thread_create(&bricklet_stack->spi_thread, bricklet_stack_spi_thread, bricklet_stack);
return 0;
}
BrickletStack* bricklet_stack_init(BrickletStackConfig *config) {
int phase = 0;
char bricklet_stack_name[129] = {'\0'};
char notification_name[129] = {'\0'};
log_debug("Initializing BrickletStack subsystem for '%s' (num %d)", config->spi_device, config->chip_select_gpio_sysfs.num);
if(config->chip_select_driver == CHIP_SELECT_GPIO) {
if(gpio_sysfs_export(&config->chip_select_gpio_sysfs) < 0) {
goto cleanup;
}
if(gpio_sysfs_set_direction(&config->chip_select_gpio_sysfs, GPIO_SYSFS_DIRECTION_OUTPUT) < 0) {
goto cleanup;
}
if(gpio_sysfs_set_output(&config->chip_select_gpio_sysfs, GPIO_SYSFS_VALUE_HIGH) < 0) {
goto cleanup;
}
}
// create bricklet_stack struct
BrickletStack *bricklet_stack = (BrickletStack*)malloc(sizeof(BrickletStack));
if(bricklet_stack == NULL) {
goto cleanup;
}
memset(bricklet_stack, 0, sizeof(BrickletStack));
bricklet_stack->spi_fd = -1;
bricklet_stack->spi_thread_running = false;
memcpy(&bricklet_stack->config, config, sizeof(BrickletStackConfig));
ringbuffer_init(&bricklet_stack->ringbuffer_recv,
BRICKLET_STACK_SPI_RECEIVE_BUFFER_LENGTH,
bricklet_stack->buffer_recv);
// create base stack
if (snprintf(bricklet_stack_name, 128, "bricklet-stack-%s", bricklet_stack->config.spi_device) < 0) {
goto cleanup;
}
if (stack_create(&bricklet_stack->base, bricklet_stack_name, bricklet_stack_dispatch_to_spi) < 0) {
log_error("Could not create base stack for BrickletStack: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
phase = 1;
// add to stacks array
if (hardware_add_stack(&bricklet_stack->base) < 0) {
goto cleanup;
}
phase = 2;
if ((bricklet_stack->notification_event = eventfd(0, EFD_NONBLOCK | EFD_SEMAPHORE)) < 0) {
log_error("Could not create bricklet notification event: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
phase = 3;
// Add notification pipe as event source.
// Event is used to dispatch packets.
if (snprintf(notification_name, 128, "bricklet-stack-notification-%s", bricklet_stack->config.spi_device) < 0) {
goto cleanup;
}
if (event_add_source(bricklet_stack->notification_event, EVENT_SOURCE_TYPE_GENERIC,
notification_name, EVENT_READ,
bricklet_stack_dispatch_from_spi, bricklet_stack) < 0) {
log_error("Could not add bricklet notification pipe as event source");
goto cleanup;
}
phase = 4;
// Initialize SPI packet queues
if (queue_create(&bricklet_stack->request_queue, sizeof(Packet)) < 0) {
log_error("Could not create SPI request queue: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
mutex_create(&bricklet_stack->request_queue_mutex);
phase = 5;
if (queue_create(&bricklet_stack->response_queue, sizeof(Packet)) < 0) {
log_error("Could not create SPI response queue: %s (%d)",
get_errno_name(errno), errno);
goto cleanup;
}
mutex_create(&bricklet_stack->response_queue_mutex);
phase = 6;
if (bricklet_stack_init_spi(bricklet_stack) < 0) {
goto cleanup;
}
phase = 7;
cleanup:
switch (phase) { // no breaks, all cases fall through intentionally
case 6:
mutex_destroy(&bricklet_stack->response_queue_mutex);
queue_destroy(&bricklet_stack->response_queue, NULL);
// fall through
case 5:
mutex_destroy(&bricklet_stack->request_queue_mutex);
queue_destroy(&bricklet_stack->request_queue, NULL);
// fall through
case 4:
event_remove_source(bricklet_stack->notification_event, EVENT_SOURCE_TYPE_GENERIC);
// fall through
case 3:
robust_close(bricklet_stack->notification_event);
// fall through
case 2:
hardware_remove_stack(&bricklet_stack->base);
// fall through
case 1:
stack_destroy(&bricklet_stack->base);
// fall through
default:
break;
}
return phase == 7 ? bricklet_stack : NULL;
}
void bricklet_stack_exit(BrickletStack *bricklet_stack) {
// Remove event as possible poll source
event_remove_source(bricklet_stack->notification_event, EVENT_SOURCE_TYPE_GENERIC);
// Make sure that Thread shuts down properly
if (bricklet_stack->spi_thread_running) {
bricklet_stack->spi_thread_running = false;
thread_join(&bricklet_stack->spi_thread);
thread_destroy(&bricklet_stack->spi_thread);
}
hardware_remove_stack(&bricklet_stack->base);
stack_destroy(&bricklet_stack->base);
queue_destroy(&bricklet_stack->request_queue, NULL);
mutex_destroy(&bricklet_stack->request_queue_mutex);
queue_destroy(&bricklet_stack->response_queue, NULL);
mutex_destroy(&bricklet_stack->response_queue_mutex);
// Close file descriptors
robust_close(bricklet_stack->notification_event);
robust_close(bricklet_stack->spi_fd);
// Everything is closed and the threads are destroyed. We can
// now free the Bricklet Stack memory. It will not be accessed anymore.
free(bricklet_stack);
}
int main(int argc, char *argv[])
{
int opt;
int do_follow, do_promisc;
int do_get_aa, do_set_aa;
int do_crc;
int do_adv_index;
int do_slave_mode;
int do_target;
enum jam_modes jam_mode = JAM_NONE;
char ubertooth_device = -1;
ubertooth_t* ut = ubertooth_init();
btle_options cb_opts = { .allowed_access_address_errors = 32 };
int r;
u32 access_address;
uint8_t mac_address[6] = { 0, };
do_follow = do_promisc = 0;
do_get_aa = do_set_aa = 0;
do_crc = -1; // 0 and 1 mean set, 2 means get
do_adv_index = 37;
do_slave_mode = do_target = 0;
while ((opt=getopt(argc,argv,"a::r:hfpU:v::A:s:t:x:c:q:jJiI")) != EOF) {
switch(opt) {
case 'a':
if (optarg == NULL) {
do_get_aa = 1;
} else {
do_set_aa = 1;
sscanf(optarg, "%08x", &access_address);
}
break;
case 'f':
do_follow = 1;
break;
case 'p':
do_promisc = 1;
break;
case 'U':
ubertooth_device = atoi(optarg);
break;
case 'r':
if (!ut->h_pcapng_le) {
if (lell_pcapng_create_file(optarg, "Ubertooth", &ut->h_pcapng_le)) {
err(1, "lell_pcapng_create_file: ");
}
}
else {
printf("Ignoring extra capture file: %s\n", optarg);
}
break;
#ifdef ENABLE_PCAP
case 'q':
if (!ut->h_pcap_le) {
if (lell_pcap_create_file(optarg, &ut->h_pcap_le)) {
err(1, "lell_pcap_create_file: ");
}
}
else {
printf("Ignoring extra capture file: %s\n", optarg);
}
break;
case 'c':
if (!ut->h_pcap_le) {
if (lell_pcap_ppi_create_file(optarg, 0, &ut->h_pcap_le)) {
err(1, "lell_pcap_ppi_create_file: ");
}
}
else {
printf("Ignoring extra capture file: %s\n", optarg);
}
break;
#endif
case 'v':
if (optarg)
do_crc = atoi(optarg) ? 1 : 0;
else
do_crc = 2; // get
break;
case 'A':
do_adv_index = atoi(optarg);
if (do_adv_index < 37 || do_adv_index > 39) {
printf("Error: advertising index must be 37, 38, or 39\n");
usage();
return 1;
}
break;
case 's':
do_slave_mode = 1;
r = convert_mac_address(optarg, mac_address);
if (!r) {
usage();
return 1;
}
break;
case 't':
do_target = 1;
r = convert_mac_address(optarg, mac_address);
if (!r) {
usage();
return 1;
}
break;
case 'x':
cb_opts.allowed_access_address_errors = (unsigned) atoi(optarg);
if (cb_opts.allowed_access_address_errors > 32) {
printf("Error: can tolerate 0-32 access address bit errors\n");
usage();
return 1;
}
break;
case 'i':
case 'j':
jam_mode = JAM_ONCE;
break;
case 'I':
case 'J':
jam_mode = JAM_CONTINUOUS;
break;
case 'h':
default:
usage();
return 1;
}
}
r = ubertooth_connect(ut, ubertooth_device);
if (r < 0) {
usage();
return 1;
}
/* Clean up on exit. */
register_cleanup_handler(ut);
if (do_follow && do_promisc) {
printf("Error: must choose either -f or -p, one or the other pal\n");
return 1;
}
if (do_follow || do_promisc) {
usb_pkt_rx rx;
r = cmd_set_jam_mode(ut->devh, jam_mode);
if (jam_mode != JAM_NONE && r != 0) {
printf("Jamming not supported\n");
return 1;
}
cmd_set_modulation(ut->devh, MOD_BT_LOW_ENERGY);
if (do_follow) {
u16 channel;
if (do_adv_index == 37)
channel = 2402;
else if (do_adv_index == 38)
channel = 2426;
else
channel = 2480;
cmd_set_channel(ut->devh, channel);
cmd_btle_sniffing(ut->devh, 2);
} else {
cmd_btle_promisc(ut->devh);
}
while (1) {
int r = cmd_poll(ut->devh, &rx);
if (r < 0) {
printf("USB error\n");
break;
}
if (r == sizeof(usb_pkt_rx)) {
ringbuffer_add(ut->packets, &rx);
cb_btle(ut, &cb_opts);
}
usleep(500);
}
ubertooth_stop(ut);
}
if (do_get_aa) {
access_address = cmd_get_access_address(ut->devh);
printf("Access address: %08x\n", access_address);
return 0;
}
if (do_set_aa) {
cmd_set_access_address(ut->devh, access_address);
printf("access address set to: %08x\n", access_address);
}
if (do_crc >= 0) {
int r;
if (do_crc == 2) {
r = cmd_get_crc_verify(ut->devh);
} else {
cmd_set_crc_verify(ut->devh, do_crc);
r = do_crc;
}
printf("CRC: %sverify\n", r ? "" : "DO NOT ");
}
if (do_slave_mode) {
u16 channel;
if (do_adv_index == 37)
channel = 2402;
else if (do_adv_index == 38)
channel = 2426;
else
channel = 2480;
cmd_set_channel(ut->devh, channel);
cmd_btle_slave(ut->devh, mac_address);
}
if (do_target) {
r = cmd_btle_set_target(ut->devh, mac_address);
if (r == 0) {
int i;
printf("target set to: ");
for (i = 0; i < 5; ++i)
printf("%02x:", mac_address[i]);
printf("%02x\n", mac_address[5]);
}
}
if (!(do_follow || do_promisc || do_get_aa || do_set_aa ||
do_crc >= 0 || do_slave_mode || do_target))
usage();
return 0;
}
int main(int argc, char *argv[])
{
int opt;
int do_mode = -1;
int do_channel = 2418;
char ubertooth_device = -1;
int r;
while ((opt=getopt(argc,argv,"frijc:U:h")) != EOF) {
switch(opt) {
case 'f':
do_mode = 0;
break;
case 'r':
do_mode = 1;
break;
case 'i':
case 'j':
do_mode = 2; // TODO take care of these magic numbers
break;
case 'c':
do_channel = atoi(optarg);
break;
case 'U':
ubertooth_device = atoi(optarg);
break;
case 'h':
default:
usage();
return 1;
}
}
ut = ubertooth_start(ubertooth_device);
if (ut == NULL) {
usage();
return 1;
}
/* Clean up on exit. */
register_cleanup_handler(ut);
if (do_mode >= 0) {
usb_pkt_rx rx;
if (do_mode == 1) // FIXME magic number!
cmd_set_channel(ut->devh, do_channel);
r = cmd_ego(ut->devh, do_mode);
if (r < 0) {
if (do_mode == 0 || do_mode == 1)
printf("Error: E-GO not supported by this firmware\n");
else
printf("Error: E-GO not supported by this firmware (or TX not enabled)\n");
return 1;
}
while (1) {
int r = cmd_poll(ut->devh, &rx);
if (r < 0) {
printf("USB error\n");
break;
}
if (r == sizeof(usb_pkt_rx)) {
ringbuffer_add(ut->packets, &rx);
cb_ego(ut, NULL);
}
usleep(500);
}
ubertooth_stop(ut);
}
return 0;
}
