gsm0710muxd.c

Linux GPRS 模块多路复用实现多个虚拟串口

			return 0;
		}
	}
	LOG(LOG_DEBUG, "Leave");
	return length;
}

/*
 * Handles received data from device. PARAMS: buf - buffer, which
 * contains received data len - the length of the buffer channel - the
 * number of devices (logical channel), where data was received
 * RETURNS: the number of remaining bytes in partial packet
 */
static int handle_channel_data(
	unsigned char *buf,
	int len,
	int channel)
{
	int written = 0;
	int i = 0;
	int last = 0;
//try to write 5 times
	while (written != len && i < GSM0710_WRITE_RETRIES)
	{
		last = write_frame(channel, buf + written, len - written, GSM0710_TYPE_UIH);
		written += last;
		if (last == 0)
			i++;
	}
	if (i == GSM0710_WRITE_RETRIES)
		LOG(LOG_WARNING, "Couldn't write data to channel %d. Wrote only %d bytes, when should have written %d",
				channel, written, len);
	return 0;
}

static int logical_channel_close(Channel* channel)
{
	if (channel->g_source >= 0)
		g_source_remove(channel->g_source);
	channel->g_source = -1;
	if (channel->fd >= 0)
		close(channel->fd);
	channel->fd = -1;
	if (channel->ptsname != NULL)
		free(channel->ptsname);
	channel->ptsname = NULL;
	if (channel->tmp != NULL)
		free(channel->tmp);
	channel->tmp = NULL;
	if (channel->origin != NULL)
		free(channel->origin);
	channel->origin = NULL;
	channel->opened = 0;
	channel->v24_signals = 0;
	channel->remaining = 0;
	return 0;
}

static int logical_channel_init(Channel* channel, int id)
{
	channel->id = id; // connected channel-id
	channel->devicename = id?"/dev/ptmx":NULL; // TODO do we need this to be dynamic anymore?
	channel->fd = -1;
	channel->g_source = -1;
	channel->ptsname = NULL;
	channel->tmp = NULL;
	channel->origin = NULL;
	return logical_channel_close(channel);
}

gboolean pseudo_device_read(GIOChannel *source, GIOCondition condition, gpointer data)
{
	LOG(LOG_DEBUG, "Enter");
	Channel* channel = (Channel*)data;
	if (condition == G_IO_IN)
	{
		unsigned char buf[4096];
		//information from virtual port
		int len = read(channel->fd, buf + channel->remaining, sizeof(buf) - channel->remaining);
		if (!channel->opened)
		{
			LOG(LOG_WARNING, "Write to a channel which wasn't acked to be open.");
			write_frame(channel->id, NULL, 0, GSM0710_TYPE_SABM | GSM0710_PF);
			LOG(LOG_DEBUG, "Leave");
			return TRUE;
		}
		if (len >= 0)
		{
			LOG(LOG_DEBUG, "Data from channel %d, %d bytes", channel->id, len);
			if (channel->remaining > 0)
			{
				memcpy(buf, channel->tmp, channel->remaining);
				free(channel->tmp);
				channel->tmp = NULL;
			}
			if (len + channel->remaining > 0)
				channel->remaining = handle_channel_data(buf, len + channel->remaining, channel->id);
			//copy remaining bytes from last packet into tmp
			if (channel->remaining > 0)
			{
				channel->tmp = malloc(channel->remaining);
				memcpy(channel->tmp, buf + sizeof(buf) - channel->remaining, channel->remaining);
			}
			LOG(LOG_DEBUG, "Leave");
			return TRUE;
		}
		// dropped connection
		if (cmux_mode)
			write_frame(channel->id, NULL, 0, GSM0710_CONTROL_CLD | GSM0710_CR);
		else
			write_frame(channel->id, close_channel_cmd, 2, GSM0710_TYPE_UIH);
		logical_channel_close(channel);
		LOG(LOG_INFO, "Logical channel %d for %s closed", channel->id, channel->origin);
	}
	else if (condition == G_IO_HUP)
	{
		if (cmux_mode)
			write_frame(channel->id, NULL, 0, GSM0710_CONTROL_CLD | GSM0710_CR);
		else
			write_frame(channel->id, close_channel_cmd, 2, GSM0710_TYPE_UIH);
		logical_channel_close(channel);
		LOG(LOG_INFO, "Logical channel %d for %s closed", channel->id, channel->origin);
	}
	LOG(LOG_DEBUG, "Leave");
	return FALSE;
}

