[libsigrok.git] / src / soft-trigger.c

/*
 * This file is part of the libsigrok project.
 *
 * Copyright (C) 2014 Bert Vermeulen <bert@biot.com>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * 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, see <http://www.gnu.org/licenses/>.
 */

#include <config.h>
#include <string.h>
#include <libsigrok/libsigrok.h>
#include "libsigrok-internal.h"

/* @cond PRIVATE */
#define LOG_PREFIX "soft-trigger"
/* @endcond */

static size_t logic_channel_unitsize(GSList *channels)
{
	size_t number = 0;
	struct sr_channel *channel;
	GSList *l;

	for (l = channels; l; l = l->next) {
		channel = l->data;
		if (channel->type == SR_CHANNEL_LOGIC)
			number += 1;
	}
	sr_dbg("number of logic channels: %d", (int) number);
	return (number + 7) / 8;
} 


SR_PRIV struct soft_trigger_logic *soft_trigger_logic_new(
		const struct sr_dev_inst *sdi, struct sr_trigger *trigger,
		int pre_trigger_samples)
{
	struct soft_trigger_logic *stl;

	stl = g_malloc0(sizeof(struct soft_trigger_logic));
	stl->sdi = sdi;
	stl->trigger = trigger;
	/* Retreive number of logic channels, unitsize */
	stl->unitsize = logic_channel_unitsize(sdi->channels);
	stl->prev_sample = g_malloc0(stl->unitsize);
	stl->pre_trigger_size = stl->unitsize * pre_trigger_samples;
	stl->pre_trigger_buffer = g_try_malloc(stl->pre_trigger_size);
	if (pre_trigger_samples > 0 && !stl->pre_trigger_buffer) {
		/*
		 * Error out if g_try_malloc() failed (or was invoked as
		 * g_try_malloc(0)) *and* more than 0 pretrigger samples
		 * were requested.
		 */
		soft_trigger_logic_free(stl);
		return NULL;
	}
	stl->pre_trigger_head = stl->pre_trigger_buffer;

	if (stl->pre_trigger_size > 0 && !stl->pre_trigger_buffer) {
		soft_trigger_logic_free(stl);
		return NULL;
	}

	return stl;
}

SR_PRIV void soft_trigger_logic_free(struct soft_trigger_logic *stl)
{
	g_free(stl->pre_trigger_buffer);
	g_free(stl->prev_sample);
	g_free(stl);
}

static void pre_trigger_append(struct soft_trigger_logic *stl,
		uint8_t *buf, int len)
{
	/* Avoid uselessly copying more than the pre-trigger size. */
	if (len > stl->pre_trigger_size) {
		buf += len - stl->pre_trigger_size;
		len = stl->pre_trigger_size;
	}

	/* Update the filling level of the pre-trigger circular buffer. */
	stl->pre_trigger_fill = MIN(stl->pre_trigger_fill + len,
	                            stl->pre_trigger_size);

	/* Actually copy data to the pre-trigger circular buffer. */
	while (len > 0) {
		size_t size = MIN(stl->pre_trigger_buffer + stl->pre_trigger_size
		                  - stl->pre_trigger_head, len);
		memcpy(stl->pre_trigger_head, buf, size);
		stl->pre_trigger_head += size;
		if (stl->pre_trigger_head >= stl->pre_trigger_buffer
		                             + stl->pre_trigger_size)
			stl->pre_trigger_head = stl->pre_trigger_buffer;
		buf += size;
		len -= size;
	}
}

static void pre_trigger_send(struct soft_trigger_logic *stl,
		int *pre_trigger_samples)
{
	struct sr_datafeed_packet packet;
	struct sr_datafeed_logic logic;

	packet.type = SR_DF_LOGIC;
	packet.payload = &logic;
	logic.unitsize = stl->unitsize;

	if (pre_trigger_samples)
		*pre_trigger_samples = 0;

	/* If pre-trigger buffer not full, rewind head to the first valid sample. */
	if (stl->pre_trigger_fill < stl->pre_trigger_size)
		stl->pre_trigger_head = stl->pre_trigger_buffer;

	/* Send logic packets for the pre-trigger circular buffer content. */
	while (stl->pre_trigger_fill > 0) {
		size_t size = MIN(stl->pre_trigger_buffer + stl->pre_trigger_size
		                  - stl->pre_trigger_head, stl->pre_trigger_fill);
		logic.length = size;
		logic.data = stl->pre_trigger_head;
		sr_session_send(stl->sdi, &packet);
		stl->pre_trigger_head = stl->pre_trigger_buffer;
		stl->pre_trigger_fill -= size;
		if (pre_trigger_samples)
			*pre_trigger_samples += size / stl->unitsize;
	}
}

