| /* |
| * |
| * AT chat library with GLib integration |
| * |
| * Copyright (C) 2008-2011 Intel Corporation. All rights reserved. |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License version 2 as |
| * published by the Free Software Foundation. |
| * |
| * This program is distributed in the hope that it will be useful, |
| * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| * GNU General Public License for more details. |
| * |
| * You should have received a copy of the GNU General Public License |
| * along with this program; if not, write to the Free Software |
| * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
| * |
| */ |
| |
| #ifdef HAVE_CONFIG_H |
| #include <config.h> |
| #endif |
| |
| #include <arpa/inet.h> |
| #include <sys/time.h> |
| #include <sys/types.h> |
| #include <sys/stat.h> |
| #include <fcntl.h> |
| #include <stdio.h> |
| #include <unistd.h> |
| #include <glib.h> |
| |
| #include "crc-ccitt.h" |
| #include "ringbuffer.h" |
| #include "gatio.h" |
| #include "gatutil.h" |
| #include "gathdlc.h" |
| |
| #define BUFFER_SIZE (2 * 2048) |
| #define MAX_BUFFERS 64 /* Maximum number of in-flight write buffers */ |
| #define HDLC_OVERHEAD 256 /* Rough estimate of HDLC protocol overhead */ |
| |
| #define HDLC_FLAG 0x7e /* Flag sequence */ |
| #define HDLC_ESCAPE 0x7d /* Asynchronous control escape */ |
| #define HDLC_TRANS 0x20 /* Asynchronous transparency modifier */ |
| |
| #define HDLC_INITFCS 0xffff /* Initial FCS value */ |
| #define HDLC_GOODFCS 0xf0b8 /* Good final FCS value */ |
| |
| #define HDLC_FCS(fcs, c) crc_ccitt_byte(fcs, c) |
| |
| #define GUARD_TIMEOUT 1000 /* Pause time before and after '+++' sequence */ |
| |
| struct _GAtHDLC { |
| gint ref_count; |
| GAtIO *io; |
| GQueue *write_queue; /* Write buffer queue */ |
| unsigned char *decode_buffer; |
| guint decode_offset; |
| guint16 decode_fcs; |
| gboolean decode_escape; |
| guint32 xmit_accm[8]; |
| guint32 recv_accm; |
| GAtReceiveFunc receive_func; |
| gpointer receive_data; |
| GAtDebugFunc debugf; |
| gpointer debug_data; |
| int record_fd; |
| gboolean in_read_handler; |
| gboolean destroyed; |
| gboolean wakeup_sent; |
| gboolean start_frame_marker; |
| gboolean no_carrier_detect; |
| GAtSuspendFunc suspend_func; |
| gpointer suspend_data; |
| guint suspend_source; |
| GTimer *timer; |
| guint num_plus; |
| }; |
| |
| static inline void hdlc_record(GAtHDLC *hdlc, gboolean in, |
| guint8 *data, guint16 length) |
| { |
| guint16 len = htons(length); |
| guint32 ts; |
| struct timeval now; |
| unsigned char id; |
| int err; |
| |
| g_at_util_debug_hexdump(in, data, length, |
| hdlc->debugf, hdlc->debug_data); |
| |
| if (hdlc->record_fd < 0) |
| return; |
| |
| if (length == 0) |
| return; |
| |
| gettimeofday(&now, NULL); |
| ts = htonl(now.tv_sec & 0xffffffff); |
| |
| id = 0x07; |
| |
| err = write(hdlc->record_fd, &id, 1); |
| if (err < 0) |
| return; |
| |
| err = write(hdlc->record_fd, &ts, 4); |
| if (err < 0) |
| return; |
| |
| id = in ? 0x02 : 0x01; |
| |
| err = write(hdlc->record_fd, &id, 1); |
| if (err < 0) |
| return; |
| |
| err = write(hdlc->record_fd, &len, 2); |
| if (err < 0) |
| return; |
| |
| err = write(hdlc->record_fd, data, length); |
| if (err < 0) |
| return; |
| } |
| |
| void g_at_hdlc_set_recording(GAtHDLC *hdlc, const char *filename) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| if (hdlc->record_fd > fileno(stderr)) { |
| close(hdlc->record_fd); |
| hdlc->record_fd = -1; |
| } |
| |
| if (filename == NULL) |
| return; |
| |
| hdlc->record_fd = open(filename, O_WRONLY | O_CREAT | O_APPEND, |
| S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); |
| } |
| |
| void g_at_hdlc_set_recv_accm(GAtHDLC *hdlc, guint32 accm) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| hdlc->recv_accm = accm; |
| } |
| |
| guint32 g_at_hdlc_get_recv_accm(GAtHDLC *hdlc) |
| { |
| if (hdlc == NULL) |
| return 0; |
| |
| return hdlc->recv_accm; |
| } |
| |
| void g_at_hdlc_set_suspend_function(GAtHDLC *hdlc, GAtSuspendFunc func, |
| gpointer user_data) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| if (func == NULL) { |
| if (hdlc->timer) { |
| g_timer_destroy(hdlc->timer); |
| hdlc->timer = NULL; |
| } |
| |
| if (hdlc->suspend_source > 0) { |
| g_source_remove(hdlc->suspend_source); |
| hdlc->suspend_source = 0; |
| } |
| } else |
| hdlc->timer = g_timer_new(); |
| |
| hdlc->suspend_func = func; |
| hdlc->suspend_data = user_data; |
| } |
| |
| static gboolean hdlc_suspend(gpointer user_data) |
| { |
| GAtHDLC *hdlc = user_data; |
| |
| g_at_io_drain_ring_buffer(hdlc->io, 3); |
| |
| g_at_io_set_write_handler(hdlc->io, NULL, NULL); |
| g_at_io_set_read_handler(hdlc->io, NULL, NULL); |
| |
| if (hdlc->suspend_func) |
| hdlc->suspend_func(hdlc->suspend_data); |
| |
| hdlc->suspend_source = 0; |
| |
| return FALSE; |
| } |
| |
| static gboolean check_escape(GAtHDLC *hdlc, struct ring_buffer *rbuf) |
| { |
| unsigned int len = ring_buffer_len(rbuf); |
| unsigned int wrap = ring_buffer_len_no_wrap(rbuf); |
| unsigned char *buf = ring_buffer_read_ptr(rbuf, 0); |
| unsigned int pos = 0; |
| unsigned int elapsed = g_timer_elapsed(hdlc->timer, NULL) * 1000; |
| unsigned int num_plus = 0; |
| gboolean guard_timeout = FALSE; |
| |
| if (elapsed >= GUARD_TIMEOUT) |
| guard_timeout = TRUE; |
| |
| while (pos < len && pos < 3) { |
| if (*buf != '+') |
| break; |
| |
| num_plus++; |
| buf++; |
| pos++; |
| |
| if (pos == wrap) |
| buf = ring_buffer_read_ptr(rbuf, pos); |
| } |
| |
| if (num_plus != len) |
| return FALSE; |
| |
| /* We got some escape chars, but no guard timeout first */ |
| if (guard_timeout == FALSE && hdlc->num_plus == 0) |
| return FALSE; |
| |
| if (num_plus != 3) { |
| hdlc->num_plus = num_plus; |
| return TRUE; |
| } |
| |
| hdlc->num_plus = 0; |
| hdlc->suspend_source = g_timeout_add(GUARD_TIMEOUT, hdlc_suspend, hdlc); |
| |
| return TRUE; |
| } |
| |
| static void new_bytes(struct ring_buffer *rbuf, gpointer user_data) |
| { |
| GAtHDLC *hdlc = user_data; |
| unsigned int len = ring_buffer_len(rbuf); |
| unsigned int wrap = ring_buffer_len_no_wrap(rbuf); |
| unsigned char *buf = ring_buffer_read_ptr(rbuf, 0); |
| unsigned int pos = 0; |
| |
| /* |
| * We delete the the paused_timeout_cb or hdlc_suspend as soons as |
| * we read a data. |
| */ |
| if (hdlc->suspend_source > 0) { |
| g_source_remove(hdlc->suspend_source); |
| hdlc->suspend_source = 0; |
| g_timer_start(hdlc->timer); |
| } else if (hdlc->timer) { |
| gboolean escaping = check_escape(hdlc, rbuf); |
| |
| g_timer_start(hdlc->timer); |
| |
| if (escaping) |
| return; |
| } |
| |
| hdlc_record(hdlc, TRUE, buf, wrap); |
| |
| hdlc->in_read_handler = TRUE; |
| |
| while (pos < len) { |
| /* |
| * We try to detect NO CARRIER conditions here. We |
| * (ab) use the fact that a HDLC_FLAG must be followed |
| * by the Address or Protocol fields, depending on whether |
| * ACFC is enabled. |
| */ |
| if (hdlc->no_carrier_detect && |
| hdlc->decode_offset == 0 && *buf == '\r') |
| break; |
| |
| if (hdlc->decode_escape == TRUE) { |
| unsigned char val = *buf ^ HDLC_TRANS; |
| |
| hdlc->decode_buffer[hdlc->decode_offset++] = val; |
| hdlc->decode_fcs = HDLC_FCS(hdlc->decode_fcs, val); |
| |
| hdlc->decode_escape = FALSE; |
| } else if (*buf == HDLC_ESCAPE) { |
| hdlc->decode_escape = TRUE; |
| } else if (*buf == HDLC_FLAG) { |
| if (hdlc->receive_func && hdlc->decode_offset > 2 && |
| hdlc->decode_fcs == HDLC_GOODFCS) { |
| hdlc->receive_func(hdlc->decode_buffer, |
| hdlc->decode_offset - 2, |
| hdlc->receive_data); |
| |
| if (hdlc->destroyed) |
| goto out; |
| } |
| |
| hdlc->decode_fcs = HDLC_INITFCS; |
| hdlc->decode_offset = 0; |
| } else if (*buf >= 0x20 || |
| (hdlc->recv_accm & (1 << *buf)) == 0) { |
| hdlc->decode_buffer[hdlc->decode_offset++] = *buf; |
| hdlc->decode_fcs = HDLC_FCS(hdlc->decode_fcs, *buf); |
| } |
| |
| buf++; |
| pos++; |
| |
| if (pos == wrap) { |
| buf = ring_buffer_read_ptr(rbuf, pos); |
| hdlc_record(hdlc, TRUE, buf, len - wrap); |
| } |
| } |
| |
| out: |
| ring_buffer_drain(rbuf, pos); |
| |
| hdlc->in_read_handler = FALSE; |
| |
| if (hdlc->destroyed) |
| g_free(hdlc); |
| } |
| |
| GAtHDLC *g_at_hdlc_new_from_io(GAtIO *io) |
| { |
| GAtHDLC *hdlc; |
| struct ring_buffer* write_buffer; |
| |
| if (io == NULL) |
| return NULL; |
| |
| hdlc = g_try_new0(GAtHDLC, 1); |
| if (hdlc == NULL) |
| return NULL; |
| |
| hdlc->ref_count = 1; |
| hdlc->decode_fcs = HDLC_INITFCS; |
| hdlc->decode_offset = 0; |
| hdlc->decode_escape = FALSE; |
| |
| hdlc->xmit_accm[0] = ~0U; |
| hdlc->xmit_accm[3] = 0x60000000; /* 0x7d, 0x7e */ |
| hdlc->recv_accm = ~0U; |
| |
| write_buffer = ring_buffer_new(BUFFER_SIZE); |
| if (!write_buffer) |
| goto error; |
| |
| hdlc->write_queue = g_queue_new(); |
| if (!hdlc->write_queue) |
| goto error; |
| |
| g_queue_push_tail(hdlc->write_queue, write_buffer); |
| |
| hdlc->decode_buffer = g_try_malloc(BUFFER_SIZE); |
| if (!hdlc->decode_buffer) |
| goto error; |
| |
| hdlc->record_fd = -1; |
| |
| hdlc->io = g_at_io_ref(io); |
| g_at_io_set_read_handler(hdlc->io, new_bytes, hdlc); |
| |
| return hdlc; |
| |
| error: |
| if (hdlc->write_queue) |
| g_queue_free(hdlc->write_queue); |
| |
| if (write_buffer) |
| ring_buffer_free(write_buffer); |
| |
| g_free(hdlc->decode_buffer); |
| |
| g_free(hdlc); |
| |
| return NULL; |
| } |
| |
| GAtHDLC *g_at_hdlc_new(GIOChannel *channel) |
| { |
| GAtIO *io; |
| GAtHDLC *hdlc; |
| |
| io = g_at_io_new(channel); |
| if (io == NULL) |
| return NULL; |
| |
| hdlc = g_at_hdlc_new_from_io(io); |
| g_at_io_unref(io); |
| |
| return hdlc; |
| } |
| |
| GAtHDLC *g_at_hdlc_ref(GAtHDLC *hdlc) |
| { |
| if (hdlc == NULL) |
| return NULL; |
| |
| g_atomic_int_inc(&hdlc->ref_count); |
| |
| return hdlc; |
| } |
| |
| void g_at_hdlc_unref(GAtHDLC *hdlc) |
| { |
| struct ring_buffer *write_buffer; |
| |
| if (hdlc == NULL) |
| return; |
| |
| if (g_atomic_int_dec_and_test(&hdlc->ref_count) == FALSE) |
| return; |
| |
| if (hdlc->record_fd > fileno(stderr)) { |
| close(hdlc->record_fd); |
| hdlc->record_fd = -1; |
| } |
| |
| g_at_io_set_write_handler(hdlc->io, NULL, NULL); |
| g_at_io_set_read_handler(hdlc->io, NULL, NULL); |
| |
| if (hdlc->suspend_source > 0) |
| g_source_remove(hdlc->suspend_source); |
| |
| g_at_io_unref(hdlc->io); |
| hdlc->io = NULL; |
| |
| while ((write_buffer = g_queue_pop_head(hdlc->write_queue))) |
| ring_buffer_free(write_buffer); |
| |
| g_queue_free(hdlc->write_queue); |
| |
| g_free(hdlc->decode_buffer); |
| |
| g_timer_destroy(hdlc->timer); |
| |
| if (hdlc->in_read_handler) |
| hdlc->destroyed = TRUE; |
| else |
| g_free(hdlc); |
| } |
| |
| void g_at_hdlc_set_debug(GAtHDLC *hdlc, GAtDebugFunc func, gpointer user_data) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| hdlc->debugf = func; |
| hdlc->debug_data = user_data; |
| } |
| |
| void g_at_hdlc_set_receive(GAtHDLC *hdlc, GAtReceiveFunc func, |
| gpointer user_data) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| hdlc->receive_func = func; |
| hdlc->receive_data = user_data; |
| } |
| |
| static gboolean can_write_data(gpointer data) |
| { |
| GAtHDLC *hdlc = data; |
| unsigned int len; |
| unsigned char *buf; |
| gsize bytes_written; |
| struct ring_buffer* write_buffer; |
| |
| /* Write data out from the head of the queue */ |
| write_buffer = g_queue_peek_head(hdlc->write_queue); |
| |
| len = ring_buffer_len_no_wrap(write_buffer); |
| buf = ring_buffer_read_ptr(write_buffer, 0); |
| |
| bytes_written = g_at_io_write(hdlc->io, (gchar *) buf, len); |
| hdlc_record(hdlc, FALSE, buf, bytes_written); |
| ring_buffer_drain(write_buffer, bytes_written); |
| |
| if (ring_buffer_len(write_buffer) > 0) |
| return TRUE; |
| |
| /* All data in current buffer is written, free it |
| * unless it's the last buffer in the queue. |
| */ |
| if ((ring_buffer_len(write_buffer) == 0) && |
| (g_queue_get_length(hdlc->write_queue) > 1)) { |
| write_buffer = g_queue_pop_head(hdlc->write_queue); |
| ring_buffer_free(write_buffer); |
| write_buffer = g_queue_peek_head(hdlc->write_queue); |
| } |
| |
| if (ring_buffer_len(write_buffer) > 0) |
| return TRUE; |
| |
| return FALSE; |
| } |
| |
| void g_at_hdlc_set_xmit_accm(GAtHDLC *hdlc, guint32 accm) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| hdlc->xmit_accm[0] = accm; |
| } |
| |
| guint32 g_at_hdlc_get_xmit_accm(GAtHDLC *hdlc) |
| { |
| if (hdlc == NULL) |
| return 0; |
| |
| return hdlc->xmit_accm[0]; |
| } |
| |
| GAtIO *g_at_hdlc_get_io(GAtHDLC *hdlc) |
| { |
| if (hdlc == NULL) |
| return NULL; |
| |
| return hdlc->io; |
| } |
| |
| #define NEED_ESCAPE(xmit_accm, c) xmit_accm[c >> 5] & (1 << (c & 0x1f)) |
| |
| gboolean g_at_hdlc_send(GAtHDLC *hdlc, const unsigned char *data, gsize size) |
| { |
| struct ring_buffer* write_buffer = g_queue_peek_tail(hdlc->write_queue); |
| |
| unsigned int avail = ring_buffer_avail(write_buffer); |
| unsigned int wrap = ring_buffer_avail_no_wrap(write_buffer); |
| unsigned char *buf; |
| unsigned char tail[2]; |
| unsigned int i = 0; |
| guint16 fcs = HDLC_INITFCS; |
| gboolean escape = FALSE; |
| gsize pos = 0; |
| |
| if (avail < size + HDLC_OVERHEAD) { |
| if (g_queue_get_length(hdlc->write_queue) > MAX_BUFFERS) |
| return FALSE; /* Too many pending buffers */ |
| |
| write_buffer = ring_buffer_new(BUFFER_SIZE); |
| if (write_buffer == NULL) |
| return FALSE; |
| |
| g_queue_push_tail(hdlc->write_queue, write_buffer); |
| |
| avail = ring_buffer_avail(write_buffer); |
| wrap = ring_buffer_avail_no_wrap(write_buffer); |
| } |
| |
| i = 0; |
| buf = ring_buffer_write_ptr(write_buffer, 0); |
| |
| if (hdlc->start_frame_marker == TRUE) { |
| /* Protocol requires 0x7e as start marker */ |
| if (pos + 1 > avail) |
| return FALSE; |
| |
| *buf++ = HDLC_FLAG; |
| pos++; |
| |
| if (pos == wrap) |
| buf = ring_buffer_write_ptr(write_buffer, pos); |
| } else if (hdlc->wakeup_sent == FALSE) { |
| /* Write an initial 0x7e as wakeup character */ |
| *buf++ = HDLC_FLAG; |
| pos++; |
| |
| hdlc->wakeup_sent = TRUE; |
| } |
| |
| while (pos < avail && i < size) { |
| if (escape == TRUE) { |
| fcs = HDLC_FCS(fcs, data[i]); |
| *buf = data[i++] ^ HDLC_TRANS; |
| escape = FALSE; |
| } else if (NEED_ESCAPE(hdlc->xmit_accm, data[i])) { |
| *buf = HDLC_ESCAPE; |
| escape = TRUE; |
| } else { |
| fcs = HDLC_FCS(fcs, data[i]); |
| *buf = data[i++]; |
| } |
| |
| buf++; |
| pos++; |
| |
| if (pos == wrap) |
| buf = ring_buffer_write_ptr(write_buffer, pos); |
| } |
| |
| if (i < size) |
| return FALSE; |
| |
| fcs ^= HDLC_INITFCS; |
| tail[0] = fcs & 0xff; |
| tail[1] = fcs >> 8; |
| |
| i = 0; |
| |
| while (pos < avail && i < sizeof(tail)) { |
| if (escape == TRUE) { |
| *buf = tail[i++] ^ HDLC_TRANS; |
| escape = FALSE; |
| } else if (NEED_ESCAPE(hdlc->xmit_accm, tail[i])) { |
| *buf = HDLC_ESCAPE; |
| escape = TRUE; |
| } else { |
| *buf = tail[i++]; |
| } |
| |
| buf++; |
| pos++; |
| |
| if (pos == wrap) |
| buf = ring_buffer_write_ptr(write_buffer, pos); |
| } |
| |
| if (i < sizeof(tail)) |
| return FALSE; |
| |
| if (pos + 1 > avail) |
| return FALSE; |
| |
| /* Add 0x7e as end marker */ |
| *buf = HDLC_FLAG; |
| pos++; |
| |
| ring_buffer_write_advance(write_buffer, pos); |
| |
| g_at_io_set_write_handler(hdlc->io, can_write_data, hdlc); |
| |
| return TRUE; |
| } |
| |
| void g_at_hdlc_set_start_frame_marker(GAtHDLC *hdlc, gboolean marker) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| hdlc->start_frame_marker = marker; |
| } |
| |
| void g_at_hdlc_set_no_carrier_detect(GAtHDLC *hdlc, gboolean detect) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| hdlc->no_carrier_detect = detect; |
| } |
| |
| void g_at_hdlc_suspend(GAtHDLC *hdlc) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| g_at_io_set_write_handler(hdlc->io, NULL, NULL); |
| g_at_io_set_read_handler(hdlc->io, NULL, NULL); |
| } |
| |
| void g_at_hdlc_resume(GAtHDLC *hdlc) |
| { |
| if (hdlc == NULL) |
| return; |
| |
| g_at_io_set_read_handler(hdlc->io, new_bytes, hdlc); |
| |
| if (g_queue_get_length(hdlc->write_queue) > 0) |
| g_at_io_set_write_handler(hdlc->io, can_write_data, hdlc); |
| } |