static gboolean watchdog(gpointer data);
static int close_devices();

static gboolean c_get_power(const char* origin)
{
	LOG(LOG_DEBUG, "Enter");
	LOG(LOG_DEBUG, "Leave");
	return serial.state != MUX_STATE_OFF;
}

static gboolean c_set_power(const char* origin, gboolean on)
{
	LOG(LOG_DEBUG, "Enter");
	if (on)
	{
		if (serial.state == MUX_STATE_OFF)
		{
			LOG(LOG_INFO, "power on");
			serial.state = MUX_STATE_OPENING;
			watchdog(&serial);
		}
		else
			LOG(LOG_WARNING, "power on request received but was already on");
	}
	else
	{
		if (serial.state == MUX_STATE_MUXING)
		{
			LOG(LOG_INFO, "power off");
			g_source_remove(serial.g_source_watchdog);
			close_devices();
		}
		else
			LOG(LOG_WARNING, "power off received but wasn't on/muxing");
	}
	LOG(LOG_DEBUG, "Leave");
	return TRUE;
}

static gboolean c_reset_modem(const char* origin)
{
	LOG(LOG_DEBUG, "Enter");
	LOG(LOG_INFO, "modem reset");
	serial.state = MUX_STATE_CLOSING;				
	return TRUE;
}

static gboolean c_alloc_channel(const char* origin, const char** name)
{
	LOG(LOG_DEBUG, "Enter");
	int i;
	if (serial.state == MUX_STATE_MUXING)
		for (i=1;i<GSM0710_MAX_CHANNELS;i++)
			if (channellist[i].fd < 0) // is this channel free?
			{
				LOG(LOG_DEBUG, "Found channel %d fd %d on %s", i, channellist[i].fd, channellist[i].devicename);
				channellist[i].origin = strdup(origin);
				SYSCHECK(channellist[i].fd = open(channellist[i].devicename, O_RDWR | O_NONBLOCK)); //open devices
				char* pts = ptsname(channellist[i].fd);
				if (pts == NULL) SYSCHECK(-1);
				channellist[i].ptsname = strdup(pts);
				struct termios options;
				tcgetattr(channellist[i].fd, &options); //get the parameters
				options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); //set raw input
				options.c_iflag &= ~(INLCR | ICRNL | IGNCR);
				options.c_oflag &= ~(OPOST| OLCUC| ONLRET| ONOCR| OCRNL); //set raw output
				tcsetattr(channellist[i].fd, TCSANOW, &options);
				if (!strcmp(channellist[i].devicename, "/dev/ptmx"))
				{
					//Otherwise programs cannot access the pseudo terminals
					SYSCHECK(grantpt(channellist[i].fd));
					SYSCHECK(unlockpt(channellist[i].fd));
				}
				channellist[i].v24_signals = GSM0710_SIGNAL_DV | GSM0710_SIGNAL_RTR | GSM0710_SIGNAL_RTC | GSM0710_EA;
				GIOChannel* g_channel = g_io_channel_unix_new(channellist[i].fd);
				channellist[i].g_source = g_io_add_watch(g_channel, G_IO_IN | G_IO_HUP, pseudo_device_read, channellist+i);
				write_frame(i, NULL, 0, GSM0710_TYPE_SABM | GSM0710_PF);
				LOG(LOG_INFO, "Connecting %s to virtual channel %d for %s on %s",
					channellist[i].ptsname, channellist[i].id, channellist[i].origin, serial.devicename);
				*name = strdup(channellist[i].ptsname);
				return TRUE;
			}
	LOG(LOG_WARNING, "not muxing or no free channel found");
	return TRUE;
}

static void my_log_handler(
	const gchar *log_domain,
	GLogLevelFlags log_level,
	const gchar *message,
	gpointer user_data)
{
	LOG(LOG_INFO, "%d %s %s", log_level, log_domain, message);
}

#include "muxercontrol.c"
#include "mux-glue.h"