static gboolean logic_check_match(struct soft_trigger_logic *stl,
		uint8_t *sample, struct sr_trigger_match *match)
{
	int bit, prev_bit;
	gboolean result;

	stl->count++;
	result = FALSE;
	bit = *(sample + match->channel->index / 8)
			& (1 << (match->channel->index % 8));
	if (match->match == SR_TRIGGER_ZERO)
		result = bit == 0;
	else if (match->match == SR_TRIGGER_ONE)
		result = bit != 0;
	else {
		/* Edge matches. */
		if (stl->count == 1)
			/* First sample, don't have enough for an edge match yet. */
			return FALSE;
		prev_bit = *(stl->prev_sample + match->channel->index / 8)
				& (1 << (match->channel->index % 8));
		if (match->match == SR_TRIGGER_RISING)
			result = prev_bit == 0 && bit != 0;
		else if (match->match == SR_TRIGGER_FALLING)
			result = prev_bit != 0 && bit == 0;
		else if (match->match == SR_TRIGGER_EDGE)
			result = prev_bit != bit;
	}

	return result;
}

/* Returns the offset (in samples) within buf of where the trigger
 * occurred, or -1 if not triggered. */
SR_PRIV int soft_trigger_logic_check(struct soft_trigger_logic *stl,
		uint8_t *buf, int len, int *pre_trigger_samples)
{
	struct sr_datafeed_packet packet;
	struct sr_trigger_stage *stage;
	struct sr_trigger_match *match;
	GSList *l, *l_stage;
	int offset;
	int i;
	gboolean match_found;

	offset = -1;
	for (i = 0; i < len; i += stl->unitsize) {
		l_stage = g_slist_nth(stl->trigger->stages, stl->cur_stage);
		stage = l_stage->data;
		if (!stage->matches)
			/* No matches supplied, client error. */
			return SR_ERR_ARG;

		match_found = TRUE;
		for (l = stage->matches; l; l = l->next) {
			match = l->data;
			if (!match->channel->enabled)
				/* Ignore disabled channels with a trigger. */
				continue;
			if (!logic_check_match(stl, buf + i, match)) {
				match_found = FALSE;
				break;
			}
		}
		memcpy(stl->prev_sample, buf + i, stl->unitsize);
		if (match_found) {
			/* Matched on the current stage. */
			if (l_stage->next) {
				/* Advance to next stage. */
				stl->cur_stage++;
			} else {
				/* Matched on last stage, send pre-trigger data. */
				pre_trigger_append(stl, buf, i);
				pre_trigger_send(stl, pre_trigger_samples);

				/* Fire trigger. */
				offset = i / stl->unitsize;

				packet.type = SR_DF_TRIGGER;
				packet.payload = NULL;
				sr_session_send(stl->sdi, &packet);
				break;
			}
		} else if (stl->cur_stage > 0) {
			/*
			 * We had a match at an earlier stage, but failed on the
			 * current stage. However, we may have a match on this
			 * stage in the next bit -- trigger on 0001 will fail on
			 * seeing 00001, so we need to go back to stage 0 -- but
			 * at the next sample from the one that matched originally,
			 * which the counter increment at the end of the loop
			 * takes care of.
			 */
			i -= stl->cur_stage * stl->unitsize;
			if (i < -1)
				i = -1; /* Oops, went back past this buffer. */
			/* Reset trigger stage. */
			stl->cur_stage = 0;
		}
	}

	if (offset == -1)
		pre_trigger_append(stl, buf, len);

	return offset;
}
Commit	Line	Data
ac136b57 BV	1	/*
	2	* This file is part of the libsigrok project.
	3	*
	4	* Copyright (C) 2014 Bert Vermeulen <bert@biot.com>
	5	*
	6	* This program is free software: you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License as published by
	8	* the Free Software Foundation, either version 3 of the License, or
	9	* (at your option) any later version.
	10	*
	11	* This program is distributed in the hope that it will be useful,
	12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	14	* GNU General Public License for more details.
	15	*
	16	* You should have received a copy of the GNU General Public License
	17	* along with this program. If not, see <http://www.gnu.org/licenses/>.
	18	*/
	19
6ec6c43b	20	#include <config.h>
ac136b57	21	#include <string.h>
c1aae900	22	#include <libsigrok/libsigrok.h>
ac136b57 BV	23	#include "libsigrok-internal.h"
	24
	25	/* @cond PRIVATE */
	26	#define LOG_PREFIX "soft-trigger"
	27	/* @endcond */
	28
