static void slc_watch_callback(void *arg)
{
struct hfp_slc_handle *handle = (struct hfp_slc_handle *)arg;
ssize_t bytes_read;
int err;
bytes_read = read(handle->rfcomm_fd,
&handle->buf[handle->buf_write_idx],
SLC_BUF_SIZE_BYTES - handle->buf_write_idx - 1);
if (bytes_read < 0) {
syslog(LOG_ERR, "Error reading slc command %s",
strerror(errno));
handle->disconnect_cb(handle);
return;
}
handle->buf_write_idx += bytes_read;
handle->buf[handle->buf_write_idx] = '\0';
while (handle->buf_read_idx != handle->buf_write_idx) {
char *end_char;
end_char = strchr(&handle->buf[handle->buf_read_idx], '\r');
if (end_char == NULL)
break;
*end_char = '\0';
err = handle_at_command(handle,
&handle->buf[handle->buf_read_idx]);
if (err < 0)
return;
/* Shift the read index */
handle->buf_read_idx = 1 + end_char - handle->buf;
if (handle->buf_read_idx == handle->buf_write_idx) {
handle->buf_read_idx = 0;
handle->buf_write_idx = 0;
}
}
/* Handle the case when buffer is full and no command found. */
if (handle->buf_write_idx == SLC_BUF_SIZE_BYTES - 1) {
if (handle->buf_read_idx) {
memmove(handle->buf,
&handle->buf[handle->buf_read_idx],
handle->buf_write_idx - handle->buf_read_idx);
handle->buf_write_idx -= handle->buf_read_idx;
handle->buf_read_idx = 0;
} else {
syslog(LOG_ERR,
"Parse SLC command error, clean up buffer");
handle->buf_write_idx = 0;
}
}
return;
}
/// main loop for time division multiplexing transparent serial
///
void
tdm_serial_loop(void)
{
__pdata uint16_t last_t = timer2_tick();
__pdata uint16_t last_link_update = last_t;
_canary = 42;
for (;;) {
__pdata uint8_t len;
__pdata uint16_t tnow, tdelta;
__pdata uint8_t max_xmit;
if (_canary != 42) {
panic("stack blown\n");
}
if (pdata_canary != 0x41) {
panic("pdata canary changed\n");
}
// give the AT command processor a chance to handle a command
at_command();
// display test data if needed
if (test_display) {
display_test_output();
test_display = 0;
}
if (seen_mavlink && feature_mavlink_framing && !at_mode_active) {
seen_mavlink = false;
MAVLink_report();
}
// set right receive channel
radio_set_channel(fhop_receive_channel());
// get the time before we check for a packet coming in
tnow = timer2_tick();
// see if we have received a packet
if (radio_receive_packet(&len, pbuf)) {
// update the activity indication
received_packet = true;
fhop_set_locked(true);
// update filtered RSSI value and packet stats
statistics.average_rssi = (radio_last_rssi() + 7*(uint16_t)statistics.average_rssi)/8;
statistics.receive_count++;
// we're not waiting for a preamble
// any more
transmit_wait = 0;
if (len < 2) {
// not a valid packet. We always send
// two control bytes at the end of every packet
continue;
}
// extract control bytes from end of packet
memcpy(&trailer, &pbuf[len-sizeof(trailer)], sizeof(trailer));
len -= sizeof(trailer);
if (trailer.window == 0 && len != 0) {
// its a control packet
if (len == sizeof(struct statistics)) {
memcpy(&remote_statistics, pbuf, len);
}
// don't count control packets in the stats
statistics.receive_count--;
} else if (trailer.window != 0) {
// sync our transmit windows based on
// received header
sync_tx_windows(len);
last_t = tnow;
if (trailer.command == 1) {
handle_at_command(len);
} else if (len != 0 &&
!packet_is_duplicate(len, pbuf, trailer.resend) &&
!at_mode_active) {
// its user data - send it out
// the serial port
//printf("rcv(%d,[", len);
LED_ACTIVITY = LED_ON;
serial_write_buf(pbuf, len);
LED_ACTIVITY = LED_OFF;
//printf("]\n");
}
}
continue;
}
// see how many 16usec ticks have passed and update
// the tdm state machine. We re-fetch tnow as a bad
// packet could have cost us a lot of time.
tnow = timer2_tick();
tdelta = tnow - last_t;
tdm_state_update(tdelta);
last_t = tnow;
// update link status every 0.5s
if (tnow - last_link_update > 32768) {
link_update();
last_link_update = tnow;
}
if (lbt_rssi != 0) {
// implement listen before talk
if (radio_current_rssi() < lbt_rssi) {
lbt_listen_time += tdelta;
} else {
lbt_listen_time = 0;
if (lbt_rand == 0) {
lbt_rand = ((uint16_t)rand()) % lbt_min_time;
}
}
if (lbt_listen_time < lbt_min_time + lbt_rand) {
// we need to listen some more
continue;
}
}
// we are allowed to transmit in our transmit window
// or in the other radios transmit window if we have
// bonus ticks
#if USE_TICK_YIELD
if (tdm_state != TDM_TRANSMIT &&
!(bonus_transmit && tdm_state == TDM_RECEIVE)) {
// we cannot transmit now
continue;
}
#else
if (tdm_state != TDM_TRANSMIT) {
continue;
}
#endif
if (transmit_yield != 0) {
// we've give up our window
continue;
}
if (transmit_wait != 0) {
// we're waiting for a preamble to turn into a packet
continue;
}
if (!received_packet &&
radio_preamble_detected() ||
radio_receive_in_progress()) {
// a preamble has been detected. Don't
// transmit for a while
transmit_wait = packet_latency;
continue;
}
// sample the background noise when it is out turn to
// transmit, but we are not transmitting,
// averaged over around 4 samples
statistics.average_noise = (radio_current_rssi() + 3*(uint16_t)statistics.average_noise)/4;
if (duty_cycle_wait) {
// we're waiting for our duty cycle to drop
continue;
}
// how many bytes could we transmit in the time we
// have left?
if (tdm_state_remaining < packet_latency) {
// none ....
continue;
}
max_xmit = (tdm_state_remaining - packet_latency) / ticks_per_byte;
if (max_xmit < PACKET_OVERHEAD) {
// can't fit the trailer in with a byte to spare
continue;
}
max_xmit -= PACKET_OVERHEAD;
if (max_xmit > max_data_packet_length) {
max_xmit = max_data_packet_length;
}
// ask the packet system for the next packet to send
if (send_at_command &&
max_xmit >= strlen(remote_at_cmd)) {
// send a remote AT command
len = strlen(remote_at_cmd);
memcpy(pbuf, remote_at_cmd, len);
trailer.command = 1;
send_at_command = false;
} else {
// get a packet from the serial port
memset(pbuf, 0x40, 16);
len = packet_get_next(max_xmit-16, pbuf+16) + 16;
trailer.command = packet_is_injected();
if (len == 16)
len = 0;
}
if (len > max_data_packet_length) {
panic("oversized tdm packet");
}
trailer.bonus = (tdm_state == TDM_RECEIVE);
trailer.resend = packet_is_resend();
if (tdm_state == TDM_TRANSMIT &&
len == 0 &&
send_statistics &&
max_xmit >= sizeof(statistics)) {
// send a statistics packet
send_statistics = 0;
memcpy(pbuf, &statistics, sizeof(statistics));
len = sizeof(statistics);
// mark a stats packet with a zero window
trailer.window = 0;
trailer.resend = 0;
} else {
// calculate the control word as the number of
// 16usec ticks that will be left in this
// tdm state after this packet is transmitted
trailer.window = (uint16_t)(tdm_state_remaining - flight_time_estimate(len+sizeof(trailer)));
}
// set right transmit channel
radio_set_channel(fhop_transmit_channel());
memcpy(&pbuf[len], &trailer, sizeof(trailer));
if (len != 0 && trailer.window != 0) {
// show the user that we're sending real data
LED_ACTIVITY = LED_ON;
}
if (len == 0) {
// sending a zero byte packet gives up
// our window, but doesn't change the
// start of the next window
transmit_yield = 1;
}
// after sending a packet leave a bit of time before
// sending the next one. The receivers don't cope well
// with back to back packets
transmit_wait = packet_latency;
// if we're implementing a duty cycle, add the
// transmit time to the number of ticks we've been transmitting
if ((duty_cycle - duty_cycle_offset) != 100) {
transmitted_ticks += flight_time_estimate(len+sizeof(trailer));
}
// start transmitting the packet
if (!radio_transmit(len + sizeof(trailer), pbuf, tdm_state_remaining + (silence_period/2)) &&
len != 0 && trailer.window != 0 && trailer.command == 0) {
packet_force_resend();
}
if (lbt_rssi != 0) {
// reset the LBT listen time
lbt_listen_time = 0;
lbt_rand = 0;
}
// set right receive channel
radio_set_channel(fhop_receive_channel());
// re-enable the receiver
radio_receiver_on();
if (len != 0 && trailer.window != 0) {
LED_ACTIVITY = LED_OFF;
}
}
}
static void process_input(struct hfp_gw *hfp)
{
char *str, *ptr;
size_t len, count;
bool free_ptr = false;
bool read_again;
do {
str = ringbuf_peek(hfp->read_buf, 0, &len);
if (!str)
return;
ptr = memchr(str, '\r', len);
if (!ptr) {
char *str2;
size_t len2;
/*
* If there is no more data in ringbuffer,
* it's just an incomplete command.
*/
if (len == ringbuf_len(hfp->read_buf))
return;
str2 = ringbuf_peek(hfp->read_buf, len, &len2);
if (!str2)
return;
ptr = memchr(str2, '\r', len2);
if (!ptr)
return;
*ptr = '\0';
count = len2 + len;
ptr = malloc(count);
if (!ptr)
return;
memcpy(ptr, str, len);
memcpy(ptr + len, str2, len2);
free_ptr = true;
str = ptr;
} else {
count = ptr - str;
*ptr = '\0';
}
if (!handle_at_command(hfp, str))
/*
* Command is not handled that means that was some
* trash. Let's skip that and keep reading from ring
* buffer.
*/
read_again = true;
else
/*
* Command has been handled. If we are waiting for a
* result from upper layer, we can stop reading. If we
* already reply i.e. ERROR on unknown command, then we
* can keep reading ring buffer. Actually ring buffer
* should be empty but lets just look there.
*/
read_again = !hfp->result_pending;
ringbuf_drain(hfp->read_buf, count + 1);
if (free_ptr)
free(ptr);
} while (read_again);
}
