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); }