6fc51fb1	29	static size_t logic_channel_unitsize(GSList *channels)
	30	{
	31	size_t number = 0;
	32	struct sr_channel *channel;
	33	GSList *l;
	34
	35	for (l = channels; l; l = l->next) {
	36	channel = l->data;
	37	if (channel->type == SR_CHANNEL_LOGIC)
	38	number += 1;
	39	}
	40	sr_dbg("number of logic channels: %d", (int) number);
	41	return (number + 7) / 8;
	42	}
	43
	44
ac136b57	45	SR_PRIV struct soft_trigger_logic *soft_trigger_logic_new(
fe5a7355 AJ	46	const struct sr_dev_inst sdi, struct sr_trigger trigger,
fe5a7355 AJ	47	int pre_trigger_samples)
ac136b57 BV	48	{
	49	struct soft_trigger_logic *stl;
	50
	51	stl = g_malloc0(sizeof(struct soft_trigger_logic));
	52	stl->sdi = sdi;
	53	stl->trigger = trigger;
6fc51fb1	54	/* Retreive number of logic channels, unitsize */
6fc51fb1	55	stl->unitsize = logic_channel_unitsize(sdi->channels);
ac136b57	56	stl->prev_sample = g_malloc0(stl->unitsize);
fe5a7355	57	stl->pre_trigger_size = stl->unitsize * pre_trigger_samples;
d52107a1	58	stl->pre_trigger_buffer = g_try_malloc(stl->pre_trigger_size);
eca97729 UH	59	if (pre_trigger_samples > 0 && !stl->pre_trigger_buffer) {
	60	/*
	61	* Error out if g_try_malloc() failed (or was invoked as
	62	* g_try_malloc(0)) and more than 0 pretrigger samples
	63	* were requested.
	64	*/
d52107a1 UH	65	soft_trigger_logic_free(stl);
	66	return NULL;
	67	}
fe5a7355 AJ	68	stl->pre_trigger_head = stl->pre_trigger_buffer;
fe5a7355 AJ	69
98fec29e	70	if (stl->pre_trigger_size > 0 && !stl->pre_trigger_buffer) {
fe5a7355 AJ	71	soft_trigger_logic_free(stl);
	72	return NULL;
	73	}
ac136b57 BV	74
	75	return stl;
	76	}
	77
	78	SR_PRIV void soft_trigger_logic_free(struct soft_trigger_logic *stl)
	79	{
fe5a7355	80	g_free(stl->pre_trigger_buffer);
ac136b57 BV	81	g_free(stl->prev_sample);
	82	g_free(stl);
	83	}
	84
fe5a7355 AJ	85	static void pre_trigger_append(struct soft_trigger_logic *stl,
	86	uint8_t *buf, int len)
	87	{
	88	/* Avoid uselessly copying more than the pre-trigger size. */
	89	if (len > stl->pre_trigger_size) {
	90	buf += len - stl->pre_trigger_size;
	91	len = stl->pre_trigger_size;
	92	}
	93
	94	/* Update the filling level of the pre-trigger circular buffer. */
	95	stl->pre_trigger_fill = MIN(stl->pre_trigger_fill + len,
	96	stl->pre_trigger_size);
	97
	98	/* Actually copy data to the pre-trigger circular buffer. */
	99	while (len > 0) {
	100	size_t size = MIN(stl->pre_trigger_buffer + stl->pre_trigger_size
	101	- stl->pre_trigger_head, len);
	102	memcpy(stl->pre_trigger_head, buf, size);
	103	stl->pre_trigger_head += size;
	104	if (stl->pre_trigger_head >= stl->pre_trigger_buffer
	105	+ stl->pre_trigger_size)
	106	stl->pre_trigger_head = stl->pre_trigger_buffer;
	107	buf += size;
	108	len -= size;
	109	}
	110	}
	111
	112	static void pre_trigger_send(struct soft_trigger_logic *stl,
	113	int *pre_trigger_samples)
	114	{
	115	struct sr_datafeed_packet packet;
	116	struct sr_datafeed_logic logic;
	117
	118	packet.type = SR_DF_LOGIC;
	119	packet.payload = &logic;
	120	logic.unitsize = stl->unitsize;
	121
	122	if (pre_trigger_samples)
	123	*pre_trigger_samples = 0;
	124
	125	/* If pre-trigger buffer not full, rewind head to the first valid sample. */
	126	if (stl->pre_trigger_fill < stl->pre_trigger_size)
	127	stl->pre_trigger_head = stl->pre_trigger_buffer;
	128
	129	/* Send logic packets for the pre-trigger circular buffer content. */
	130	while (stl->pre_trigger_fill > 0) {
	131	size_t size = MIN(stl->pre_trigger_buffer + stl->pre_trigger_size
	132	- stl->pre_trigger_head, stl->pre_trigger_fill);
	133	logic.length = size;
	134	logic.data = stl->pre_trigger_head;
	135	sr_session_send(stl->sdi, &packet);
	136	stl->pre_trigger_head = stl->pre_trigger_buffer;
	137	stl->pre_trigger_fill -= size;
	138	if (pre_trigger_samples)
	139	*pre_trigger_samples += size / stl->unitsize;
	140	}
	141	}
	142
