1/* $FreeBSD$ */
2
3/*-
4 * Copyright (c) 2011 Hans Petter Selasky. All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 *    notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 *    notice, this list of conditions and the following disclaimer in the
13 *    documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 */
27
28#include <stdio.h>
29#include <stdint.h>
30#include <stdlib.h>
31#include <unistd.h>
32#include <sysexits.h>
33#include <err.h>
34#include <string.h>
35
36#include <sys/queue.h>
37
38#include "bus_sections.h"
39
40#define	MAX_STRING	64
41
42struct format_info;
43typedef TAILQ_HEAD(,format_info) format_info_head_t;
44typedef TAILQ_ENTRY(format_info) format_info_entry_t;
45
46static format_info_head_t format_head = TAILQ_HEAD_INITIALIZER(format_head);
47
48struct format_info {
49	format_info_entry_t entry;
50	format_info_head_t fields;
51	char	name[MAX_STRING];
52	uint16_t bit_offset;
53	uint16_t bit_size;
54};
55
56static struct format_info *
57format_info_new(char *pstr, uint16_t bo, uint16_t bs)
58{
59	struct format_info *pfi;
60
61	pfi = malloc(sizeof(*pfi));
62	if (pfi == NULL)
63		errx(EX_SOFTWARE, "Out of memory.");
64
65	memset(pfi, 0, sizeof(*pfi));
66
67	TAILQ_INIT(&pfi->fields);
68
69	strlcpy(pfi->name, pstr, sizeof(pfi->name));
70	pfi->bit_offset = bo;
71	pfi->bit_size = bs;
72	return (pfi);
73}
74
75static const struct format_info *
76format_get_section(const char *section)
77{
78	const struct format_info *psub;
79	static const struct format_info *psub_last;
80	static const char *psub_cache;
81
82	if (psub_cache && strcmp(psub_cache, section) == 0)
83		return (psub_last);
84
85	TAILQ_FOREACH(psub, &format_head, entry) {
86		if (strcmp(section, psub->name) == 0) {
87			psub_cache = section;
88			psub_last = psub;
89			return (psub);
90		}
91	}
92	warnx("Section '%s' not found", section);
93	psub_cache = section;
94	psub_last = psub;
95	return (NULL);
96}
97
98uint16_t
99format_get_section_size(const char *section)
100{
101	const struct format_info *pfi;
102
103	pfi = format_get_section(section);
104	if (pfi == NULL)
105		return (0);
106
107	return ((pfi->bit_offset + 7) / 8);
108}
109
110
111uint8_t
112format_get_field(const char *section, const char *field,
113    const uint8_t *ptr, uint16_t size)
114{
115	const struct format_info *pfi;
116	const struct format_info *psub;
117	uint16_t rem;
118	uint16_t off;
119	uint16_t sz;
120
121	pfi = format_get_section(section);
122	if (pfi == NULL)
123		return (0);
124
125	/* skip until we find the fields */
126	while (pfi && TAILQ_FIRST(&pfi->fields) == NULL)
127		pfi = TAILQ_NEXT(pfi, entry);
128
129	if (pfi == NULL)
130		return (0);
131
132	TAILQ_FOREACH(psub, &pfi->fields, entry) {
133		if (strcmp(field, psub->name) == 0) {
134
135			/* range check */
136			if (((psub->bit_offset + psub->bit_size) / 8) > size)
137				return (0);
138
139			/* compute byte offset */
140			rem = psub->bit_offset & 7;
141			off = psub->bit_offset / 8;
142			sz = psub->bit_size;
143
144			/* extract bit-field */
145			return ((ptr[off] >> rem) & ((1 << sz) - 1));
146		}
147	}
148	warnx("Field '%s' not found in '%s'", field, pfi->name);
149	return (0);
150}
151
152void
153format_parse_entries(const uint8_t *ptr, uint32_t len)
154{
155	static const char *command_list = "012345678:";
156	const char *cmd;
157	struct format_info *pfi;
158	struct format_info *pfi_last = NULL;
159	char linebuf[3][MAX_STRING];
160	uint32_t off = 0;
161	uint16_t bit_offset = 0;
162	uint8_t state = 0;
163	uint8_t cmd_index;
164	int c;
165
166	/*
167	 * The format we are parsing:
168	 * <string>{string,string}<next_string>{...}
169	 */
170	while (len--) {
171		c = *(ptr++);
172
173		/* skip some characters */
174		if (c == 0 || c == '\n' || c == '\r' || c == ' ' || c == '\t')
175			continue;
176
177		/* accumulate non-field delimiters */
178		if (strchr("{,}", c) == NULL) {
179			if (off < (MAX_STRING - 1)) {
180				linebuf[state][off] = c;
181				off++;
182			}
183			continue;
184		}
185		/* parse keyword */
186		linebuf[state][off] = 0;
187		off = 0;
188		state++;
189		if (state == 3) {
190			/* check for command in command list */
191			cmd = strchr(command_list, linebuf[2][0]);
192			if (cmd != NULL)
193				cmd_index = cmd - command_list;
194			else
195				cmd_index = 255;
196
197			/*
198			 * Check for new field, format is:
199			 *
200			 * <field_name>{bit_offset_xor, bit_size}
201			 */
202			if (cmd_index < 9 && pfi_last != NULL) {
203				pfi = format_info_new(linebuf[0], bit_offset ^
204				    atoi(linebuf[1]), cmd_index);
205				TAILQ_INSERT_TAIL(&pfi_last->fields, pfi, entry);
206				bit_offset += cmd_index;
207			}
208			/*
209			 * Check for new section, format is:
210			 *
211			 * <section_name>{section_bit_size, :}
212			 */
213			if (cmd_index == 9) {
214				pfi_last = format_info_new(linebuf[0],
215				    atoi(linebuf[1]), cmd_index);
216				TAILQ_INSERT_TAIL(&format_head, pfi_last, entry);
217				bit_offset = 0;
218			}
219			state = 0;
220			continue;
221		}
222	}
223}
224