altos: Shrink ao_add_task by rolling up a memset loop

[fw/altos] / src / ao_radio.c

diff --git a/src/ao_radio.c b/src/ao_radio.c
index 362b73aa3053c67452ace38ce15fa879aed4d0bb..4c382bb9685b1f66045678066553e0d7e7378ffe 100644 (file)
--- a/src/ao_radio.c
+++ b/src/ao_radio.c
@@ -161,8 +161,6 @@ static __code uint8_t radio_setup[] = {
        RF_SYNC0_OFF,           0x91,
 
        /* max packet length */
-       RF_PKTLEN_OFF,          sizeof (struct ao_telemetry),
-
        RF_PKTCTRL1_OFF,        ((1 << PKTCTRL1_PQT_SHIFT)|
                                 PKTCTRL1_APPEND_STATUS|
                                 PKTCTRL1_ADR_CHK_NONE),
@@ -217,33 +215,7 @@ static __code uint8_t rdf_setup[] = {
                                 RF_PKTCTRL0_LENGTH_CONFIG_FIXED),
 };
 
-static __code uint8_t telemetry_setup[] = {
-       RF_MDMCFG4_OFF,         ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) |
-                                (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) |
-                                (DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)),
-       RF_MDMCFG3_OFF,         (DRATE_M << RF_MDMCFG3_DRATE_M_SHIFT),
-       RF_MDMCFG2_OFF,         (RF_MDMCFG2_DEM_DCFILT_OFF |
-                                RF_MDMCFG2_MOD_FORMAT_GFSK |
-                                RF_MDMCFG2_SYNC_MODE_15_16_THRES),
-       RF_MDMCFG1_OFF,         (RF_MDMCFG1_FEC_EN |
-                                RF_MDMCFG1_NUM_PREAMBLE_4 |
-                                (2 << RF_MDMCFG1_CHANSPC_E_SHIFT)),
-
-       RF_DEVIATN_OFF,         ((DEVIATION_E << RF_DEVIATN_DEVIATION_E_SHIFT) |
-                                (DEVIATION_M << RF_DEVIATN_DEVIATION_M_SHIFT)),
-
-       /* max packet length */
-       RF_PKTLEN_OFF,          sizeof (struct ao_telemetry),
-       RF_PKTCTRL1_OFF,        ((1 << PKTCTRL1_PQT_SHIFT)|
-                                PKTCTRL1_APPEND_STATUS|
-                                PKTCTRL1_ADR_CHK_NONE),
-       RF_PKTCTRL0_OFF,        (RF_PKTCTRL0_WHITE_DATA|
-                                RF_PKTCTRL0_PKT_FORMAT_NORMAL|
-                                RF_PKTCTRL0_CRC_EN|
-                                RF_PKTCTRL0_LENGTH_CONFIG_FIXED),
-};
-
-static __code uint8_t packet_setup[] = {
+static __code uint8_t fixed_pkt_setup[] = {
        RF_MDMCFG4_OFF,         ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) |
                                 (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) |
                                 (DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)),
@@ -258,8 +230,7 @@ static __code uint8_t packet_setup[] = {
        RF_DEVIATN_OFF,         ((DEVIATION_E << RF_DEVIATN_DEVIATION_E_SHIFT) |
                                 (DEVIATION_M << RF_DEVIATN_DEVIATION_M_SHIFT)),
 
-       /* max packet length */
-       RF_PKTLEN_OFF,          sizeof (struct ao_packet),
+       /* max packet length -- now set inline */
        RF_PKTCTRL1_OFF,        ((1 << PKTCTRL1_PQT_SHIFT)|
                                 PKTCTRL1_APPEND_STATUS|
                                 PKTCTRL1_ADR_CHK_NONE),
@@ -289,28 +260,12 @@ ao_radio_general_isr(void) __interrupt 16
        }
 }
 
-void
-ao_radio_set_telemetry(void)
-{
-       uint8_t i;
-       for (i = 0; i < sizeof (telemetry_setup); i += 2)
-               RF[telemetry_setup[i]] = telemetry_setup[i+1];
-}
-
 void
 ao_radio_set_packet(void)
 {
        uint8_t i;
-       for (i = 0; i < sizeof (packet_setup); i += 2)
-               RF[packet_setup[i]] = packet_setup[i+1];
-}
-
-void
-ao_radio_set_rdf(void)
-{
-       uint8_t i;
-       for (i = 0; i < sizeof (rdf_setup); i += 2)
-               RF[rdf_setup[i]] = rdf_setup[i+1];
+       for (i = 0; i < sizeof (fixed_pkt_setup); i += 2)
+               RF[fixed_pkt_setup[i]] = fixed_pkt_setup[i+1];
 }
 
 void