ac136b57 BV	143	static gboolean logic_check_match(struct soft_trigger_logic *stl,
	144	uint8_t sample, struct sr_trigger_match match)
	145	{
	146	int bit, prev_bit;
	147	gboolean result;
	148
	149	stl->count++;
	150	result = FALSE;
	151	bit = *(sample + match->channel->index / 8)
	152	& (1 << (match->channel->index % 8));
	153	if (match->match == SR_TRIGGER_ZERO)
	154	result = bit == 0;
	155	else if (match->match == SR_TRIGGER_ONE)
	156	result = bit != 0;
	157	else {
	158	/* Edge matches. */
	159	if (stl->count == 1)
	160	/* First sample, don't have enough for an edge match yet. */
	161	return FALSE;
	162	prev_bit = *(stl->prev_sample + match->channel->index / 8)
	163	& (1 << (match->channel->index % 8));
	164	if (match->match == SR_TRIGGER_RISING)
	165	result = prev_bit == 0 && bit != 0;
	166	else if (match->match == SR_TRIGGER_FALLING)
	167	result = prev_bit != 0 && bit == 0;
	168	else if (match->match == SR_TRIGGER_EDGE)
	169	result = prev_bit != bit;
	170	}
	171
	172	return result;
	173	}
	174
	175	/* Returns the offset (in samples) within buf of where the trigger
	176	* occurred, or -1 if not triggered. */
	177	SR_PRIV int soft_trigger_logic_check(struct soft_trigger_logic *stl,
fe5a7355	178	uint8_t buf, int len, int pre_trigger_samples)
ac136b57 BV	179	{
	180	struct sr_datafeed_packet packet;
	181	struct sr_trigger_stage *stage;
	182	struct sr_trigger_match *match;
	183	GSList l, l_stage;
	184	int offset;
	185	int i;
	186	gboolean match_found;
	187
	188	offset = -1;
	189	for (i = 0; i < len; i += stl->unitsize) {
	190	l_stage = g_slist_nth(stl->trigger->stages, stl->cur_stage);
	191	stage = l_stage->data;
	192	if (!stage->matches)
	193	/* No matches supplied, client error. */
	194	return SR_ERR_ARG;
	195
	196	match_found = TRUE;
	197	for (l = stage->matches; l; l = l->next) {
	198	match = l->data;
	199	if (!match->channel->enabled)
	200	/* Ignore disabled channels with a trigger. */
	201	continue;
	202	if (!logic_check_match(stl, buf + i, match)) {
	203	match_found = FALSE;
	204	break;
	205	}
	206	}
	207	memcpy(stl->prev_sample, buf + i, stl->unitsize);
	208	if (match_found) {
	209	/* Matched on the current stage. */
	210	if (l_stage->next) {
	211	/* Advance to next stage. */
	212	stl->cur_stage++;
	213	} else {
fe5a7355 AJ	214	/* Matched on last stage, send pre-trigger data. */
	215	pre_trigger_append(stl, buf, i);
	216	pre_trigger_send(stl, pre_trigger_samples);
	217
	218	/* Fire trigger. */
ac136b57 BV	219	offset = i / stl->unitsize;
	220
	221	packet.type = SR_DF_TRIGGER;
	222	packet.payload = NULL;
	223	sr_session_send(stl->sdi, &packet);
	224	break;
	225	}
	226	} else if (stl->cur_stage > 0) {
	227	/*
	228	* We had a match at an earlier stage, but failed on the
	229	* current stage. However, we may have a match on this
	230	* stage in the next bit -- trigger on 0001 will fail on
	231	* seeing 00001, so we need to go back to stage 0 -- but
	232	* at the next sample from the one that matched originally,
	233	* which the counter increment at the end of the loop
	234	* takes care of.
	235	*/
	236	i -= stl->cur_stage * stl->unitsize;
	237	if (i < -1)
	238	i = -1; /* Oops, went back past this buffer. */
	239	/* Reset trigger stage. */
	240	stl->cur_stage = 0;
	241	}
	242	}
	243
fe5a7355 AJ	244	if (offset == -1)
	245	pre_trigger_append(stl, buf, len);
	246
ac136b57 BV	247	return offset;
ac136b57 BV	248	}