static int dbus_init()
{
	g_log_set_handler(NULL, G_LOG_LEVEL_MASK, my_log_handler, NULL);
	g_type_init();
	g_log_set_always_fatal(G_LOG_LEVEL_WARNING);
	GError* g_err = NULL;
	g_conn = dbus_g_bus_get(DBUS_BUS_SYSTEM, &g_err);
	if (g_err != NULL)
	{
		//g_printerr
		LOG(LOG_ERR, "Connecting to system bus failed: %s", g_err->message);
		g_error_free(g_err);
		return -1;
	}
	GObject* obj = (GObject*)muxer_control_gen();
	DBusError err;
	memset(&err, 0, sizeof(err));
	if (dbus_bus_request_name(dbus_g_connection_get_connection(g_conn), "org.pyneo.muxer", DBUS_NAME_FLAG_ALLOW_REPLACEMENT|DBUS_NAME_FLAG_REPLACE_EXISTING, &err) < 0)
	{
		if (dbus_error_is_set(&err))
			LOG(LOG_ERR, "dbus error with dbus_bus_request_name '%s' '%s'", err.name, err.message);
		else
			LOG(LOG_ERR, "dbus error with dbus_bus_request_name");
		return -1;
	}
	dbus_g_object_type_install_info(TYPE_MUXER_CONTROL, &dbus_glib_mux_object_info);
	dbus_g_connection_register_g_object(g_conn, object_name, obj);
	return 0;
}

static int dbus_deinit()
{
	if (g_conn)
		dbus_g_connection_unref(g_conn);
	return 0;
}

static int dbus_signal_send_deactivate(const char* sigvalue)
{
	DBusConnection* conn = dbus_g_connection_get_connection(g_conn);
	if (NULL == conn)
	{
		LOG(LOG_ERR, "Connection null"); 
		return -1;
	}
	DBusMessage* msg = dbus_message_new_signal(object_name, // object name of the signal
		"org.freesmartphone.GSM.MUX", // interface name of the signal
		"deactivate"); // name of the signal
	if (NULL == msg) 
	{ 
		LOG(LOG_ERR, "Message Null"); 
		return -1;
	}
	DBusMessageIter args;
	dbus_message_iter_init_append(msg, &args); // append arguments onto signal
	if (!dbus_message_iter_append_basic(&args, DBUS_TYPE_STRING, &sigvalue))
	{ 
		LOG(LOG_ERR, "Out Of Memory"); 
		return -1;
	}
	dbus_uint32_t g_serial = 0; // unique number to associate replies with requests
	if (!dbus_connection_send(conn, msg, &g_serial)) // send the message and flush the connection
	{ 
		LOG(LOG_ERR, "Out Of Memory"); 
		return -1;
	}
	dbus_connection_flush(conn);
	// free the message 
	dbus_message_unref(msg);
	return 0;
}

/**
	GPtrArray *table = parse("(14-5),(3-7),(4-9),(33)");

	for(int i = 0; i < table->len; i++)
	{
		int *arr = g_ptr_array_index(table, i);
		printf("(%d, %d)\n", arr[0], arr[1]);
	}
*/
GPtrArray* parse(const gchar* str)
{
	GPtrArray *table = g_ptr_array_new();
	int i = -1;
	int first = -1;
	int pos = 0;
	while(str[++i] != 0)
	{
		if(str[i] == '(')
		{
			pos = i+1;
			first = -1;
		}
		else if(str[i] == ')')
		{
			int *tuple = malloc(sizeof(int)*2);
			
			tuple[1] = atoi(str + pos);
			tuple[0] = first == -1 ? tuple[1] : first;
			
			g_ptr_array_add(table, (gpointer)tuple);
		}
		else if(str[i] == '-')
		{
			first = atoi(str + pos);
			pos = i+1;
		}
	}
	return table;
}

//////////////////////////////////////////////// real functions
/* Allocates memory for a new buffer and initializes it.
 *
 * RETURNS:
 * the pointer to a new buufer
 */
static GSM0710_Buffer *gsm0710_buffer_init(
	)
{
	GSM0710_Buffer* buf = (GSM0710_Buffer*)malloc(sizeof(GSM0710_Buffer));
	if (buf)
	{
		memset(buf, 0, sizeof(GSM0710_Buffer));
		buf->readp = buf->data;
		buf->writep = buf->data;
		buf->endp = buf->data + GSM0710_BUFFER_SIZE;
	}
	return buf;
}

