d7a_com.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "dbg.h"
00004 #include "d7a.h"
00005 #include "d7a_com.h"
00006 #include "d7a_alp.h"
00007 #include "d7a_common.h"
00008 #include "d7a_fs.h"
00009 #include "d7a_modem.h"
00010 #include "d7a_sys.h"
00011 #include "d7a_typedefs.h"
00012 #include "cbuffer.h"
00013 
00014 #if 1
00015     #define COM_DPRINT(...)         DPRINT(__VA_ARGS__)
00016     #define COM_DPRINT_DATA(...)    DPRINT_DATA(__VA_ARGS__)
00017     #define COM_FPRINT(...)         FPRINT(__VA_ARGS__)
00018 #else
00019     #define COM_DPRINT(...);
00020     #define COM_DPRINT_DATA(...);
00021     #define COM_FPRINT(...);
00022 #endif
00023 
00024 #define XON_SIGNAL              (0x0001 << 0)
00025 #define START_SIGNAL            (0x0001 << 1)
00026 
00027 typedef struct {
00028     uint32_t len;
00029     uint8_t buf[1];
00030 } d7a_com_tx_buf_t;
00031 
00032 static uint8_t                          g_com_state;
00033 static d7a_com_rx_msg_t                 g_com_msg;
00034 static uint16_t                         g_com_skipped_bytes;
00035 static uint8_t                          g_com_tx_seq;
00036 static uint8_t                          g_com_rx_seq;
00037 
00038 static MBED_DigitalOut*                 g_com_rts;
00039 static MBED_InterruptIn*                g_com_cts;
00040 static MBED_RawSerial*                  g_com_serial;
00041 static MBED_Timer                       g_com_tim;
00042 
00043 static UTILS_CBuffer<uint8_t, 512>      g_com_rx_buf;
00044 
00045 static OS_Semaphore                     g_com_data_parsing(0);
00046 static OS_Semaphore                     g_com_cts_int(0);
00047 static OS_Thread                        g_com_rx_thread(osPriorityRealtime, 512, NULL);
00048 static OS_Thread                        g_com_tx_thread(osPriorityHigh, 512, NULL);
00049 static OS_Queue<d7a_com_tx_buf_t, 8>    g_com_tx_queue;
00050 
00051 enum {
00052     SEARCH_HEADER,
00053     PARSE_HEADER,
00054     SEARCH_BODY,
00055     PARSE_BODY,
00056 };
00057 
00058 
00059 /**
00060     OS_Thread   for parsing packets from RX buffer.
00061 
00062     @param void
00063     @return void
00064 */
00065 void d7a_com_rx_thread();
00066 void d7a_com_tx_thread();
00067 
00068 
00069 /**
00070     Serial Rx Interrupt Service Routine.
00071     Add recevied bytes to the RX buffer.
00072 
00073     @param void
00074     @return void
00075 */
00076 void rx_isr()
00077 {
00078 // Loop just in case more than one character is in UART's receive FIFO buffer
00079     while (g_com_serial->readable())
00080     {
00081         g_com_rx_buf.push(g_com_serial->getc());
00082         //PRINT("-");
00083     }
00084     
00085     // unlock data parsing thread
00086     if (g_com_state == SEARCH_HEADER && g_com_rx_buf.available_data() >= KAL_COM_HEADER_LEN)
00087     {
00088         g_com_state = PARSE_HEADER;
00089         g_com_data_parsing.release();
00090     }
00091     else if (g_com_state == SEARCH_BODY && g_com_rx_buf.available_data() >= g_com_msg.blen)
00092     {
00093         g_com_state = PARSE_BODY;
00094         g_com_data_parsing.release();
00095     }
00096 }
00097 
00098 /**
00099     CTS pin Interrupt Service Routine.
00100     For flow control (not yet inplemented)
00101 
00102     @param void
00103     @return void
00104 */
00105 void cts_isr()
00106 {
00107     //PRINT("CTS_INT\r\n");
00108     //g_com_cts_int->release();
00109 }
00110 
00111 // D7a_com constructor.
00112 // Opens a serial port and monitors the input for ALP packets.
00113 // Pins are those of the host, not the modem:
00114 // TX-host -> RX-modem
00115 // RX-host <- TX-modem
00116 // RTS-host -> CTS-modem
00117 // CTS-host <- RTS-modem
00118 d7a_errors_t d7a_com_open( const d7a_com_config_t* config )
00119 {
00120     COM_FPRINT("\r\n");
00121     
00122     g_com_state = SEARCH_HEADER;
00123     g_com_skipped_bytes = 0;
00124     g_com_tx_seq = 0;
00125     g_com_rx_seq = 0;
00126     
00127     g_com_serial =          new MBED_RawSerial(config->tx, config->rx, 115200);
00128     g_com_rts =             new MBED_DigitalOut(config->rts);
00129     g_com_cts =             new MBED_InterruptIn(config->cts);
00130     
00131     g_com_cts->rise(&cts_isr);
00132     
00133     g_com_serial->format(8, SerialBase::None, 1);
00134     g_com_serial->attach(&rx_isr, Serial::RxIrq);
00135     
00136     osStatus err = g_com_rx_thread.start(d7a_com_rx_thread);
00137     ASSERT(err == osOK, "Failed to start d7a_com_rx_thread (err: %d)\r\n", err);
00138     
00139     err = g_com_tx_thread.start(d7a_com_tx_thread);
00140     ASSERT(err == osOK, "Failed to start d7a_com_tx_thread (err: %d)\r\n", err);
00141     
00142     return D7A_ERR_NONE;
00143 }
00144 
00145 // Destructor
00146 d7a_errors_t d7a_com_close(void)
00147 {
00148     COM_FPRINT("\r\n");
00149     g_com_rx_thread.terminate();
00150     g_com_tx_thread.terminate();
00151     delete g_com_serial;
00152     delete g_com_rts;
00153     delete g_com_cts;
00154     
00155     return D7A_ERR_NONE;
00156 }
00157 
00158 
00159 void d7a_com_restart(void)
00160 {
00161     COM_FPRINT("\r\n");
00162     
00163     g_com_serial->attach(NULL, Serial::RxIrq);
00164 
00165     g_com_state = SEARCH_HEADER;
00166     g_com_skipped_bytes = 0;
00167     g_com_tx_seq = 0;
00168     g_com_rx_seq = 0;
00169     
00170     g_com_rx_buf.reset();
00171     
00172     g_com_serial->attach(&rx_isr, Serial::RxIrq);
00173 }
00174 
00175 
00176 /**
00177     Wakes-up modem and send data throught Serial.
00178 
00179     @param const uint8_t*       Pointer to data buffer
00180     @param int                  Data length
00181     @return void
00182 */
00183 static void d7a_com_send(d7a_com_tx_buf_t* tx_buf)
00184 {
00185     COM_FPRINT("\r\n");
00186     
00187     COM_DPRINT("<-- (0x%02X) %d\r\n", tx_buf->buf[4], (tx_buf->len - KAL_COM_HEADER_LEN));
00188     
00189     *(g_com_rts) = 1;
00190     
00191     //dbg_print_data("", "%02X ", (uint8_t*)tx_buf->buf, tx_buf->len, "\r\n");
00192     
00193     OS_Thread  ::wait(3);
00194     
00195     for (uint32_t i=0 ; i<tx_buf->len ; i++)
00196     {
00197         g_com_serial->putc(tx_buf->buf[i]);
00198     }
00199    
00200     // Important to not release the ressource too soon
00201     OS_Thread  ::wait(2);
00202     
00203     *(g_com_rts) = 0;
00204 }
00205 
00206 // Formats and send packet throught Serial.
00207 d7a_com_tx_buf_t* d7a_com_new_msg(d7a_com_tx_msg_t* msg)
00208 {    
00209     uint8_t len = KAL_COM_HEADER_LEN + msg->alen + msg->plen;
00210     COM_FPRINT("(len:%d)\r\n", len);
00211     
00212     d7a_com_tx_buf_t* tx_buf = (d7a_com_tx_buf_t*)MALLOC(sizeof(d7a_com_tx_buf_t) - 1 + len);
00213     
00214     // construct serial header
00215     // concatenate and update tx_seq ID
00216     uint8_t* p = tx_buf->buf;
00217     uint8_t* t = p;
00218     *p++ = (uint8_t)KAL_COM_SYNC_BYTE_0;
00219     *p++ = (uint8_t)KAL_COM_SYNC_BYTE_1;
00220     *p++ = (uint8_t)msg->alen + msg->plen;
00221     *p++ = (uint8_t)g_com_tx_seq++;
00222     *p++ = (uint8_t)msg->id;
00223 
00224     // copy payload and parameters
00225     memcpy(p, msg->pbuf, msg->plen);
00226     p += msg->plen;
00227     memcpy(p, msg->abuf, msg->alen);
00228     p += msg->alen;
00229     
00230     tx_buf->len = (uint32_t)(p - t);
00231     
00232     ASSERT(tx_buf->len == len, "New msg wrong length %d expected %d\r\n", tx_buf->len, len);
00233     
00234     return tx_buf;
00235 }
00236 
00237 void d7a_com_post_msg(d7a_com_tx_msg_t* msg)
00238 {
00239     COM_FPRINT("\r\n");
00240     
00241     g_com_tx_queue.put(d7a_com_new_msg(msg));
00242 }
00243 
00244 void d7a_com_dump(uint8_t* buf, uint8_t len, d7a_com_flow_t flow)
00245 {
00246     d7a_com_tx_msg_t msg;
00247 
00248     msg.id = flow;
00249     msg.pbuf = buf;
00250     msg.plen = len;
00251     msg.alen = 0;
00252     d7a_com_post_msg(&msg);
00253 }
00254 
00255 static void d7a_com_new_pkt(d7a_com_rx_msg_t* pkt)
00256 {
00257     //COM_COM_FPRINT("\r\n");
00258 
00259     COM_DPRINT("--> (0x%02X) %d\r\n", pkt->id, pkt->blen);
00260 
00261     // Distribute packet types to processes
00262     switch (KAL_COM_FLOWID(pkt->id))
00263     {
00264         case KAL_COM_FLOWID_FS:
00265             d7a_fs_new_pkt(pkt);
00266             break;
00267         case KAL_COM_FLOWID_CMD:
00268             d7a_modem_new_pkt(pkt);
00269             break;
00270         case KAL_COM_FLOWID_ALP:
00271             d7a_alp_new_pkt(pkt);
00272             break;
00273         case KAL_COM_FLOWID_SYS:
00274             // This has to be here to avoid going to another process
00275             if (pkt->id == KAL_COM_FLOW_SYS_XON)
00276             {
00277                 FREE(pkt);
00278                 COM_DPRINT("XON\r\n");
00279                 g_com_tx_thread.signal_set(XON_SIGNAL);
00280             }
00281             else if (pkt->id == KAL_COM_FLOW_SYS_XOFF)
00282             {
00283                 FREE(pkt);
00284                 COM_DPRINT("XOFF\r\n");
00285                 d7a_sys_xack();
00286             }
00287             else
00288             {
00289                 d7a_sys_new_pkt(pkt);
00290             }
00291             break;
00292         default:
00293             EPRINT("Untreated pkt type 0x%02X\r\n", pkt->id);
00294             FREE(pkt);
00295             break;
00296     }
00297 }
00298 
00299 
00300 /**
00301     Reads the Rx buffer, parses the packets
00302 
00303     @param void
00304     @return void
00305 */
00306 static void parse_packet_header(void)
00307 {
00308     COM_FPRINT("\r\n");
00309     
00310     uint8_t header[KAL_COM_HEADER_LEN];
00311     uint8_t seqnum;
00312     
00313     ASSERT(g_com_rx_buf.available_data() >= KAL_COM_HEADER_LEN, "Not enough data for header\r\n");
00314     
00315     g_com_skipped_bytes = 0;
00316     
00317     header[0] = g_com_rx_buf.pop();
00318     
00319     while (g_com_rx_buf.available_data() >= KAL_COM_HEADER_LEN - 1)
00320     {
00321         header[1] = g_com_rx_buf.pop();
00322         
00323         // Check sync bytes
00324         if(KAL_COM_SYNC_BYTE_0 == header[0] && KAL_COM_SYNC_BYTE_1 == header[1])
00325         {
00326             // Copy header
00327             g_com_rx_buf.get(&header[2], KAL_COM_HEADER_LEN - 2);
00328             
00329             // Fill temp header
00330             g_com_msg.blen = header[2];
00331             seqnum = header[3];
00332             g_com_msg.id = header[4];
00333             
00334             // Update seqnum
00335             WARNING(g_com_rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d\r\n", g_com_rx_seq, seqnum);
00336             g_com_rx_seq = seqnum + 1;
00337             
00338             // search for body
00339             g_com_state = SEARCH_BODY;
00340             
00341             // Start parsing if data is already available
00342             if (g_com_rx_buf.available_data() >= g_com_msg.blen)
00343             {
00344                 g_com_state = PARSE_BODY;
00345                 g_com_data_parsing.release();
00346             }
00347             
00348             //COM_DPRINT("COM header found (id: %02X seq: %d body: %d/%d bytes)\r\n", g_com_msg.id, seqnum, g_com_rx_buf.available_data(), g_com_msg.blen);
00349             break;
00350         }
00351         else
00352         {
00353             // Shift by 1 byte
00354             //WARNING(false, "COM Skipped byte 0x%02X.\r\n", header[0]);
00355             g_com_skipped_bytes++;
00356             header[0] = header[1];
00357         }
00358     }
00359     
00360     WARNING(!g_com_skipped_bytes, "COM Skipped %d bytes.\r\n", g_com_skipped_bytes);
00361 }
00362 
00363 /**
00364     Reads the Rx buffer, parses the packets
00365 
00366     @param void
00367     @return void
00368 */
00369 static void parse_packet_body(void)
00370 {
00371     ASSERT(g_com_rx_buf.available_data() >= g_com_msg.blen, "Not enough data for body\r\n");
00372     
00373     if (KAL_COM_FLOWID(g_com_msg.id) != KAL_COM_FLOWID_TRC)
00374     {
00375         //COM_DPRINT("COM body found (%d bytes)\r\n", g_com_msg.blen);
00376         
00377         d7a_com_rx_msg_t* pkt = (d7a_com_rx_msg_t*)MALLOC(sizeof(d7a_com_rx_msg_t) - 1 + g_com_msg.blen);
00378 
00379         // copy data to buffer
00380         pkt->blen = g_com_msg.blen;
00381         pkt->id = g_com_msg.id;
00382         if (g_com_msg.blen)
00383         {
00384             g_com_rx_buf.get(pkt->buffer, g_com_msg.blen);
00385         }
00386 
00387         // add packet to queue
00388         d7a_com_new_pkt(pkt);
00389     }
00390     else
00391     {
00392         // Ignore packet
00393         //COM_DPRINT("Ignore pkt id %02X\r\n", g_com_msg.id);
00394         if (g_com_msg.blen)
00395         {
00396             g_com_rx_buf.get(NULL, g_com_msg.blen);
00397         }
00398     }
00399     
00400     // Seach for next header
00401     g_com_state = SEARCH_HEADER;
00402     
00403     // Start parsing if data is already available
00404     if (g_com_rx_buf.available_data() >= KAL_COM_HEADER_LEN)
00405     {
00406         g_com_state = PARSE_HEADER;
00407         g_com_data_parsing.release();
00408     }
00409 }
00410 
00411 
00412 // OS_Thread   for parsing packets from RX buffer.
00413 void d7a_com_rx_thread()
00414 {
00415     COM_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
00416     while (true)
00417     {
00418         // wait for data available
00419         g_com_data_parsing.wait();
00420         
00421         if (g_com_state == PARSE_HEADER)
00422         {
00423             parse_packet_header();
00424         }
00425         else if (g_com_state == PARSE_BODY)
00426         {
00427             parse_packet_body();
00428         }
00429     }
00430 }
00431 
00432 void d7a_com_tx_thread()
00433 {
00434     COM_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
00435     
00436     d7a_com_tx_buf_t* msg;
00437     osEvent evt;
00438     uint8_t flow_id;
00439     
00440     while (true)
00441     {
00442         // wait for data to send
00443         evt = g_com_tx_queue.get();
00444         msg = (evt.status == osEventMessage)? (d7a_com_tx_buf_t*)evt.value.p : NULL;
00445 
00446         
00447         // send message
00448         if (msg != NULL)
00449         {
00450             flow_id = msg->buf[4];
00451 
00452             d7a_com_send(msg);
00453             FREE(msg);
00454             
00455             if (KAL_COM_FLOW_SYS_XACK == flow_id)
00456             {
00457                 COM_DPRINT("XACK\r\n");
00458                 g_com_tx_thread.signal_wait(XON_SIGNAL);
00459             }
00460         }
00461     }
00462 }