static int evt_write_sandisk_param(struct state *st, struct pkt *pkt) {
struct evt_nand_unk_sandisk_param evt;
struct pkt second_pkt, third_pkt;
evt_fill_header(&evt, pkt->header.sec, pkt->header.nsec,
sizeof(evt), EVT_NAND_SANDISK_VENDOR_PARAM);
// Make sure the subsequent packet is an address
packet_get_next(st, &second_pkt);
if (!nand_ale(second_pkt.data.nand_cycle.control)
&& !nand_we(second_pkt.data.nand_cycle.control)) {
fprintf(stderr, "Not a Sandisk param packet!\n");
evt_write_nand_unk(st, pkt);
evt_write_nand_unk(st, &second_pkt);
return 0;
}
packet_get_next(st, &third_pkt);
if (nand_ale(third_pkt.data.nand_cycle.control)
|| nand_cle(third_pkt.data.nand_cycle.control)
|| nand_re(third_pkt.data.nand_cycle.control)) {
fprintf(stderr, "Not a Sandisk param packet!\n");
evt_write_nand_unk(st, pkt);
evt_write_nand_unk(st, &second_pkt);
evt_write_nand_unk(st, &third_pkt);
return 0;
}
evt.addr = second_pkt.data.nand_cycle.data;
evt.data = third_pkt.data.nand_cycle.data;
evt_fill_end(&evt, third_pkt.header.sec, third_pkt.header.nsec);
write(st->out_fd, &evt, sizeof(evt));
return 0;
}
static int evt_write_id(struct state *st, struct pkt *pkt) {
struct evt_nand_id evt;
evt_fill_header(&evt, pkt->header.sec, pkt->header.nsec,
sizeof(evt), EVT_NAND_ID);
// Grab the "address" byte.
packet_get_next(st, pkt);
if (!nand_ale(pkt->data.nand_cycle.control)
|| !nand_we(pkt->data.nand_cycle.control))
fprintf(stderr, "Warning: ALE/WE not set for 'Read ID'\n");
evt.addr = pkt->data.nand_cycle.data;
// Read the actual ID
evt_fill_end(&evt, pkt->header.sec, pkt->header.nsec);
packet_get_next(st, pkt);
for (evt.size=0;
evt.size<sizeof(evt.id) && nand_re(pkt->data.nand_cycle.control);
evt.size++) {
evt.id[evt.size] = pkt->data.nand_cycle.data;
evt_fill_end(&evt, pkt->header.sec, pkt->header.nsec);
packet_get_next(st, pkt);
}
if (!nand_re(pkt->data.nand_cycle.control))
packet_unget(st, pkt);
evt_fill_end(&evt, pkt->header.sec, pkt->header.nsec);
write(st->out_fd, &evt, sizeof(evt));
return 0;
}
static int evt_write_sandisk_charge2(struct state *st, struct pkt *pkt) {
struct evt_nand_sandisk_charge2 evt;
struct pkt second_pkt, third_pkt, fourth_pkt;
evt_fill_header(&evt, pkt->header.sec, pkt->header.nsec,
sizeof(evt), EVT_NAND_SANDISK_CHARGE1);
// Make sure the subsequent packet is an address
packet_get_next(st, &second_pkt);
if (!nand_ale(second_pkt.data.nand_cycle.control)
|| nand_cle(second_pkt.data.nand_cycle.control)
|| !nand_we(second_pkt.data.nand_cycle.control)) {
fprintf(stderr, "Not a Sandisk charge2(?) packet!\n");
evt_write_nand_unk(st, pkt);
evt_write_nand_unk(st, &second_pkt);
return 0;
}
packet_get_next(st, &third_pkt);
if (!nand_ale(third_pkt.data.nand_cycle.control)
|| nand_cle(third_pkt.data.nand_cycle.control)
|| !nand_we(third_pkt.data.nand_cycle.control)) {
fprintf(stderr, "Not a Sandisk charge2(?) packet!\n");
evt_write_nand_unk(st, pkt);
evt_write_nand_unk(st, &second_pkt);
evt_write_nand_unk(st, &third_pkt);
return 0;
}
packet_get_next(st, &fourth_pkt);
if (!nand_ale(fourth_pkt.data.nand_cycle.control)
|| nand_cle(fourth_pkt.data.nand_cycle.control)
|| !nand_we(fourth_pkt.data.nand_cycle.control)) {
fprintf(stderr, "Not a Sandisk charge2(?) packet!\n");
evt_write_nand_unk(st, pkt);
evt_write_nand_unk(st, &second_pkt);
evt_write_nand_unk(st, &third_pkt);
evt_write_nand_unk(st, &fourth_pkt);
return 0;
}
evt.addr[0] = second_pkt.data.nand_cycle.data;
evt.addr[1] = third_pkt.data.nand_cycle.data;
evt.addr[2] = fourth_pkt.data.nand_cycle.data;
evt_fill_end(&evt, fourth_pkt.header.sec, fourth_pkt.header.nsec);
write(st->out_fd, &evt, sizeof(evt));
return 0;
}
static int evt_write_nand_parameter_page(struct state *st, struct pkt *pkt) {
struct evt_nand_parameter_read evt;
struct pkt second_pkt;
evt_fill_header(&evt, pkt->header.sec, pkt->header.nsec,
sizeof(evt), EVT_NAND_PARAMETER_READ);
// Make sure the subsequent packet is a read of status
packet_get_next(st, &second_pkt);
if (!nand_ale(second_pkt.data.nand_cycle.control)
|| nand_cle(second_pkt.data.nand_cycle.control)
|| !nand_we(second_pkt.data.nand_cycle.control)) {
fprintf(stderr, "Not a NAND parameter read!\n");
evt_write_nand_unk(st, pkt);
evt_write_nand_unk(st, &second_pkt);
return 0;
}
evt.count = 0;
evt.addr = second_pkt.data.nand_cycle.data;
memset(evt.data, 0, sizeof(evt.data));
evt.count = 0;
evt_fill_end(&evt, second_pkt.header.sec, second_pkt.header.nsec);
packet_get_next(st, pkt);
while (nand_re(pkt->data.nand_cycle.control)) {
evt.data[evt.count++] = pkt->data.nand_cycle.data;
evt_fill_end(&evt, pkt->header.sec, pkt->header.nsec);
packet_get_next(st, pkt);
}
packet_unget(st, pkt);
write(st->out_fd, &evt, sizeof(evt));
return 0;
}
static int evt_write_nand_reset(struct state *st, struct pkt *pkt) {
struct evt_nand_reset evt;
struct pkt second_pkt;
evt_fill_header(&evt, pkt->header.sec, pkt->header.nsec,
sizeof(evt), EVT_NAND_RESET);
// Make sure the subsequent packet is 0xc5
packet_get_next(st, &second_pkt);
if (!nand_cle(second_pkt.data.nand_cycle.control)
|| second_pkt.data.nand_cycle.data != 0x00) {
fprintf(stderr, "Not a reset packet!\n");
evt_write_nand_unk(st, pkt);
evt_write_nand_unk(st, &second_pkt);
return 0;
}
evt_fill_end(&evt, second_pkt.header.sec, second_pkt.header.nsec);
write(st->out_fd, &evt, sizeof(evt));
return 0;
}
static int evt_write_nand_status(struct state *st, struct pkt *pkt) {
struct evt_nand_status evt;
struct pkt second_pkt;
evt_fill_header(&evt, pkt->header.sec, pkt->header.nsec,
sizeof(evt), EVT_NAND_STATUS);
// Make sure the subsequent packet is a read of status
packet_get_next(st, &second_pkt);
if (nand_ale(second_pkt.data.nand_cycle.control)
|| nand_cle(second_pkt.data.nand_cycle.control)
|| nand_we(second_pkt.data.nand_cycle.control)) {
fprintf(stderr, "Not a NAND status packet!\n");
evt_write_nand_unk(st, pkt);
evt_write_nand_unk(st, &second_pkt);
return 0;
}
evt.status = second_pkt.data.nand_cycle.data;
evt_fill_end(&evt, second_pkt.header.sec, second_pkt.header.nsec);
write(st->out_fd, &evt, sizeof(evt));
return 0;
}
// Searching for either a NAND block or a sync point
static int st_scanning(struct state *st) {
struct pkt pkt;
int ret;
while ((ret = packet_get_next(st, &pkt)) == 0) {
if (pkt.header.type == PACKET_HELLO) {
evt_write_hello(st, &pkt);
}
else if (pkt.header.type == PACKET_RESET) {
evt_write_reset(st, &pkt);
}
else if (pkt.header.type == PACKET_NAND_CYCLE) {
write_nand_cmd(st, &pkt);
}
else if (pkt.header.type == PACKET_COMMAND) {
if (pkt.data.command.start_stop == CMD_STOP) {
struct evt_net_cmd *net = evt_take(st, EVT_NET_CMD);
if (!net) {
struct evt_net_cmd evt;
fprintf(stderr, "NET_CMD end without begin\n");
evt_fill_header(&evt, pkt.header.sec, pkt.header.nsec,
sizeof(evt), EVT_NET_CMD);
evt.cmd[0] = pkt.data.command.cmd[0];
evt.cmd[1] = pkt.data.command.cmd[1];
evt.arg = pkt.data.command.arg;
evt_fill_end(&evt, pkt.header.sec, pkt.header.nsec);
evt.arg = htonl(evt.arg);
write(st->out_fd, &evt, sizeof(evt));
}
else {
evt_fill_end(net, pkt.header.sec, pkt.header.nsec);
net->arg = htonl(net->arg);
write(st->out_fd, net, sizeof(*net));
free(net);
}
}
else {
struct evt_net_cmd *net = evt_take(st, EVT_NET_CMD);
if (net) {
fprintf(stderr, "Multiple NET_CMDs going at once\n");
free(net);
}
net = malloc(sizeof(struct evt_net_cmd));
evt_fill_header(net, pkt.header.sec, pkt.header.nsec,
sizeof(*net), EVT_NET_CMD);
net->cmd[0] = pkt.data.command.cmd[0];
net->cmd[1] = pkt.data.command.cmd[1];
net->arg = pkt.data.command.arg;
evt_put(st, net);
}
}
else if (pkt.header.type == PACKET_BUFFER_DRAIN) {
if (pkt.data.buffer_drain.start_stop == PKT_BUFFER_DRAIN_STOP) {
struct evt_buffer_drain *evt = evt_take(st, EVT_BUFFER_DRAIN);
if (!evt) {
struct evt_buffer_drain evt;
fprintf(stderr, "BUFFER_DRAIN end without begin\n");
evt_fill_header(&evt, pkt.header.sec, pkt.header.nsec,
sizeof(evt), EVT_BUFFER_DRAIN);
evt_fill_end(&evt, pkt.header.sec, pkt.header.nsec);
write(st->out_fd, &evt, sizeof(evt));
}
else {
evt_fill_end(evt, pkt.header.sec, pkt.header.nsec);
write(st->out_fd, evt, sizeof(*evt));
free(evt);
}
}
else {
struct evt_buffer_drain *evt = evt_take(st, EVT_BUFFER_DRAIN);
if (evt) {
fprintf(stderr, "Multiple BUFFER_DRAINs going at once\n");
free(evt);
}
evt = malloc(sizeof(struct evt_buffer_drain));
evt_fill_header(evt, pkt.header.sec, pkt.header.nsec,
sizeof(*evt), EVT_BUFFER_DRAIN);
evt_put(st, evt);
}
}
else if (pkt.header.type == PACKET_SD_CMD_ARG) {
struct evt_sd_cmd *evt = evt_take(st, EVT_SD_CMD);
struct pkt_sd_cmd_arg *sd = &pkt.data.sd_cmd_arg;
if (!evt) {
evt = malloc(sizeof(struct evt_sd_cmd));
memset(evt, 0, sizeof(*evt));
evt_fill_header(evt, pkt.header.sec, pkt.header.nsec,
sizeof(*evt), EVT_SD_CMD);
}
// Ignore args for CMD55
if ((evt->num_args || sd->reg>0) && evt->cmd != 0x55) {
evt->args[evt->num_args++] = sd->val;
}
// Register 0 implies this is a CMD.
else if (sd->reg == 0) {
if (evt->cmd == 0x55)
evt->cmd = 0x80 | (0x3f & sd->val);
else
evt->cmd = 0x3f & sd->val;
}
evt_put(st, evt);
}
else if (pkt.header.type == PACKET_SD_RESPONSE) {
struct evt_sd_cmd *evt = evt_take(st, EVT_SD_CMD);
// Ignore CMD17, as we'll pick it up on the PACKET_SD_DATA packet
if (evt->cmd == 17) {
evt_put(st, evt);
}
else {
struct pkt_sd_response *sd = &pkt.data.response;
if (!evt) {
fprintf(stderr, "Couldn't find old EVT_SD_CMD in SD_RESPONSE\n");
continue;
}
evt->result[evt->num_results++] = sd->byte;
evt->num_results = htonl(evt->num_results);
evt->num_args = htonl(evt->num_args);
evt_fill_end(evt, pkt.header.sec, pkt.header.nsec);
write(st->out_fd, evt, sizeof(*evt));
free(evt);
}
}
else if (pkt.header.type == PACKET_SD_DATA) {
struct evt_sd_cmd *evt = evt_take(st, EVT_SD_CMD);
struct pkt_sd_data *sd = &pkt.data.sd_data;
int offset;
if (!evt) {
fprintf(stderr, "Couldn't find old SD_EVT_CMD in SD_DATA\n");
continue;
}
for (offset=0; offset<sizeof(sd->data); offset++)
evt->result[evt->num_results++] = sd->data[offset];
evt->num_results = htonl(evt->num_results);
evt->num_args = htonl(evt->num_args);
evt_fill_end(evt, pkt.header.sec, pkt.header.nsec);
write(st->out_fd, evt, sizeof(*evt));
free(evt);
}
else {
printf("Unknown packet type: %s\n", types[pkt.header.type]);
}
}
return ret;
}
static int evt_write_nand_read(struct state *st, struct pkt *pkt) {
struct evt_nand_read evt;
struct pkt pkts[6];
int counter;
evt_fill_header(&evt, pkt->header.sec, pkt->header.nsec,
sizeof(evt), EVT_NAND_READ);
for (counter=0; counter<5; counter++) {
// Make sure the subsequent packet is an address
packet_get_next(st, &pkts[counter]);
if (!nand_ale(pkts[counter].data.nand_cycle.control)
|| nand_cle(pkts[counter].data.nand_cycle.control)
|| !nand_we(pkts[counter].data.nand_cycle.control)) {
int countdown;
fprintf(stderr, "Not a nand_read packet (counter %d)\n", counter);
evt_write_nand_unk(st, pkt);
for (countdown=0; countdown<=counter; countdown++)
evt_write_nand_unk(st, &pkts[countdown]);
return 0;
}
}
// Next one should be a command, with type 0xe0
packet_get_next(st, &pkts[counter]);
if (nand_ale(pkts[counter].data.nand_cycle.control)
|| !nand_cle(pkts[counter].data.nand_cycle.control)
|| !nand_we(pkts[counter].data.nand_cycle.control)
|| pkts[counter].data.nand_cycle.data != 0x30) {
int countdown;
fprintf(stderr, "Not a nand_read packet (last packet wrong)\n");
evt_write_nand_unk(st, pkt);
for (countdown=0; countdown<=counter; countdown++)
evt_write_nand_unk(st, &pkts[countdown]);
return 0;
}
evt.addr[0] = pkts[0].data.nand_cycle.data;
evt.addr[1] = pkts[1].data.nand_cycle.data;
evt.addr[2] = pkts[2].data.nand_cycle.data;
evt.addr[3] = pkts[3].data.nand_cycle.data;
evt.addr[4] = pkts[4].data.nand_cycle.data;
evt.addr[5] = pkts[5].data.nand_cycle.data;
evt.count = 0;
evt_fill_end(&evt, pkts[6].header.sec, pkts[6].header.nsec);
memcpy(evt.unknown, &pkt->data.nand_cycle.unknown, sizeof(evt.unknown));
packet_get_next(st, pkt);
while (nand_re(pkt->data.nand_cycle.control)) {
evt.data[evt.count++] = pkt->data.nand_cycle.data;
evt_fill_end(&evt, pkt->header.sec, pkt->header.nsec);
memcpy(evt.unknown, &pkt->data.nand_cycle.unknown, sizeof(evt.unknown));
packet_get_next(st, pkt);
}
packet_unget(st, pkt);
evt.count = htonl(evt.count);
write(st->out_fd, &evt, sizeof(evt));
return 0;
}
/// 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;
}
}
}
