--- gatchat/gathdlc.c | 75 ++++++++++++++++++++++++++++------------------------ 1 files changed, 40 insertions(+), 35 deletions(-)
diff --git a/gatchat/gathdlc.c b/gatchat/gathdlc.c index c5c02cf..13e15a1 100644 --- a/gatchat/gathdlc.c +++ b/gatchat/gathdlc.c @@ -59,11 +59,12 @@ static void new_bytes(GAtHDLC *hdlc) { unsigned int len = ring_buffer_len(hdlc->read_buffer); unsigned char *buf = ring_buffer_read_ptr(hdlc->read_buffer, 0); + unsigned int wrap = ring_buffer_len_no_wrap(hdlc->read_buffer); unsigned char val; unsigned int pos = 0; while (pos < len) { - if (buf[pos] == 0x7e) { + if (*buf == 0x7e) { if (hdlc->receive_func && hdlc->decode_offset > 2 && hdlc->decode_fcs == 0xf0b8) { hdlc->receive_func(hdlc->decode_buffer, @@ -74,29 +75,44 @@ static void new_bytes(GAtHDLC *hdlc) hdlc->decode_fcs = 0xffff; hdlc->decode_offset = 0; pos++; + buf++; + if (pos == wrap) + buf = ring_buffer_read_ptr(hdlc->read_buffer, + pos); continue; } if (hdlc->recv_escape_func && hdlc->recv_escape_func(hdlc->recv_escape_data, - buf[pos])) { + *buf)) { pos++; + buf++; + if (pos == wrap) + buf = ring_buffer_read_ptr(hdlc->read_buffer, + pos); continue; } - if (buf[pos] == 0x7d) { + if (*buf == 0x7d) { if (pos + 2 > len) break; pos++; - val = buf[pos] ^ 0x20; + buf++; + if (pos == wrap) + buf = ring_buffer_read_ptr(hdlc->read_buffer, + pos); + val = *buf ^ 0x20; } else - val = buf[pos]; + val = *buf; hdlc->decode_buffer[hdlc->decode_offset] = val; hdlc->decode_fcs = crc_ccitt_byte(hdlc->decode_fcs, val); hdlc->decode_offset++; pos++; + buf++; + if (pos == wrap) + buf = ring_buffer_read_ptr(hdlc->read_buffer, pos); } ring_buffer_drain(hdlc->read_buffer, pos); @@ -344,55 +360,44 @@ static void wakeup_write(GAtHDLC *hdlc) can_write_data, hdlc, write_watch_destroy); } -static inline void hdlc_put(GAtHDLC *hdlc, guint8 *buf, gsize *pos, guint8 c, - gboolean scan_result) +static inline void hdlc_buffer_put_char(struct ring_buffer *buffer, + const char val) { - gsize i = *pos; + ring_buffer_write(buffer, &val, 1); +} +static inline void hdlc_put(GAtHDLC *hdlc, guint8 c, gboolean scan_result) +{ if ((hdlc->send_escape_func && hdlc->send_escape_func(hdlc->send_escape_data, c, scan_result)) || c == 0x7e || c == 0x7d) { - buf[i++] = 0x7d; - buf[i++] = c ^ 0x20; + hdlc_buffer_put_char(hdlc->write_buffer, 0x7d); + hdlc_buffer_put_char(hdlc->write_buffer, c ^ 0x20); } else - buf[i++] = c; - - *pos = i; + hdlc_buffer_put_char(hdlc->write_buffer, c); } gboolean g_at_hdlc_send(GAtHDLC *hdlc, const unsigned char *data, gsize size) { - unsigned char *buf; - unsigned int space, i = 0; guint16 fcs = 0xffff; - gsize pos; gboolean scan_result = FALSE; + unsigned int i = 0; if (hdlc->scan_func) scan_result = hdlc->scan_func(hdlc->scan_data, data); - do { - space = ring_buffer_avail_no_wrap(hdlc->write_buffer); - if (space == 0) - break; + hdlc_buffer_put_char(hdlc->write_buffer, 0x7e); - buf = ring_buffer_write_ptr(hdlc->write_buffer); - pos = 0; - - while (size--) { - fcs = crc_ccitt_byte(fcs, data[i]); - hdlc_put(hdlc, buf, &pos, data[i++], scan_result); - } - - fcs ^= 0xffff; - hdlc_put(hdlc, buf, &pos, fcs & 0xff, scan_result); - hdlc_put(hdlc, buf, &pos, fcs >> 8, scan_result); - - buf[pos++] = 0x7e; + while (size--) { + fcs = crc_ccitt_byte(fcs, data[i]); + hdlc_put(hdlc, data[i++], scan_result); + } - ring_buffer_write_advance(hdlc->write_buffer, pos); - } while (0); + fcs ^= 0xffff; + hdlc_put(hdlc, fcs & 0xff, scan_result); + hdlc_put(hdlc, fcs >> 8, scan_result); + hdlc_buffer_put_char(hdlc->write_buffer, 0x7e); wakeup_write(hdlc); -- 1.6.6.1 _______________________________________________ ofono mailing list ofono@ofono.org http://lists.ofono.org/listinfo/ofono