/* Destroys the buffer (i.e. frees up the memory
 *
 * PARAMS:
 * buf - buffer to be destroyed
 */
static void gsm0710_buffer_destroy(
	GSM0710_Buffer* buf)
{
	free(buf);
}

/* Writes data to the buffer
 *
 * PARAMS
 * buf - pointer to the buffer
 * input - input data (in user memory)
 * length - how many characters should be written
 * RETURNS
 * number of characters written
 */
static int gsm0710_buffer_write(
	GSM0710_Buffer* buf,
	const unsigned char *input,
	int length)
{
	LOG(LOG_DEBUG, "Enter");
	int c = buf->endp - buf->writep;
	length = min(length, gsm0710_buffer_free(buf));
	if (length > c)
	{
		memcpy(buf->writep, input, c);
		memcpy(buf->data, input + c, length - c);
		buf->writep = buf->data + (length - c);
	}
	else
	{
		memcpy(buf->writep, input, length);
		buf->writep += length;
		if (buf->writep == buf->endp)
			buf->writep = buf->data;
	}
	LOG(LOG_DEBUG, "Leave");
	return length;
}

/**
 * destroys a frame
 */
static void destroy_frame(
	GSM0710_Frame * frame)
{
	if (frame->length > 0)
		free(frame->data);
	free(frame);
}

/* Gets a frame from buffer. You have to remember to free this frame
 * when it's not needed anymore
 *
 * PARAMS:
 * buf - the buffer, where the frame is extracted
 * RETURNS:
 * frame or null, if there isn't ready frame with given index
 */
static GSM0710_Frame* gsm0710_base_buffer_get_frame(
	GSM0710_Buffer * buf)
{
	int end;
	int length_needed = 5;// channel, type, length, fcs, flag
	unsigned char *data;
	unsigned char fcs = 0xFF;
	GSM0710_Frame *frame = NULL;
//Find start flag
	while (!buf->flag_found && gsm0710_buffer_length(buf) > 0)
	{
		if (*buf->readp == GSM0710_FRAME_FLAG)
			buf->flag_found = 1;
		gsm0710_buffer_inc(buf, buf->readp);
	}
	if (!buf->flag_found)// no frame started
		return NULL;
//skip empty frames (this causes troubles if we're using DLC 62)
	while (gsm0710_buffer_length(buf) > 0 && (*buf->readp == GSM0710_FRAME_FLAG))
	{
		gsm0710_buffer_inc(buf, buf->readp);
	}
	if (gsm0710_buffer_length(buf) >= length_needed)
	{
		data = buf->readp;
		if ((frame = (GSM0710_Frame*)malloc(sizeof(GSM0710_Frame))) != NULL)
		{
			frame->channel = ((*data & 252) >> 2);
			fcs = r_crctable[fcs ^ *data];
			gsm0710_buffer_inc(buf, data);
			frame->control = *data;
			fcs = r_crctable[fcs ^ *data];
			gsm0710_buffer_inc(buf, data);
			frame->length = (*data & 254) >> 1;
			fcs = r_crctable[fcs ^ *data];
		}
		else
			LOG(LOG_ALERT, "Out of memory, when allocating space for frame");
		if ((*data & 1) == 0)
		{
//Current spec (version 7.1.0) states these kind of
//frames to be invalid Long lost of sync might be
//caused if we would expect a long frame because of an
//error in length field.
			/*
			gsm0710_buffer_inc(buf,data);
			frame->length += (*data*128);
			fcs = r_crctable[fcs^*data];
			length_needed++;
			*/
			free(frame);
			buf->readp = data;
			buf->flag_found = 0;
			return gsm0710_base_buffer_get_frame(buf);
		}
		length_needed += frame->length;
		if (!(gsm0710_buffer_length(buf) >= length_needed))
		{
			free(frame);
			return NULL;
		}
		gsm0710_buffer_inc(buf, data);
//extract data
		if (frame->length > 0)
		{
			if ((frame->data = malloc(sizeof(char) * frame->length)) != NULL)