1223534Shselasky/* $FreeBSD$ */ 2223534Shselasky 3223534Shselasky/*- 4223534Shselasky * Copyright (c) 2011 Hans Petter Selasky. All rights reserved. 5223534Shselasky * 6223534Shselasky * Redistribution and use in source and binary forms, with or without 7223534Shselasky * modification, are permitted provided that the following conditions 8223534Shselasky * are met: 9223534Shselasky * 1. Redistributions of source code must retain the above copyright 10223534Shselasky * notice, this list of conditions and the following disclaimer. 11223534Shselasky * 2. Redistributions in binary form must reproduce the above copyright 12223534Shselasky * notice, this list of conditions and the following disclaimer in the 13223534Shselasky * documentation and/or other materials provided with the distribution. 14223534Shselasky * 15223534Shselasky * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 16223534Shselasky * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17223534Shselasky * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18223534Shselasky * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 19223534Shselasky * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20223534Shselasky * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 21223534Shselasky * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22223534Shselasky * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 23223534Shselasky * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 24223534Shselasky * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 25223534Shselasky * SUCH DAMAGE. 26223534Shselasky */ 27223534Shselasky 28223534Shselasky#include <stdio.h> 29223534Shselasky#include <stdint.h> 30223534Shselasky#include <stdlib.h> 31223534Shselasky#include <unistd.h> 32223534Shselasky#include <sysexits.h> 33223534Shselasky#include <err.h> 34223534Shselasky#include <string.h> 35223534Shselasky 36223534Shselasky#include <sys/queue.h> 37223534Shselasky 38223534Shselasky#include "bus_sections.h" 39223534Shselasky 40223534Shselasky#define MAX_STRING 64 41223534Shselasky 42223534Shselaskystruct format_info; 43223534Shselaskytypedef TAILQ_HEAD(,format_info) format_info_head_t; 44223534Shselaskytypedef TAILQ_ENTRY(format_info) format_info_entry_t; 45223534Shselasky 46223534Shselaskystatic format_info_head_t format_head = TAILQ_HEAD_INITIALIZER(format_head); 47223534Shselasky 48223534Shselaskystruct format_info { 49223534Shselasky format_info_entry_t entry; 50223534Shselasky format_info_head_t fields; 51223534Shselasky char name[MAX_STRING]; 52223534Shselasky uint16_t bit_offset; 53223534Shselasky uint16_t bit_size; 54223534Shselasky}; 55223534Shselasky 56223534Shselaskystatic struct format_info * 57223534Shselaskyformat_info_new(char *pstr, uint16_t bo, uint16_t bs) 58223534Shselasky{ 59223534Shselasky struct format_info *pfi; 60223534Shselasky 61223534Shselasky pfi = malloc(sizeof(*pfi)); 62223534Shselasky if (pfi == NULL) 63223534Shselasky errx(EX_SOFTWARE, "Out of memory."); 64223534Shselasky 65223534Shselasky memset(pfi, 0, sizeof(*pfi)); 66223534Shselasky 67223534Shselasky TAILQ_INIT(&pfi->fields); 68223534Shselasky 69223534Shselasky strlcpy(pfi->name, pstr, sizeof(pfi->name)); 70223534Shselasky pfi->bit_offset = bo; 71223534Shselasky pfi->bit_size = bs; 72223534Shselasky return (pfi); 73223534Shselasky} 74223534Shselasky 75223534Shselaskystatic const struct format_info * 76223534Shselaskyformat_get_section(const char *section) 77223534Shselasky{ 78223534Shselasky const struct format_info *psub; 79223534Shselasky static const struct format_info *psub_last; 80223534Shselasky static const char *psub_cache; 81223534Shselasky 82223534Shselasky if (psub_cache && strcmp(psub_cache, section) == 0) 83223534Shselasky return (psub_last); 84223534Shselasky 85223534Shselasky TAILQ_FOREACH(psub, &format_head, entry) { 86223534Shselasky if (strcmp(section, psub->name) == 0) { 87223534Shselasky psub_cache = section; 88223534Shselasky psub_last = psub; 89223534Shselasky return (psub); 90223534Shselasky } 91223534Shselasky } 92223534Shselasky warnx("Section '%s' not found", section); 93223534Shselasky psub_cache = section; 94223534Shselasky psub_last = psub; 95223534Shselasky return (NULL); 96223534Shselasky} 97223534Shselasky 98223534Shselaskyuint16_t 99223534Shselaskyformat_get_section_size(const char *section) 100223534Shselasky{ 101223534Shselasky const struct format_info *pfi; 102223534Shselasky 103223534Shselasky pfi = format_get_section(section); 104223534Shselasky if (pfi == NULL) 105223534Shselasky return (0); 106223534Shselasky 107223534Shselasky return ((pfi->bit_offset + 7) / 8); 108223534Shselasky} 109223534Shselasky 110223534Shselasky 111223534Shselaskyuint8_t 112223534Shselaskyformat_get_field(const char *section, const char *field, 113223534Shselasky const uint8_t *ptr, uint16_t size) 114223534Shselasky{ 115223534Shselasky const struct format_info *pfi; 116223534Shselasky const struct format_info *psub; 117223534Shselasky uint16_t rem; 118223534Shselasky uint16_t off; 119223534Shselasky uint16_t sz; 120223534Shselasky 121223534Shselasky pfi = format_get_section(section); 122223534Shselasky if (pfi == NULL) 123223534Shselasky return (0); 124223534Shselasky 125223534Shselasky /* skip until we find the fields */ 126223534Shselasky while (pfi && TAILQ_FIRST(&pfi->fields) == NULL) 127223534Shselasky pfi = TAILQ_NEXT(pfi, entry); 128223534Shselasky 129223534Shselasky if (pfi == NULL) 130223534Shselasky return (0); 131223534Shselasky 132223534Shselasky TAILQ_FOREACH(psub, &pfi->fields, entry) { 133223534Shselasky if (strcmp(field, psub->name) == 0) { 134223534Shselasky 135223534Shselasky /* range check */ 136223534Shselasky if (((psub->bit_offset + psub->bit_size) / 8) > size) 137223534Shselasky return (0); 138223534Shselasky 139223534Shselasky /* compute byte offset */ 140223534Shselasky rem = psub->bit_offset & 7; 141223534Shselasky off = psub->bit_offset / 8; 142223534Shselasky sz = psub->bit_size; 143223534Shselasky 144223534Shselasky /* extract bit-field */ 145223534Shselasky return ((ptr[off] >> rem) & ((1 << sz) - 1)); 146223534Shselasky } 147223534Shselasky } 148223534Shselasky warnx("Field '%s' not found in '%s'", field, pfi->name); 149223534Shselasky return (0); 150223534Shselasky} 151223534Shselasky 152223534Shselaskyvoid 153223534Shselaskyformat_parse_entries(const uint8_t *ptr, uint32_t len) 154223534Shselasky{ 155223534Shselasky static const char *command_list = "012345678:"; 156223534Shselasky const char *cmd; 157223534Shselasky struct format_info *pfi; 158223534Shselasky struct format_info *pfi_last = NULL; 159223534Shselasky char linebuf[3][MAX_STRING]; 160223534Shselasky uint32_t off = 0; 161223534Shselasky uint16_t bit_offset = 0; 162223534Shselasky uint8_t state = 0; 163223534Shselasky uint8_t cmd_index; 164223534Shselasky int c; 165223534Shselasky 166223534Shselasky /* 167223534Shselasky * The format we are parsing: 168223534Shselasky * <string>{string,string}<next_string>{...} 169223534Shselasky */ 170223534Shselasky while (len--) { 171223534Shselasky c = *(ptr++); 172223534Shselasky 173223534Shselasky /* skip some characters */ 174223534Shselasky if (c == 0 || c == '\n' || c == '\r' || c == ' ' || c == '\t') 175223534Shselasky continue; 176223534Shselasky 177223534Shselasky /* accumulate non-field delimiters */ 178223534Shselasky if (strchr("{,}", c) == NULL) { 179223534Shselasky if (off < (MAX_STRING - 1)) { 180223534Shselasky linebuf[state][off] = c; 181223534Shselasky off++; 182223534Shselasky } 183223534Shselasky continue; 184223534Shselasky } 185223534Shselasky /* parse keyword */ 186223534Shselasky linebuf[state][off] = 0; 187223534Shselasky off = 0; 188223534Shselasky state++; 189223534Shselasky if (state == 3) { 190223534Shselasky /* check for command in command list */ 191223534Shselasky cmd = strchr(command_list, linebuf[2][0]); 192223534Shselasky if (cmd != NULL) 193223534Shselasky cmd_index = cmd - command_list; 194223534Shselasky else 195223534Shselasky cmd_index = 255; 196223534Shselasky 197223534Shselasky /* 198223534Shselasky * Check for new field, format is: 199223534Shselasky * 200223534Shselasky * <field_name>{bit_offset_xor, bit_size} 201223534Shselasky */ 202223534Shselasky if (cmd_index < 9 && pfi_last != NULL) { 203223534Shselasky pfi = format_info_new(linebuf[0], bit_offset ^ 204223534Shselasky atoi(linebuf[1]), cmd_index); 205223534Shselasky TAILQ_INSERT_TAIL(&pfi_last->fields, pfi, entry); 206223534Shselasky bit_offset += cmd_index; 207223534Shselasky } 208223534Shselasky /* 209223534Shselasky * Check for new section, format is: 210223534Shselasky * 211223534Shselasky * <section_name>{section_bit_size, :} 212223534Shselasky */ 213223534Shselasky if (cmd_index == 9) { 214223534Shselasky pfi_last = format_info_new(linebuf[0], 215223534Shselasky atoi(linebuf[1]), cmd_index); 216223534Shselasky TAILQ_INSERT_TAIL(&format_head, pfi_last, entry); 217223534Shselasky bit_offset = 0; 218223534Shselasky } 219223534Shselasky state = 0; 220223534Shselasky continue; 221223534Shselasky } 222223534Shselasky } 223223534Shselasky} 224