WizziCom.cpp

00001 #include "WizziCom.h"
00002 
00003 #if 0
00004     #define COM_DPRINT(...)         PRINT(__VA_ARGS__)
00005     #define COM_DPRINT_DATA(...)    PRINT_DATA(__VA_ARGS__)
00006     #define COM_FPRINT(...)         FPRINT(__VA_ARGS__)
00007 #else
00008     #define COM_DPRINT(...);
00009     #define COM_DPRINT_DATA(...);
00010     #define COM_FPRINT(...);
00011 #endif
00012 
00013 #define XON_SIGNAL              (0x0001 << 0)
00014 #define START_SIGNAL            (0x0001 << 1)
00015 
00016 // +--------------+--------+--------+--------+---- - - - - - - - - - - --------+
00017 // |     SYNC     |   LEN  |   SEQ  |   ID   |             PAYLOAD             |
00018 // +--------------+--------+--------+--------+---- - - - - - - - - - - --------+
00019 //
00020 //      2 bytes     1 byte   1 byte   1 byte              LEN bytes
00021 // |<------------>|<------>|<------>|<------>|<--- - - - - - - - - - - ------->|
00022 
00023 
00024 // first byte of the sync word
00025 // (ASCII start of heading)
00026 #define KAL_COM_SYNC_BYTE_0          0x01
00027 
00028 // second byte of the sync word
00029 // (ASCII group separator)
00030 #define KAL_COM_SYNC_BYTE_1          0x1F
00031 
00032 
00033 //======================================================================
00034 // wizzi_com_fid_t
00035 //----------------------------------------------------------------------
00036 // Enumerator of serial Flow-ids
00037 //======================================================================
00038 typedef enum
00039 {
00040     // Trace channel
00041     KAL_COM_FLOWID_TRC = 0,
00042     // General purpose Command channel
00043     KAL_COM_FLOWID_CMD,
00044     // ALP channel
00045     KAL_COM_FLOWID_ALP,
00046     // System notifications
00047     KAL_COM_FLOWID_SYS,
00048     // File System channel
00049     KAL_COM_FLOWID_FS,
00050 
00051     KAL_COM_FLOWID_QTY
00052 
00053 } wizzi_com_fid_t;
00054 
00055 #define KAL_COM_FLOW(fid,type)      ((((fid)&0x7)<<4) | ((type)&0xF))
00056 #define KAL_COM_FLOWID(id)          (((id)>>4)&0x7)
00057 #define KAL_COM_FLOWID_REDIRECT     0x80
00058 
00059 //======================================================================
00060 // wizzi_com_flow_t
00061 //----------------------------------------------------------------------
00062 // Enumerator of serial flows
00063 //======================================================================
00064 typedef enum
00065 {
00066     // Default printf type
00067     KAL_COM_FLOW_PRINTF                     = KAL_COM_FLOW(KAL_COM_FLOWID_TRC,0),
00068     // Substitute the string by a codeword
00069     // interpreted by the PC com tool
00070     KAL_COM_FLOW_PRINTF_COMPRESSED          = KAL_COM_FLOW(KAL_COM_FLOWID_TRC,1),
00071     // Display the payload as hex data
00072     KAL_COM_FLOW_PRINT_HEX                  = KAL_COM_FLOW(KAL_COM_FLOWID_TRC,2),
00073 
00074     // AT command
00075     KAL_COM_FLOW_AT_CMD                     = KAL_COM_FLOW(KAL_COM_FLOWID_ALP,0),
00076     // AT command response
00077     KAL_COM_FLOW_AT_RESP                    = KAL_COM_FLOW(KAL_COM_FLOWID_ALP,1),
00078     // AT unsolicited message
00079     KAL_COM_FLOW_AT_UNS                     = KAL_COM_FLOW(KAL_COM_FLOWID_ALP,2),
00080     // AT unsolicited error
00081     KAL_COM_FLOW_AT_ERR                     = KAL_COM_FLOW(KAL_COM_FLOWID_ALP,3),
00082 
00083     // Remote Commands
00084     KAL_COM_FLOW_CMD                        = KAL_COM_FLOW(KAL_COM_FLOWID_CMD,0),
00085 
00086     // Remote System reset
00087     KAL_COM_FLOW_SYS_RST                    = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,0),
00088     // Button Emulator
00089     KAL_COM_FLOW_SYS_BUTTON                 = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,1),
00090     // Dump Debug parameters
00091     KAL_COM_FLOW_SYS_INFO                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,2),
00092     // CUP signalisation
00093     KAL_COM_FLOW_SYS_CUP                    = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,3),
00094     // Ping distant COM
00095     KAL_COM_FLOW_SYS_PING                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,4),
00096     // Pong from distant COM
00097     KAL_COM_FLOW_SYS_PONG                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,5),
00098     // Enable system config from distant COM
00099     KAL_COM_FLOW_SYS_CFG                    = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,6),
00100     // Configure Output Trace level from distant COM
00101     KAL_COM_FLOW_SYS_TLEV                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,7),
00102     // Configure COM port redirection
00103     KAL_COM_FLOW_SYS_REDIR                  = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,8),
00104     // Flow control signalling
00105     KAL_COM_FLOW_SYS_XON                    = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,9),
00106     KAL_COM_FLOW_SYS_XOFF                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,10),
00107     KAL_COM_FLOW_SYS_XACK                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,11),
00108 
00109     // File System Command/Response
00110     KAL_COM_FLOW_FS_CMD                     = KAL_COM_FLOW(KAL_COM_FLOWID_FS,0),
00111     KAL_COM_FLOW_FS_RESP                    = KAL_COM_FLOW(KAL_COM_FLOWID_FS,1),
00112     
00113 } wizzi_com_flow_t;
00114 
00115 
00116 
00117 
00118 enum {
00119     SEARCH_HEADER,
00120     SEARCH_BODY,
00121 };
00122 
00123 void wizzi_com_rx_thread();
00124 void wizzi_com_tx_thread();
00125 
00126 const uint8_t g_type_to_flow[WIZZICOM_PKT_QTY] = {
00127     // Default printf type
00128     KAL_COM_FLOW_PRINTF,
00129     // Substitute the string by a codeword
00130     // interpreted by the PC com tool
00131     KAL_COM_FLOW_PRINTF_COMPRESSED,
00132     // Display the payload as hex data
00133     KAL_COM_FLOW_PRINT_HEX,
00134 
00135     // AT command
00136     KAL_COM_FLOW_AT_CMD,
00137     // AT command response
00138     KAL_COM_FLOW_AT_RESP,
00139     // AT unsolicited message
00140     KAL_COM_FLOW_AT_UNS,
00141     // AT unsolicited error
00142     KAL_COM_FLOW_AT_ERR,
00143 
00144     // Remote Commands
00145     KAL_COM_FLOW_CMD,
00146     
00147     // Remote System reset
00148     KAL_COM_FLOW_SYS_RST,
00149     // Button Emulator
00150     KAL_COM_FLOW_SYS_BUTTON,
00151     // Dump Debug parameters
00152     KAL_COM_FLOW_SYS_INFO,
00153     // CUP signalisation
00154     KAL_COM_FLOW_SYS_CUP,
00155     // Ping distant COM
00156     KAL_COM_FLOW_SYS_PING,
00157     // Pong from distant COM
00158     KAL_COM_FLOW_SYS_PONG,
00159     // Enable system config from distant COM
00160     KAL_COM_FLOW_SYS_CFG,
00161     // Configure Output Trace level from distant COM
00162     KAL_COM_FLOW_SYS_TLEV,
00163     // Configure COM port redirection
00164     KAL_COM_FLOW_SYS_REDIR,
00165     // Flow control signalling
00166     KAL_COM_FLOW_SYS_XON,
00167     KAL_COM_FLOW_SYS_XOFF,
00168     KAL_COM_FLOW_SYS_XACK,
00169 
00170     // File System Command/Response
00171     KAL_COM_FLOW_FS_CMD,
00172     KAL_COM_FLOW_FS_RESP
00173 };
00174 
00175 uint8_t wizzicom_type_to_flow(uint8_t packet_type)
00176 {
00177     return g_type_to_flow[packet_type];
00178 }
00179 
00180 uint8_t wizzicom_flow_to_type(uint8_t flow_id)
00181 {
00182     // Get packet type from flow_id
00183     uint8_t packet_type = 0;
00184     while ((g_type_to_flow[packet_type] != flow_id) && (packet_type < WIZZICOM_PKT_QTY))
00185     {
00186         packet_type++;
00187     }
00188     return packet_type;
00189 }
00190 
00191 WizziCom::WizziCom(PinName tx, PinName rx, PinName irq_out, PinName irq_in) : 
00192 _tx_done(0),
00193 _rx_done(0),
00194 _irq_in_int(0),
00195 _rx_thread(osPriorityHigh, 512, NULL, "wzc_rx"),
00196 _tx_thread(osPriorityHigh, 512, NULL, "wzc_tx"),
00197 _cb_thread(osPriorityHigh, 1024, NULL, "wzc_cb")
00198 {
00199     _tx_seq = 0;
00200     _rx_seq = 0;
00201     
00202     memset(_callback, 0, sizeof(_callback));
00203     
00204     _cbuf = &_cbuf_h;
00205     kal_buf_circ_create_static(&_cbuf_h, _cbuf_b, RX_BUF_SIZE, sizeof(uint8_t));
00206     
00207     _serial = new RawSerial(tx, rx, 115200);
00208     _serial->format(8, SerialBase::None, 1);
00209     //_serial->attach(callback(this, &WizziCom::_rx_isr), Serial::RxIrq);
00210     _serial->read(&_rx_byte, sizeof(uint8_t), callback(this, &WizziCom::_rx_done_isr));
00211     _serial->set_flow_control(SerialBase::Disabled);
00212     
00213     _irq_out = (irq_out != NC)? new DigitalOut(irq_out) : NULL;
00214     
00215     if (irq_in != NC)
00216     {
00217         _irq_in = new InterruptIn(irq_in);
00218         _irq_in->rise(callback(this, &WizziCom::_irq_in_isr));
00219     }
00220     else
00221     {
00222         _irq_in = NULL;
00223     }
00224         
00225     osStatus err = _rx_thread.start(callback(this, &WizziCom::_thread_rx));
00226     ASSERT(err == osOK, "Failed to start WizziCom _thread_rx (err: %d)\r\n", err);
00227     
00228     err = _tx_thread.start(callback(this, &WizziCom::_thread_tx));
00229     ASSERT(err == osOK, "Failed to start WizziCom _thread_tx (err: %d)\r\n", err);
00230     
00231     err = _cb_thread.start(callback(this, &WizziCom::_thread_callback));
00232     ASSERT(err == osOK, "Failed to start WizziCom _thread_callback (err: %d)\r\n", err);
00233 }
00234 
00235 WizziCom::~WizziCom()
00236 {  
00237     _rx_thread.terminate();
00238     _tx_thread.terminate();
00239     _cb_thread.terminate();
00240     delete _serial;
00241     delete _irq_out;
00242     delete _irq_in;
00243 }
00244 
00245 void WizziCom::attach(WizziComCallback function, WizziComPacketType packet_type)
00246 {
00247     _callback[packet_type] = callback(function);
00248 }
00249 
00250 void WizziCom::reset(void)
00251 {
00252     COM_FPRINT("\r\n");
00253     
00254     _tx_seq = 0;
00255     _rx_seq = 0;
00256 }
00257 
00258 void WizziCom::send(WizziComPacketType type, uint8_t length, uint8_t* data)
00259 {
00260     wizzi_com_tx_msg_t msg;
00261 
00262     msg.id = g_type_to_flow[type];
00263     msg.pbuf = data;
00264     msg.plen = length;
00265     msg.alen = 0;
00266     
00267     _post_msg(&msg);
00268 }
00269 
00270 void WizziCom::send(WizziComPacket_t* packet)
00271 {
00272     wizzi_com_tx_msg_t msg;
00273 
00274     msg.id = g_type_to_flow[packet->type];
00275     msg.pbuf = packet->data;
00276     msg.plen = packet->length;
00277     msg.alen = 0;
00278     
00279     _post_msg(&msg);
00280 }
00281 
00282 void WizziCom::print_raw(char* str)
00283 {
00284     _send_raw((uint8_t*)str, (uint32_t)strlen(str));
00285 }
00286 
00287 void WizziCom::send_raw(uint8_t* data, uint32_t len)
00288 {
00289     _send_raw(data, len);
00290 }
00291 
00292 /**
00293     Serial Rx Interrupt Service Routine.
00294     Add recevied bytes to the RX buffer.
00295 
00296     @param void
00297     @return void
00298 */
00299 
00300 void WizziCom::_tx_done_isr(int event)
00301 {
00302     _tx_done.release();
00303 }
00304 
00305 void WizziCom::_rx_isr(void)
00306 {
00307     uint8_t byte;
00308     
00309     while (_serial->readable())
00310     {
00311         byte = _serial->getc();
00312         kal_buf_circ_push(_cbuf, &byte);
00313     }
00314 
00315     _rx_done.release();
00316 }
00317 
00318 void WizziCom::_rx_done_isr(int event)
00319 {
00320     uint8_t byte = _rx_byte;
00321     
00322     _serial->read(&_rx_byte, sizeof(uint8_t), callback(this, &WizziCom::_rx_done_isr));
00323     
00324     kal_buf_circ_push(_cbuf, &byte);
00325 
00326     _rx_done.release();
00327 }
00328 /**
00329     CTS pin Interrupt Service Routine.
00330     For flow control (not yet inplemented)
00331 
00332     @param void
00333     @return void
00334 */
00335 void WizziCom::_irq_in_isr()
00336 {
00337     //PRINT("IRQ_IN_ISR\r\n");
00338     //_irq_in_int->release();
00339 }
00340 
00341 /**
00342     Wakes-up modem and send data throught Serial.
00343 
00344     @param const uint8_t*       Pointer to data buffer
00345     @param uint32_t             Data length
00346     @return void
00347 */
00348 void WizziCom::_send_raw(uint8_t* data, uint32_t len)
00349 {
00350     if (_irq_out)
00351     {
00352         *(_irq_out) = 1;
00353         ThisThread::sleep_for(10);
00354     }
00355     
00356     _serial->write(data, len, callback(this, &WizziCom::_tx_done_isr));
00357     _tx_done.acquire();
00358    
00359     if (_irq_out)
00360     {
00361         // Important to not release the ressource too soon
00362         ThisThread::sleep_for(2);
00363         *(_irq_out) = 0;
00364     }
00365 }
00366 
00367 void WizziCom::_sys_xack(void)
00368 {
00369     send(WizziComPacketSysXack, 0, NULL);
00370 }
00371 
00372 // Formats to send packet throught Serial.
00373 wizzi_com_tx_buf_t* WizziCom::_new_msg(wizzi_com_tx_msg_t* msg)
00374 {    
00375     uint8_t len = KAL_COM_HEADER_LEN + msg->alen + msg->plen;
00376     COM_FPRINT("(len:%d)\r\n", len);
00377     
00378     wizzi_com_tx_buf_t* tx_buf = (wizzi_com_tx_buf_t*)MALLOC(sizeof(wizzi_com_tx_buf_t) - 1 + len);
00379     
00380     // construct serial header
00381     // concatenate and update tx_seq ID
00382     uint8_t* p = tx_buf->buf;
00383     uint8_t* t = p;
00384     *p++ = (uint8_t)KAL_COM_SYNC_BYTE_0;
00385     *p++ = (uint8_t)KAL_COM_SYNC_BYTE_1;
00386     *p++ = (uint8_t)msg->alen + msg->plen;
00387     *p++ = (uint8_t)_tx_seq++;
00388     *p++ = (uint8_t)msg->id;
00389 
00390     // copy payload and parameters
00391     memcpy(p, msg->pbuf, msg->plen);
00392     p += msg->plen;
00393     memcpy(p, msg->abuf, msg->alen);
00394     p += msg->alen;
00395     
00396     tx_buf->len = (uint32_t)(p - t);
00397     
00398     ASSERT(tx_buf->len == len, "New msg wrong length %d expected %d\r\n", tx_buf->len, len);
00399     
00400     return tx_buf;
00401 }
00402 
00403 void WizziCom::_post_msg(wizzi_com_tx_msg_t* msg)
00404 {
00405     COM_FPRINT("\r\n");
00406     
00407     wizzi_com_tx_buf_t* new_m = _new_msg(msg);
00408     tx_queue_element_t* new_e = _tx_mpool.alloc();
00409     
00410     memcpy(new_e, &new_m, sizeof(tx_queue_element_t));
00411     
00412     ASSERT(_tx_queue.put(new_e) == osOK, "WizziCom TX queue full!\r\n");
00413 }
00414 
00415 void WizziCom::_new_pkt(WizziComPacket_t* packet)
00416 {
00417     //COM_COM_FPRINT("\r\n");
00418 
00419     COM_DPRINT("--> (%02d) %d\r\n", packet->type, packet->length);
00420 
00421     // Distribute packet types to callbacks
00422     switch (packet->type)
00423     {
00424         case WizziComPacketSysXon:
00425             FREE(packet);
00426             COM_DPRINT("XON\r\n");
00427             _tx_thread.flags_set(XON_SIGNAL);
00428             break;
00429         case WizziComPacketSysXoff:
00430             FREE(packet);
00431             COM_DPRINT("XOFF\r\n");
00432             _sys_xack();
00433             break;
00434         default:
00435             rx_queue_element_t* new_e = _rx_mpool.alloc();
00436             memcpy(new_e, &packet, sizeof(rx_queue_element_t));
00437             ASSERT(_rx_queue.put(new_e) == osOK, "WizziCom RX queue full!\r\n");
00438             break;
00439     }
00440 }
00441 
00442 // Thread for calling callbacks
00443 // Like arg, arg thread is stalled by callbacks but not the parsing thread.
00444 void WizziCom::_thread_callback(void)
00445 {
00446     osEvent evt;
00447     rx_queue_element_t* e;
00448     WizziComPacket_t* packet;
00449     
00450     COM_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
00451     while (true)
00452     {
00453         // wait for available packet
00454         evt = _rx_queue.get();
00455         e = (evt.status == osEventMessage)? (rx_queue_element_t*)evt.value.p : NULL;
00456         
00457         if (e != NULL)
00458         {
00459             packet = *e;
00460 
00461             if (_callback[packet->type])
00462             {
00463                 _callback[packet->type].call(this, packet);
00464             }
00465             else if (_callback[WizziComPacketUntreated])
00466             {
00467                 _callback[WizziComPacketUntreated].call(this, packet);
00468             }
00469             else
00470             {
00471                 EPRINT("Untreated pkt type %d in queue!\r\n", packet->type);
00472             }
00473             FREE(packet);
00474             _rx_mpool.free(e);
00475         }
00476     }
00477 }
00478 
00479 
00480 // Thread for parsing packets from RX buffer.
00481 void WizziCom::_thread_rx(void)
00482 {
00483     uint8_t seqnum;
00484     uint8_t header[KAL_COM_HEADER_LEN];
00485         
00486     COM_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
00487     while (true)
00488     {
00489         // Wait for header data
00490         while (kal_buf_circ_size(_cbuf) < KAL_COM_HEADER_LEN)
00491         {
00492             _rx_done.acquire();
00493         }
00494         
00495         // Copy header from buffer (data stays in buffer)
00496         kal_buf_circ_fetch(_cbuf, header, KAL_COM_HEADER_LEN);
00497         
00498         // Check sync bytes
00499         if(KAL_COM_SYNC_BYTE_0 == header[0] && KAL_COM_SYNC_BYTE_1 == header[1])
00500         {
00501             // Packet valid, drop header from buffer
00502             kal_buf_circ_get(_cbuf, NULL, KAL_COM_HEADER_LEN);
00503             
00504             // Fill temp header
00505             _msg.blen = header[2];
00506             seqnum = header[3];
00507             _msg.id = wizzicom_flow_to_type(header[4]);
00508             
00509             // Wait for body data
00510             while (kal_buf_circ_size(_cbuf) < _msg.blen)
00511             {
00512                 _rx_done.acquire();
00513             }
00514             
00515             // Update seqnum
00516             WARNING(_rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d\r\n", _rx_seq, seqnum);
00517             _rx_seq = seqnum + 1;
00518             
00519             COM_DPRINT("COM packet (id: %02X seq: %d body: %d bytes)\r\n", _msg.id, seqnum, _msg.blen);
00520                         
00521             if (_callback[_msg.id] || _callback[WizziComPacketUntreated])
00522             {            
00523                 WizziComPacket_t* pkt = (WizziComPacket_t*)MALLOC(sizeof(WizziComPacket_t) - 1 + _msg.blen);
00524         
00525                 // copy data to buffer
00526                 pkt->length = _msg.blen;
00527                 pkt->type = _msg.id;
00528                 
00529                 if (_msg.blen)
00530                 {
00531                     // Get payload from buffer
00532                     kal_buf_circ_get(_cbuf, pkt->data, _msg.blen);
00533                 }
00534         
00535                 // add packet to queue
00536                 _new_pkt(pkt);
00537             }
00538             else
00539             {
00540                 // Ignore packet
00541                 COM_DPRINT("Ignore pkt id %02X\r\n", _msg.id);
00542                 
00543                 // Drop payload from buffer
00544                 kal_buf_circ_get(_cbuf, NULL, _msg.blen);
00545             }
00546         }
00547         else
00548         {
00549             // Resync
00550             PRINT("COM: Resync\n");
00551             
00552             // Drop a byte
00553             kal_buf_circ_pop(_cbuf, NULL);
00554         }
00555     }
00556 }
00557 
00558 void WizziCom::_thread_tx(void)
00559 {
00560     COM_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
00561     
00562     tx_queue_element_t* e;
00563     wizzi_com_tx_buf_t* msg;
00564     osEvent evt;
00565     uint8_t flow_id;
00566     
00567     while (true)
00568     {
00569         // wait for data to send
00570         evt = _tx_queue.get();
00571         e = (evt.status == osEventMessage)? (tx_queue_element_t*)evt.value.p : NULL;
00572         
00573         // send message
00574         if (e != NULL)
00575         {
00576             msg = *e;
00577 
00578             flow_id = msg->buf[4];
00579             
00580             COM_DPRINT("<-- (%02d) %d\r\n", wizzicom_flow_to_type(flow_id), (msg->len - KAL_COM_HEADER_LEN));
00581 
00582             _send_raw(msg->buf, msg->len);
00583             FREE(msg);
00584             _tx_mpool.free(e);
00585             
00586             if (KAL_COM_FLOW_SYS_XACK == flow_id)
00587             {
00588                 COM_DPRINT("XACK\r\n");
00589                ThisThread::flags_wait_all(XON_SIGNAL);
00590             }
00591         }
00592     }
00593 }