@@ -326,7 +281,7 @@ ao_radio_idle(void)
 }
 
 void
-ao_radio_get(void)
+ao_radio_get(uint8_t len)
 {
        ao_config_get();
        ao_mutex_get(&ao_radio_mutex);
@@ -335,13 +290,14 @@ ao_radio_get(void)
        RF_FREQ2 = (uint8_t) (ao_config.radio_cal >> 16);
        RF_FREQ1 = (uint8_t) (ao_config.radio_cal >> 8);
        RF_FREQ0 = (uint8_t) (ao_config.radio_cal);
+       RF_PKTLEN = len;
 }
 
 
 void
 ao_radio_send(__xdata void *packet, uint8_t size) __reentrant
 {
-       ao_radio_get();
+       ao_radio_get(size);
        ao_radio_done = 0;
        ao_dma_set_transfer(ao_radio_dma,
                            packet,
@@ -364,7 +320,7 @@ uint8_t
 ao_radio_recv(__xdata void *packet, uint8_t size) __reentrant
 {
        ao_radio_abort = 0;
-       ao_radio_get();
+       ao_radio_get(size - 2);
        ao_dma_set_transfer(ao_radio_dma,
                            &RFDXADDR,
                            packet,
@@ -377,8 +333,13 @@ ao_radio_recv(__xdata void *packet, uint8_t size) __reentrant
                            DMA_CFG1_PRIORITY_HIGH);
        ao_dma_start(ao_radio_dma);
        RFST = RFST_SRX;
+
+       /* Wait for DMA to be done, for the radio receive process to
+        * get aborted or for a receive timeout to fire
+        */
        __critical while (!ao_radio_dma_done && !ao_radio_abort)
-                          ao_sleep(&ao_radio_dma_done);
+                          if (ao_sleep(&ao_radio_dma_done))
+                                  break;
 
        /* If recv was aborted, clean up by stopping the DMA engine
         * and idling the radio
@@ -411,12 +372,6 @@ ao_radio_rdf(int ms)
        uint8_t i;
        uint8_t pkt_len;
 
-       ao_radio_abort = 0;
-       ao_radio_get();
-       ao_radio_done = 0;
-       for (i = 0; i < sizeof (rdf_setup); i += 2)
-               RF[rdf_setup[i]] = rdf_setup[i+1];
-
        /*
         * Compute the packet length as follows:
         *
@@ -427,7 +382,12 @@ ao_radio_rdf(int ms)
        if (ms > (255 * 4))
                ms = 255 * 4;
        pkt_len = ms >> 2;
-       RF[RF_PKTLEN_OFF] = pkt_len;
+
+       ao_radio_abort = 0;
+       ao_radio_get(pkt_len);
+       ao_radio_done = 0;
+       for (i = 0; i < sizeof (rdf_setup); i += 2)
+               RF[rdf_setup[i]] = rdf_setup[i+1];
 
        ao_dma_set_transfer(ao_radio_dma,
                            &ao_radio_rdf_value,
@@ -447,8 +407,7 @@ ao_radio_rdf(int ms)
                ao_dma_abort(ao_radio_dma);
                ao_radio_idle();
        }
-       for (i = 0; i < sizeof (telemetry_setup); i += 2)
-               RF[telemetry_setup[i]] = telemetry_setup[i+1];
+       ao_radio_set_packet();
        ao_radio_put();
 }
 
@@ -474,8 +433,10 @@ ao_radio_test(void)
        mode++;
        if ((mode & 2) && !radio_on) {
                ao_set_monitor(0);
+#if PACKET_HAS_SLAVE
                ao_packet_slave_stop();
-               ao_radio_get();
+#endif
+               ao_radio_get(0xff);
                RFST = RFST_STX;
                radio_on = 1;
        }
@@ -492,8 +453,8 @@ ao_radio_test(void)
 }
 
 __code struct ao_cmds ao_radio_cmds[] = {
-       { 'C',  ao_radio_test,  "C <1 start, 0 stop, none both>     Radio carrier test" },
-       { 0,    ao_radio_test,  NULL },
+       { ao_radio_test,        "C <1 start, 0 stop, none both>\0Radio carrier test" },
+       { 0,    NULL },
 };
 
 void
@@ -502,7 +463,7 @@ ao_radio_init(void)
        uint8_t i;
        for (i = 0; i < sizeof (radio_setup); i += 2)
                RF[radio_setup[i]] = radio_setup[i+1];
-       ao_radio_set_telemetry();
+       ao_radio_set_packet();
        ao_radio_dma_done = 1;
        ao_radio_dma = ao_dma_alloc(&ao_radio_dma_done);
        RFIF = 0;