static bool probe_packet_resize(probe_t * probe, size_t size)
{
size_t offset = 0, // offset between the begining of the packet and the current position
i, num_layers = probe_get_num_layers(probe);
layer_t * layer;
uint8_t * segment;
if (!packet_resize(probe->packet, size)) {
return false;
}
// TODO update bitfield
// Update each layer's segment
for (i = 0; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
segment = packet_get_bytes(probe->packet) + offset;
layer_set_segment(layer, segment);
if (layer->protocol) {
// We're in a layer related to a protocol. Update "length" field (if any).
// It concerns: ipv4, ipv6, udp but not tcp, icmpv4, icmpv6
layer_set_field_and_free(layer, I16("length", size - offset));
offset += layer->segment_size;
}
}
return true;
}
bool probe_set_metafield_ext(probe_t * probe, size_t depth, field_t * field)
{
bool ret = true;
field_t * hacked_field;
// TODO: TEMP HACK IPv4 flow id is encoded in src_port
if (strcmp(field->key, "flow_id") != 0) {
fprintf(stderr, "probe_set_metafield_ext: cannot set %s\n", field->key);
return false;
}
// We add 24000 to use port to increase chances to traverse firewalls
if ((hacked_field = I16("src_port", 24000 + field->value.int16))) {
ret = probe_set_field(probe, hacked_field);
field_free(hacked_field);
}
return ret;
/*
metafield = metafield_search(field->key);
if (!metafield) return false; // Metafield not found
// Does the probe verifies one metafield pattern ?
// Does the value conflict with a previously set field ?
*/
}
void code2(int mode, LPSYMBOL s1, LPSYMBOL s2)
{
switch (mode) {
case AM_R8:
R8(s1->instr, s2->val8);
break;
case AM_R16:
R16(s1->instr, s2->val8);
break;
case AM_M8:
M8(s1->instr, s2->val8);
break;
case AM_M16:
M16(s1->instr, s2->val8);
break;
case AM_A8:
A8(s1->instr, s2->val16);
break;
case AM_A16:
A16(s1->instr, s2->val16);
break;
case AM_I16:
I16(s1->instr, s2->val16);
break;
case AM_I8:
I8(s1->instr, s2->val8);
break;
default:
break;
}
}
static UINT32 opMOVEA(void) // movea imm16, reg1, reg2
{
UINT32 op1=GETREG(GET1);
UINT32 op2=R_OP(PC);
PC+=2;
op2=I16(op2);
SETREG(GET2,op1+op2);
return clkIF;
}
static bool probe_set_tag(probe_t * probe, uint16_t tag_probe) {
bool ret = false;
field_t * field;
if ((field = I16("checksum", tag_probe))) {
ret = probe_set_field_ext(probe, 1, field);
field_free(field);
}
return ret;
}
static UINT32 opADDI(void) // addi imm16, reg1, reg2
{
UINT32 op1=GETREG(GET1);
UINT32 op2=R_OP(PC);
UINT64 res;
PC+=2;
op2=I16(op2);
res=(UINT64)op2+(UINT64)op1;
CHECK_CY(res);
CHECK_OVADD(op1,op2,res);
CHECK_ZS(res);
SETREG(GET2,res);
return clkIF;
}
u_char
*pnp_scan_resources(device_t dev, u_char *resources, int len,
struct isa_config *config, int ldn, pnp_scan_cb *cb)
{
u_char *p;
u_char tag;
int l;
p = resources;
while (len > 0) {
tag = *p++;
len--;
if (PNP_RES_TYPE(tag) == 0) {
/* small resource */
l = PNP_SRES_LEN(tag);
if (len < l)
break;
if ((*cb)(dev, tag, p, l, config, ldn))
return (p + l);
if (PNP_SRES_NUM(tag) == PNP_TAG_END)
return (p + l);
} else {
/* large resource */
if (len < 2)
break;
l = I16(p);
p += 2;
len -= 2;
if (len < l)
break;
if ((*cb)(dev, tag, p, l, config, ldn))
return (p + l);
}
p += l;
len -= l;
}
return NULL;
}
static bool probe_update_length(probe_t * probe)
{
bool ret = true;
size_t i, offset,
num_layers = probe_get_num_layers(probe),
packet_size = probe_get_size(probe);
layer_t * layer;
for (i = 0, offset = 0; i < num_layers; i++) {
layer = probe_get_layer(probe, i);
if (layer->protocol) {
// Update 'length' field (if any)
// This protocol field must always corresponds to the size of the
// header + its contents.
layer_set_field_and_free(layer, I16("length", packet_size - offset));
offset += layer->protocol->get_header_size(layer->segment);
} else {
// Update payload size
layer_set_segment_size(layer, packet_size - offset);
}
}
return ret;
}
int main(int argc, char ** argv)
{
int exit_code = EXIT_FAILURE;
char * version = strdup("version 1.0");
const char * usage = "usage: %s [options] host\n";
void * algorithm_options;
traceroute_options_t traceroute_options;
traceroute_options_t * ptraceroute_options;
mda_options_t mda_options;
probe_t * probe;
pt_loop_t * loop;
int family;
address_t dst_addr;
options_t * options;
char * dst_ip;
const char * algorithm_name;
const char * protocol_name;
bool use_icmp, use_udp, use_tcp;
// Prepare the commande line options
if (!(options = init_options(version))) {
fprintf(stderr, "E: Can't initialize options\n");
goto ERR_INIT_OPTIONS;
}
// Retrieve values passed in the command-line
if (options_parse(options, usage, argv) != 1) {
fprintf(stderr, "%s: destination required\n", basename(argv[0]));
goto ERR_OPT_PARSE;
}
// We assume that the target IP address is always the last argument
dst_ip = argv[argc - 1];
algorithm_name = algorithm_names[0];
protocol_name = protocol_names[0];
// Checking if there is any conflicts between options passed in the commandline
if (!check_options(is_icmp, is_tcp, is_udp, is_ipv4, is_ipv6, dst_port[3], src_port[3], protocol_name, algorithm_name)) {
goto ERR_CHECK_OPTIONS;
}
use_icmp = is_icmp || strcmp(protocol_name, "icmp") == 0;
use_tcp = is_tcp || strcmp(protocol_name, "tcp") == 0;
use_udp = is_udp || strcmp(protocol_name, "udp") == 0;
// If not any ip version is set, call address_guess_family.
// If only one is set to true, set family to AF_INET or AF_INET6
if (is_ipv4) {
family = AF_INET;
} else if (is_ipv6) {
family = AF_INET6;
} else {
// Get address family if not defined by the user
if (!address_guess_family(dst_ip, &family)) goto ERR_ADDRESS_GUESS_FAMILY;
}
// Translate the string IP / FQDN into an address_t * instance
if (address_from_string(family, dst_ip, &dst_addr) != 0) {
fprintf(stderr, "E: Invalid destination address %s\n", dst_ip);
goto ERR_ADDRESS_IP_FROM_STRING;
}
// Probe skeleton definition: IPv4/UDP probe targetting 'dst_ip'
if (!(probe = probe_create())) {
fprintf(stderr,"E: Cannot create probe skeleton");
goto ERR_PROBE_CREATE;
}
// Prepare the probe skeleton
probe_set_protocols(
probe,
get_ip_protocol_name(family), // "ipv4" | "ipv6"
get_protocol_name(family, use_icmp, use_tcp, use_udp), // "icmpv4" | "icmpv6" | "tcp" | "udp"
NULL
);
probe_set_field(probe, ADDRESS("dst_ip", &dst_addr));
if (send_time[3]) {
if(send_time[0] <= 10) { // seconds
probe_set_delay(probe, DOUBLE("delay", send_time[0]));
} else { // milli-seconds
probe_set_delay(probe, DOUBLE("delay", 0.001 * send_time[0]));
}
}
// ICMPv* do not support src_port and dst_port fields nor payload.
if (!use_icmp) {
uint16_t sport = 0,
dport = 0;
if (use_udp) {
// Option -U sets port to 53 (DNS) if dst_port is not explicitely set
sport = src_port[3] ? src_port[0] : UDP_DEFAULT_SRC_PORT;
dport = dst_port[3] ? dst_port[0] : (is_udp ? UDP_DST_PORT_USING_U : UDP_DEFAULT_DST_PORT);
} else if (use_tcp) {
// Option -T sets port to 80 (http) if dst_port is not explicitely set
sport = src_port[3] ? src_port[0] : TCP_DEFAULT_SRC_PORT;
dport = dst_port[3] ? dst_port[0] : (is_tcp ? TCP_DST_PORT_USING_T : TCP_DEFAULT_DST_PORT);
}
// Update ports
probe_set_fields(
probe,
I16("src_port", sport),
I16("dst_port", dport),
NULL
);
// Resize payload (it will be use to set our customized checksum in the {TCP, UDP} layer)
probe_payload_resize(probe, 2);
}
// Algorithm options (dedicated options)
if (strcmp(algorithm_name, "paris-traceroute") == 0) {
traceroute_options = traceroute_get_default_options();
ptraceroute_options = &traceroute_options;
algorithm_options = &traceroute_options;
algorithm_name = "traceroute";
} else if ((strcmp(algorithm_name, "mda") == 0) || options_mda_get_is_set()) {
mda_options = mda_get_default_options();
ptraceroute_options = &mda_options.traceroute_options;
algorithm_options = &mda_options;
options_mda_init(&mda_options);
} else {
fprintf(stderr, "E: Unknown algorithm");
goto ERR_UNKNOWN_ALGORITHM;
}
// Algorithm options (common options)
options_traceroute_init(ptraceroute_options, &dst_addr);
// Create libparistraceroute loop
if (!(loop = pt_loop_create(loop_handler, NULL))) {
fprintf(stderr, "E: Cannot create libparistraceroute loop");
goto ERR_LOOP_CREATE;
}
// Set network options (network and verbose)
options_network_init(loop->network, is_debug);
printf("%s to %s (", algorithm_name, dst_ip);
address_dump(&dst_addr);
printf("), %u hops max, %u bytes packets\n",
ptraceroute_options->max_ttl,
(unsigned int)packet_get_size(probe->packet)
);
// Add an algorithm instance in the main loop
if (!pt_add_instance(loop, algorithm_name, algorithm_options, probe)) {
fprintf(stderr, "E: Cannot add the chosen algorithm");
goto ERR_INSTANCE;
}
// Wait for events. They will be catched by handler_user()
if (pt_loop(loop, 0) < 0) {
fprintf(stderr, "E: Main loop interrupted");
goto ERR_PT_LOOP;
}
exit_code = EXIT_SUCCESS;
// Leave the program
ERR_PT_LOOP:
ERR_INSTANCE:
// pt_loop_free() automatically removes algorithms instances,
// probe_replies and events from the memory.
// Options and probe must be manually removed.
pt_loop_free(loop);
ERR_LOOP_CREATE:
ERR_UNKNOWN_ALGORITHM:
probe_free(probe);
ERR_PROBE_CREATE:
ERR_ADDRESS_IP_FROM_STRING:
ERR_ADDRESS_GUESS_FAMILY:
if (errno) perror(gai_strerror(errno));
ERR_CHECK_OPTIONS:
ERR_OPT_PARSE:
ERR_INIT_OPTIONS:
free(version);
exit(exit_code);
}
static int
pnp_parse_desc(device_t dev, u_char tag, u_char *res, int len,
struct isa_config *config, int ldn)
{
char buf[100];
u_int32_t id;
u_int32_t compat_id;
int temp;
id = isa_get_logicalid(dev);
if (PNP_RES_TYPE(tag) == 0) {
/* Small resource */
switch (PNP_SRES_NUM(tag)) {
case PNP_TAG_VERSION:
case PNP_TAG_VENDOR:
/* these descriptors are quietly ignored */
break;
case PNP_TAG_LOGICAL_DEVICE:
case PNP_TAG_START_DEPENDANT:
case PNP_TAG_END_DEPENDANT:
if (bootverbose)
pnp_printf(id, "unexpected small tag %d\n",
PNP_SRES_NUM(tag));
/* shouldn't happen; quit now */
return (1);
case PNP_TAG_COMPAT_DEVICE:
/*
* Got a compatible device id resource.
* Should keep a list of compat ids in the device.
*/
bcopy(res, &compat_id, 4);
if (isa_get_compatid(dev) == 0)
isa_set_compatid(dev, compat_id);
break;
case PNP_TAG_IRQ_FORMAT:
if (config->ic_nirq == ISA_NIRQ) {
pnp_printf(id, "too many irqs\n");
return (1);
}
if (I16(res) == 0) {
/* a null descriptor */
config->ic_irqmask[config->ic_nirq] = 0;
config->ic_nirq++;
break;
}
if (bootverbose)
pnp_printf(id, "adding irq mask %#02x\n",
I16(res));
config->ic_irqmask[config->ic_nirq] = I16(res);
config->ic_nirq++;
break;
case PNP_TAG_DMA_FORMAT:
if (config->ic_ndrq == ISA_NDRQ) {
pnp_printf(id, "too many drqs\n");
return (1);
}
if (res[0] == 0) {
/* a null descriptor */
config->ic_drqmask[config->ic_ndrq] = 0;
config->ic_ndrq++;
break;
}
if (bootverbose)
pnp_printf(id, "adding dma mask %#02x\n",
res[0]);
config->ic_drqmask[config->ic_ndrq] = res[0];
config->ic_ndrq++;
break;
case PNP_TAG_IO_RANGE:
if (config->ic_nport == ISA_NPORT) {
pnp_printf(id, "too many ports\n");
return (1);
}
if (res[6] == 0) {
/* a null descriptor */
config->ic_port[config->ic_nport].ir_start = 0;
config->ic_port[config->ic_nport].ir_end = 0;
config->ic_port[config->ic_nport].ir_size = 0;
config->ic_port[config->ic_nport].ir_align = 0;
config->ic_nport++;
break;
}
if (bootverbose) {
pnp_printf(id, "adding io range "
"%#x-%#x, size=%#x, "
"align=%#x\n",
I16(res + 1),
I16(res + 3) + res[6]-1,
res[6], res[5]);
}
config->ic_port[config->ic_nport].ir_start =
I16(res + 1);
config->ic_port[config->ic_nport].ir_end =
I16(res + 3) + res[6] - 1;
config->ic_port[config->ic_nport].ir_size = res[6];
if (res[5] == 0) {
/* Make sure align is at least one */
res[5] = 1;
}
config->ic_port[config->ic_nport].ir_align = res[5];
config->ic_nport++;
pnp_check_quirks(isa_get_vendorid(dev),
isa_get_logicalid(dev), ldn, config);
break;
case PNP_TAG_IO_FIXED:
if (config->ic_nport == ISA_NPORT) {
pnp_printf(id, "too many ports\n");
return (1);
}
if (res[2] == 0) {
/* a null descriptor */
config->ic_port[config->ic_nport].ir_start = 0;
config->ic_port[config->ic_nport].ir_end = 0;
config->ic_port[config->ic_nport].ir_size = 0;
config->ic_port[config->ic_nport].ir_align = 0;
config->ic_nport++;
break;
}
if (bootverbose) {
pnp_printf(id, "adding fixed io range "
"%#x-%#x, size=%#x, "
"align=%#x\n",
I16(res),
I16(res) + res[2] - 1,
res[2], 1);
}
config->ic_port[config->ic_nport].ir_start = I16(res);
config->ic_port[config->ic_nport].ir_end =
I16(res) + res[2] - 1;
config->ic_port[config->ic_nport].ir_size = res[2];
config->ic_port[config->ic_nport].ir_align = 1;
config->ic_nport++;
break;
case PNP_TAG_END:
if (bootverbose)
pnp_printf(id, "end config\n");
return (1);
default:
/* Skip this resource */
pnp_printf(id, "unexpected small tag %d\n",
PNP_SRES_NUM(tag));
break;
}
} else {
/* Large resource */
switch (PNP_LRES_NUM(tag)) {
case PNP_TAG_ID_UNICODE:
case PNP_TAG_LARGE_VENDOR:
/* these descriptors are quietly ignored */
break;
case PNP_TAG_ID_ANSI:
if (len > sizeof(buf) - 1)
len = sizeof(buf) - 1;
bcopy(res, buf, len);
/*
* Trim trailing spaces and garbage.
*/
while (len > 0 && buf[len - 1] <= ' ')
len--;
buf[len] = '\0';
device_set_desc_copy(dev, buf);
break;
case PNP_TAG_MEMORY_RANGE:
if (config->ic_nmem == ISA_NMEM) {
pnp_printf(id, "too many memory ranges\n");
return (1);
}
if (I16(res + 7) == 0) {
/* a null descriptor */
config->ic_mem[config->ic_nmem].ir_start = 0;
config->ic_mem[config->ic_nmem].ir_end = 0;
config->ic_mem[config->ic_nmem].ir_size = 0;
config->ic_mem[config->ic_nmem].ir_align = 0;
config->ic_nmem++;
break;
}
if (bootverbose) {
temp = I16(res + 7) << 8;
pnp_printf(id, "adding memory range "
"%#x-%#x, size=%#x, "
"align=%#x\n",
I16(res + 1) << 8,
(I16(res + 3) << 8) + temp - 1,
temp, I16(res + 5));
}
config->ic_mem[config->ic_nmem].ir_start =
I16(res + 1) << 8;
config->ic_mem[config->ic_nmem].ir_end =
(I16(res + 3) << 8) + (I16(res + 7) << 8) - 1;
config->ic_mem[config->ic_nmem].ir_size =
I16(res + 7) << 8;
config->ic_mem[config->ic_nmem].ir_align = I16(res + 5);
if (!config->ic_mem[config->ic_nmem].ir_align)
config->ic_mem[config->ic_nmem].ir_align =
0x10000;
config->ic_nmem++;
break;
case PNP_TAG_MEMORY32_RANGE:
if (config->ic_nmem == ISA_NMEM) {
pnp_printf(id, "too many memory ranges\n");
return (1);
}
if (I32(res + 13) == 0) {
/* a null descriptor */
config->ic_mem[config->ic_nmem].ir_start = 0;
config->ic_mem[config->ic_nmem].ir_end = 0;
config->ic_mem[config->ic_nmem].ir_size = 0;
config->ic_mem[config->ic_nmem].ir_align = 0;
config->ic_nmem++;
break;
}
if (bootverbose) {
pnp_printf(id, "adding memory32 range "
"%#x-%#x, size=%#x, "
"align=%#x\n",
I32(res + 1),
I32(res + 5) + I32(res + 13) - 1,
I32(res + 13), I32(res + 9));
}
config->ic_mem[config->ic_nmem].ir_start = I32(res + 1);
config->ic_mem[config->ic_nmem].ir_end =
I32(res + 5) + I32(res + 13) - 1;
config->ic_mem[config->ic_nmem].ir_size = I32(res + 13);
config->ic_mem[config->ic_nmem].ir_align = I32(res + 9);
config->ic_nmem++;
break;
case PNP_TAG_MEMORY32_FIXED:
if (config->ic_nmem == ISA_NMEM) {
pnp_printf(id, "too many memory ranges\n");
return (1);
}
if (I32(res + 5) == 0) {
/* a null descriptor */
config->ic_mem[config->ic_nmem].ir_start = 0;
config->ic_mem[config->ic_nmem].ir_end = 0;
config->ic_mem[config->ic_nmem].ir_size = 0;
config->ic_mem[config->ic_nmem].ir_align = 0;
break;
}
if (bootverbose) {
pnp_printf(id, "adding fixed memory32 range "
"%#x-%#x, size=%#x\n",
I32(res + 1),
I32(res + 1) + I32(res + 5) - 1,
I32(res + 5));
}
config->ic_mem[config->ic_nmem].ir_start = I32(res + 1);
config->ic_mem[config->ic_nmem].ir_end =
I32(res + 1) + I32(res + 5) - 1;
config->ic_mem[config->ic_nmem].ir_size = I32(res + 5);
config->ic_mem[config->ic_nmem].ir_align = 1;
config->ic_nmem++;
break;
default:
/* Skip this resource */
pnp_printf(id, "unexpected large tag %d\n",
PNP_SRES_NUM(tag));
break;
}
}
return (0);
}
int main(int argc, char ** argv)
{
algorithm_instance_t * instance;
traceroute_options_t options = traceroute_get_default_options();
probe_t * probe;
pt_loop_t * loop;
int family;
int ret = EXIT_FAILURE;
const char * ip_protocol_name;
const char * protocol_name;
address_t dst_addr;
// Harcoded command line parsing here
char dst_ip[] = "8.8.8.8";
//char dst_ip[] = "1.1.1.2";
//char dst_ip[] = "2001:db8:85a3::8a2e:370:7338";
if (!address_guess_family(dst_ip, &family)) {
fprintf(stderr, "Cannot guess family of destination address (%s)", dst_ip);
goto ERR_ADDRESS_GUESS_FAMILY;
}
if (address_from_string(family, dst_ip, &dst_addr) != 0) {
fprintf(stderr, "Cannot guess family of destination address (%s)", dst_ip);
goto ERR_ADDRESS_FROM_STRING;
}
// Prepare options related to the 'traceroute' algorithm
options.do_resolv = false;
options.dst_addr = &dst_addr;
options.num_probes = 1;
// options.max_ttl = 1;
printf("num_probes = %zu max_ttl = %u\n", options.num_probes, options.max_ttl);
// Create libparistraceroute loop
// No information shared by traceroute algorithm instances, so we pass NULL
if (!(loop = pt_loop_create(loop_handler, NULL))) {
fprintf(stderr, "Cannot create libparistraceroute loop");
goto ERR_LOOP_CREATE;
}
// Probe skeleton definition: IPv4/UDP probe targetting 'dst_ip'
if (!(probe = probe_create())) {
fprintf(stderr, "Cannot create probe skeleton");
goto ERR_PROBE_CREATE;
}
switch (family) {
case AF_INET:
ip_protocol_name = "ipv4";
protocol_name = "icmpv4";
break;
case AF_INET6:
ip_protocol_name = "ipv6";
protocol_name = "icmpv6";
break;
default:
fprintf(stderr, "Internet family not supported (%d)\n", family);
goto ERR_FAMILY;
}
// protocol_name = "udp";
protocol_name = "tcp";
printf("protocol_name = %s\n", protocol_name);
if (!probe_set_protocols(probe, ip_protocol_name, protocol_name, NULL)) {
fprintf(stderr, "Can't set protocols %s/%s\n", ip_protocol_name, protocol_name);
goto ERR_PROBE_SET_PROTOCOLS;
}
if (strncmp("icmp", protocol_name, 4) != 0) {
probe_write_payload(probe, "\0\0\0\0", 4);
} else {
probe_set_field(probe, I32("body", 1));
}
probe_set_fields(probe,
ADDRESS("dst_ip", &dst_addr),
I16("dst_port", 3000),
NULL
);
/*
if (strcmp("tcp", protocol_name) == 0) {
printf("setting tcp fields\n");
uint8_t one = 1;
probe_set_fields(probe,
BITS("reserved", 1, &one),
BITS("ns", 1, &one),
BITS("cwr", 1, &one),
BITS("ece", 1, &one),
BITS("urg", 1, &one),
BITS("ack", 1, &one),
BITS("psh", 1, &one),
BITS("rst", 1, &one),
BITS("syn", 1, &one),
BITS("fin", 1, &one),
NULL
);
}
*/
probe_dump(probe);
// Instanciate a 'traceroute' algorithm
if (!(instance = pt_add_instance(loop, "traceroute", &options, probe))) {
fprintf(stderr, "Cannot add 'traceroute' algorithm");
goto ERR_INSTANCE;
}
// Wait for events. They will be catched by handler_user()
if (pt_loop(loop, 0) < 0) {
fprintf(stderr, "Main loop interrupted");
goto ERR_IN_PT_LOOP;
}
ret = EXIT_SUCCESS;
// Free data and quit properly
ERR_IN_PT_LOOP:
// instance is freed by pt_loop_free
ERR_INSTANCE:
ERR_PROBE_SET_PROTOCOLS:
ERR_FAMILY:
probe_free(probe);
ERR_PROBE_CREATE:
pt_loop_free(loop);
ERR_LOOP_CREATE:
ERR_ADDRESS_FROM_STRING:
ERR_ADDRESS_GUESS_FAMILY:
exit(ret);
}
void
pnp_parse_resources(device_t dev, u_char *resources, int len, int ldn)
{
struct isa_config *configs;
struct isa_config *config;
device_t parent;
int priorities[1 + MAXDEP];
u_char *start;
u_char *p;
u_char tag;
u_int32_t id;
int ncfgs;
int l;
int i;
parent = device_get_parent(dev);
id = isa_get_logicalid(dev);
configs = (struct isa_config *)malloc(sizeof(*configs)*(1 + MAXDEP),
M_DEVBUF, M_NOWAIT | M_ZERO);
if (configs == NULL) {
device_printf(parent, "No memory to parse PNP data\n");
return;
}
config = &configs[0];
priorities[0] = 0;
ncfgs = 1;
p = resources;
start = NULL;
while (len > 0) {
tag = *p++;
len--;
if (PNP_RES_TYPE(tag) == 0) {
/* Small resource */
l = PNP_SRES_LEN(tag);
if (len < l) {
len = 0;
continue;
}
len -= l;
switch (PNP_SRES_NUM(tag)) {
case PNP_TAG_START_DEPENDANT:
if (start != NULL) {
/*
* Copy the common resources first,
* then parse the "dependent" resources.
*/
pnp_merge_resources(dev, &configs[0],
config);
pnp_parse_dependant(dev, start,
p - start - 1,
config, ldn);
}
start = p + l;
if (ncfgs > MAXDEP) {
device_printf(parent, "too many dependant configs (%d)\n", MAXDEP);
len = 0;
break;
}
config = &configs[ncfgs];
/*
* If the priority is not specified,
* then use the default of 'acceptable'
*/
if (l > 0)
priorities[ncfgs] = p[0];
else
priorities[ncfgs] = 1;
if (bootverbose)
pnp_printf(id, "start dependent (%d)\n",
priorities[ncfgs]);
ncfgs++;
break;
case PNP_TAG_END_DEPENDANT:
if (start == NULL) {
device_printf(parent,
"malformed resources\n");
len = 0;
break;
}
/*
* Copy the common resources first,
* then parse the "dependent" resources.
*/
pnp_merge_resources(dev, &configs[0], config);
pnp_parse_dependant(dev, start, p - start - 1,
config, ldn);
start = NULL;
if (bootverbose)
pnp_printf(id, "end dependent\n");
/*
* Back to the common part; clear it
* as its contents has already been copied
* to each dependant.
*/
config = &configs[0];
bzero(config, sizeof(*config));
break;
case PNP_TAG_END:
if (start != NULL) {
device_printf(parent,
"malformed resources\n");
}
len = 0;
break;
default:
if (start != NULL)
/* defer parsing a dependent section */
break;
if (pnp_parse_desc(dev, tag, p, l, config, ldn))
len = 0;
break;
}
p += l;
} else {
/* Large resource */
if (len < 2) {
len = 0;
break;
}
l = I16(p);
p += 2;
len -= 2;
if (len < l) {
len = 0;
break;
}
len -= l;
if (start == NULL &&
pnp_parse_desc(dev, tag, p, l, config, ldn)) {
len = 0;
break;
}
p += l;
}
}
if (ncfgs == 1) {
/* Single config without dependants */
ISA_ADD_CONFIG(parent, dev, priorities[0], &configs[0]);
free(configs, M_DEVBUF);
return;
}
for (i = 1; i < ncfgs; i++) {
/*
* Merge the remaining part of the common resources,
* if any. Strictly speaking, there shouldn't be common/main
* resources after the END_DEPENDENT tag.
*/
pnp_merge_resources(dev, &configs[0], &configs[i]);
ISA_ADD_CONFIG(parent, dev, priorities[i], &configs[i]);
}
free(configs, M_DEVBUF);
}
static void
internalize_ehdr(Elf_Byte bo, Elf_Ehdr *ehdr)
{
#if ELFSIZE == 32
I16(ehdr->e_type);
I16(ehdr->e_machine);
I32(ehdr->e_version);
I32(ehdr->e_entry);
I32(ehdr->e_phoff);
I32(ehdr->e_shoff);
I32(ehdr->e_flags);
I16(ehdr->e_ehsize);
I16(ehdr->e_phentsize);
I16(ehdr->e_phnum);
I16(ehdr->e_shentsize);
I16(ehdr->e_shnum);
I16(ehdr->e_shstrndx);
#elif ELFSIZE == 64
I16(ehdr->e_type);
I16(ehdr->e_machine);
I32(ehdr->e_version);
I64(ehdr->e_entry);
I64(ehdr->e_phoff);
I64(ehdr->e_shoff);
I32(ehdr->e_flags);
I16(ehdr->e_ehsize);
I16(ehdr->e_phentsize);
I16(ehdr->e_phnum);
I16(ehdr->e_shentsize);
I16(ehdr->e_shnum);
I16(ehdr->e_shstrndx);
#else
#error ELFSIZE is not 32 or 64
#endif
}
offs_t v810_disassembler::disassemble(std::ostream &stream, offs_t pc, const data_buffer &opcodes, const data_buffer ¶ms)
{
uint32_t flags = 0;
uint32_t opc,opc2;
unsigned size;
opc = opcodes.r16(pc);
opc2 = opcodes.r16(pc+2);
switch(opc>>10)
{
case 0x00: util::stream_format(stream,"MOV %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x01: util::stream_format(stream,"ADD %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x02: util::stream_format(stream,"SUB %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x03: util::stream_format(stream,"CMP %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x04: util::stream_format(stream,"SHL %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x05: util::stream_format(stream,"SHR %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x06: util::stream_format(stream,"JMP [%s]",GET1s(opc)); size=2; if ((opc&0x1f) == 31) flags = STEP_OUT; break;
case 0x07: util::stream_format(stream,"SAR %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x08: util::stream_format(stream,"MUL %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x09: util::stream_format(stream,"DIV %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0a: util::stream_format(stream,"MULU %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0b: util::stream_format(stream,"DIVU %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0c: util::stream_format(stream,"OR %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0d: util::stream_format(stream,"AND %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0e: util::stream_format(stream,"XOR %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x0f: util::stream_format(stream,"NOT %s,%s",GET1s(opc),GET2s(opc)); size=2; break;
case 0x10: util::stream_format(stream,"MOV %X,%s",I5(opc),GET2s(opc)); size=2; break;
case 0x11: util::stream_format(stream,"ADD %X,%s",I5(opc),GET2s(opc)); size=2; break;
case 0x12: util::stream_format(stream,"SETF %X,%s",I5(opc),GET2s(opc)); size=2; break;
case 0x13: util::stream_format(stream,"CMP %X,%s",I5(opc),GET2s(opc)); size=2; break;
case 0x14: util::stream_format(stream,"SHL %X,%s",UI5(opc),GET2s(opc)); size=2; break;
case 0x15: util::stream_format(stream,"SHR %X,%s",UI5(opc),GET2s(opc)); size=2; break;
case 0x16: util::stream_format(stream,"EI"); size=2; break;
case 0x17: util::stream_format(stream,"SAR %X,%s",UI5(opc),GET2s(opc)); size=2; break;
case 0x18: util::stream_format(stream,"TRAP %X",I5(opc)); size=2; break;
case 0x19: util::stream_format(stream,"RETI"); size=2; flags = STEP_OUT; break;
case 0x1a: util::stream_format(stream,"HALT"); size=2; break;
case 0x1b: util::stream_format(stream,"Unk 0x1B"); size=2; break;
case 0x1c: util::stream_format(stream,"LDSR %s,%s",GET2s(opc),GETRs(opc));size=2; break;
case 0x1d: util::stream_format(stream,"STSR %s,%s",GETRs(opc),GET2s(opc));size=2; break;
case 0x1e: util::stream_format(stream,"DI"); size=2; break;
case 0x1f:
switch(opc&0x1f)
{
case 0x00: util::stream_format(stream,"SCH0BSU"); break;
case 0x01: util::stream_format(stream,"SCH0BSD"); break;
case 0x02: util::stream_format(stream,"SCH1BSU"); break;
case 0x03: util::stream_format(stream,"SCH1BSD"); break;
case 0x04: util::stream_format(stream,"UnkS 4"); break;
case 0x05: util::stream_format(stream,"UnkS 5"); break;
case 0x06: util::stream_format(stream,"UnkS 6"); break;
case 0x08: util::stream_format(stream,"ORBSU"); break;
case 0x09: util::stream_format(stream,"ANDBSU"); break;
case 0x0a: util::stream_format(stream,"XORBSU"); break;
case 0x0b: util::stream_format(stream,"MOVBSU"); break;
case 0x0c: util::stream_format(stream,"ORNBSU"); break;
case 0x0d: util::stream_format(stream,"ANDNBSU"); break;
case 0x0e: util::stream_format(stream,"XORNBSU"); break;
case 0x0f: util::stream_format(stream,"NOTBSU"); break;
default: util::stream_format(stream,"UnkBS 0x%X",opc&0x1f); break;
}
size=2;
break;
case 0x20:
case 0x21:
case 0x22:
case 0x23:
case 0x24:
case 0x25:
case 0x26:
case 0x27: switch( (opc>>9) &0xf)
{
case 0x0: util::stream_format(stream,"BV %X",pc+D9(opc)); break;
case 0x1: util::stream_format(stream,"BL %X",pc+D9(opc)); break;
case 0x2: util::stream_format(stream,"BE %X",pc+D9(opc)); break;
case 0x3: util::stream_format(stream,"BNH %X",pc+D9(opc)); break;
case 0x4: util::stream_format(stream,"BN %X",pc+D9(opc)); break;
case 0x5: util::stream_format(stream,"BR %X",pc+D9(opc)); break;
case 0x6: util::stream_format(stream,"BLT %X",pc+D9(opc)); break;
case 0x7: util::stream_format(stream,"BLE %X",pc+D9(opc)); break;
case 0x8: util::stream_format(stream,"BNV %X",pc+D9(opc)); break;
case 0x9: util::stream_format(stream,"BNL %X",pc+D9(opc)); break;
case 0xa: util::stream_format(stream,"BNE %X",pc+D9(opc)); break;
case 0xb: util::stream_format(stream,"BH %X",pc+D9(opc)); break;
case 0xc: util::stream_format(stream,"BP %X",pc+D9(opc)); break;
case 0xd: util::stream_format(stream,"NOP"); break;
case 0xe: util::stream_format(stream,"BGE %X",pc+D9(opc)); break;
case 0xf: util::stream_format(stream,"BGT %X",pc+D9(opc)); break;
}
size=2;
break;
case 0x28: util::stream_format(stream,"MOVEA %X, %s, %s",I16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x29: util::stream_format(stream,"ADDI %X, %s, %s",I16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x2a: util::stream_format(stream,"JR %X",pc+D26(opc,opc2));size=4; break;
case 0x2b: util::stream_format(stream,"JAL %X",pc+D26(opc,opc2));size=4; flags = STEP_OVER; break;
case 0x2c: util::stream_format(stream,"ORI %X, %s, %s",UI16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x2d: util::stream_format(stream,"ANDI %X, %s, %s",UI16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x2e: util::stream_format(stream,"XORI %X, %s, %s",UI16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x2f: util::stream_format(stream,"MOVHI %X, %s, %s",UI16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x30: util::stream_format(stream,"LDB %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x31: util::stream_format(stream,"LDH %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x32: util::stream_format(stream,"Unk 0x32"); size=2; break;
case 0x33: util::stream_format(stream,"LDW %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x34: util::stream_format(stream,"STB %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x35: util::stream_format(stream,"STH %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x36: util::stream_format(stream,"Unk 0x36"); size=2; break;
case 0x37: util::stream_format(stream,"STW %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x38: util::stream_format(stream,"INB %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x39: util::stream_format(stream,"INH %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x3a: util::stream_format(stream,"CAXI %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x3b: util::stream_format(stream,"INW %X[%s], %s",D16(opc2),GET1s(opc),GET2s(opc));size=4; break;
case 0x3c: util::stream_format(stream,"OUTB %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x3d: util::stream_format(stream,"OUTH %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
case 0x3e:
switch((opc2&0xfc00)>>10)
{
case 0x0: util::stream_format(stream,"CMPF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x2: util::stream_format(stream,"CVT.WS %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x3: util::stream_format(stream,"CVT.SW %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x4: util::stream_format(stream,"ADDF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x5: util::stream_format(stream,"SUBF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x6: util::stream_format(stream,"MULF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0x7: util::stream_format(stream,"DIVF.S %s, %s",GET1s(opc),GET2s(opc)); break;
case 0xb: util::stream_format(stream,"TRNC.SW %s, %s",GET1s(opc),GET2s(opc)); break;
default : util::stream_format(stream,"Unkf 0x%X",(opc2&0xfc00)>>10); break;
}
size=4;
break;
case 0x3f: util::stream_format(stream,"OUTW %s, %X[%s]",GET2s(opc),D16(opc2),GET1s(opc));size=4; break;
default : size=2;
}
return size | flags | SUPPORTED;
}
bool probe_set_protocols(probe_t * probe, const char * name1, ...)
{
// TODO A similar function should allow hooking into the layer structure
// and not at the top layer
va_list args, args2;
size_t packet_size, offset, segment_size;
const char * name;
layer_t * layer = NULL,
* prev_layer;
const protocol_t * protocol;
// Remove the former layer structure
probe_layers_clear(probe);
// Set up the new layer structure
va_start(args, name1);
// Allocate the buffer according to the layer structure
packet_size = 0;
va_copy(args2, args);
for (name = name1; name; name = va_arg(args2, char *)) {
if (!(protocol = protocol_search(name))) {
fprintf(stderr, "Cannot find %s protocol, known protocols are:", name);
protocols_dump();
goto ERR_PROTOCOL_SEARCH;
}
packet_size += protocol->write_default_header(NULL);
}
va_end(args2);
if (!(packet_resize(probe->packet, packet_size))) goto ERR_PACKET_RESIZE;
// Create each layer
offset = 0;
prev_layer = NULL;
for (name = name1; name; name = va_arg(args, char *)) {
// Associate protocol to the layer
if (!(protocol = protocol_search(name))) goto ERR_PROTOCOL_SEARCH2;
segment_size = protocol->write_default_header(packet_get_bytes(probe->packet) + offset);
if (!(layer = layer_create_from_segment(protocol, packet_get_bytes(probe->packet) + offset, segment_size))) {
fprintf(stderr, "Can't create segment for %s header\n", layer->protocol->name);
goto ERR_LAYER_CREATE;
}
// TODO layer_set_mask(layer, bitfield_get_mask(probe->bitfield) + offset);
// Update 'length' field (if any). It concerns IPv* and UDP, but not TCP or ICMPv*
layer_set_field_and_free(layer, I16("length", packet_size - offset));
// Update 'protocol' field of the previous inserted layer (if any)
if (prev_layer) {
if (!layer_set_field_and_free(prev_layer, I8("protocol", layer->protocol->protocol))) {
fprintf(stderr, "Can't set 'protocol' in %s header\n", layer->protocol->name);
goto ERR_SET_PROTOCOL;
}
}
offset += layer->segment_size;
if (!probe_push_layer(probe, layer)) {
fprintf(stderr, "Can't add protocol layer\n");
goto ERR_PUSH_LAYER;
}
prev_layer = layer;
}
va_end(args);
layer = NULL;
// Payload : initially empty
if (!probe_push_payload(probe, 0)) {
goto ERR_PUSH_PAYLOAD;
}
// Size and checksum are pending, they depend on payload
return true;
ERR_PUSH_PAYLOAD:
ERR_PUSH_LAYER:
ERR_SET_PROTOCOL:
if (layer) layer_free(layer);
ERR_LAYER_CREATE:
ERR_PROTOCOL_SEARCH2:
probe_layers_clear(probe);
ERR_PACKET_RESIZE:
ERR_PROTOCOL_SEARCH:
return false;
}
int
ImagingFliDecode(Imaging im, ImagingCodecState state, UINT8* buf, Py_ssize_t bytes)
{
UINT8* ptr;
int framesize;
int c, chunks;
int l, lines;
int i, j, x = 0, y, ymax;
/* If not even the chunk size is present, we'd better leave */
if (bytes < 4)
return 0;
/* We don't decode anything unless we have a full chunk in the
input buffer (on the other hand, the Python part of the driver
makes sure this is always the case) */
ptr = buf;
framesize = I32(ptr);
if (framesize < I32(ptr))
return 0;
/* Make sure this is a frame chunk. The Python driver takes
case of other chunk types. */
if (I16(ptr+4) != 0xF1FA) {
state->errcode = IMAGING_CODEC_UNKNOWN;
return -1;
}
chunks = I16(ptr+6);
ptr += 16;
/* Process subchunks */
for (c = 0; c < chunks; c++) {
UINT8 *data = ptr + 6;
switch (I16(ptr+4)) {
case 4: case 11:
/* FLI COLOR chunk */
break; /* ignored; handled by Python code */
case 7:
/* FLI SS2 chunk (word delta) */
lines = I16(data); data += 2;
for (l = y = 0; l < lines && y < state->ysize; l++, y++) {
UINT8* buf = (UINT8*) im->image[y];
int p, packets;
packets = I16(data); data += 2;
while (packets & 0x8000) {
/* flag word */
if (packets & 0x4000) {
y += 65536 - packets; /* skip lines */
if (y >= state->ysize) {
state->errcode = IMAGING_CODEC_OVERRUN;
return -1;
}
buf = (UINT8*) im->image[y];
} else {
/* store last byte (used if line width is odd) */
buf[state->xsize-1] = (UINT8) packets;
}
packets = I16(data); data += 2;
}
for (p = x = 0; p < packets; p++) {
x += data[0]; /* pixel skip */
if (data[1] >= 128) {
i = 256-data[1]; /* run */
if (x + i + i > state->xsize)
break;
for (j = 0; j < i; j++) {
buf[x++] = data[2];
buf[x++] = data[3];
}
data += 2 + 2;
} else {
i = 2 * (int) data[1]; /* chunk */
if (x + i > state->xsize)
break;
memcpy(buf + x, data + 2, i);
data += 2 + i;
x += i;
}
}
if (p < packets)
break; /* didn't process all packets */
}
if (l < lines) {
/* didn't process all lines */
state->errcode = IMAGING_CODEC_OVERRUN;
return -1;
}
break;
case 12:
/* FLI LC chunk (byte delta) */
y = I16(data); ymax = y + I16(data+2); data += 4;
for (; y < ymax && y < state->ysize; y++) {
UINT8* out = (UINT8*) im->image[y];
int p, packets = *data++;
for (p = x = 0; p < packets; p++, x += i) {
x += data[0]; /* skip pixels */
if (data[1] & 0x80) {
i = 256-data[1]; /* run */
if (x + i > state->xsize)
break;
memset(out + x, data[2], i);
data += 3;
} else {
i = data[1]; /* chunk */
if (x + i > state->xsize)
break;
memcpy(out + x, data + 2, i);
data += i + 2;
}
}
if (p < packets)
break; /* didn't process all packets */
}
if (y < ymax) {
/* didn't process all lines */
state->errcode = IMAGING_CODEC_OVERRUN;
return -1;
}
break;
case 13:
/* FLI BLACK chunk */
for (y = 0; y < state->ysize; y++)
memset(im->image[y], 0, state->xsize);
break;
case 15:
/* FLI BRUN chunk */
for (y = 0; y < state->ysize; y++) {
UINT8* out = (UINT8*) im->image[y];
data += 1; /* ignore packetcount byte */
for (x = 0; x < state->xsize; x += i) {
if (data[0] & 0x80) {
i = 256 - data[0];
if (x + i > state->xsize)
break; /* safety first */
memcpy(out + x, data + 1, i);
data += i + 1;
} else {
i = data[0];
if (x + i > state->xsize)
break; /* safety first */
memset(out + x, data[1], i);
data += 2;
}
}
if (x != state->xsize) {
/* didn't unpack whole line */
state->errcode = IMAGING_CODEC_OVERRUN;
return -1;
}
}
break;
case 16:
/* COPY chunk */
for (y = 0; y < state->ysize; y++) {
UINT8* buf = (UINT8*) im->image[y];
memcpy(buf, data, state->xsize);
data += state->xsize;
}
break;
case 18:
/* PSTAMP chunk */
break; /* ignored */
default:
/* unknown chunk */
/* printf("unknown FLI/FLC chunk: %d\n", I16(ptr+4)); */
state->errcode = IMAGING_CODEC_UNKNOWN;
return -1;
}
ptr += I32(ptr);
}
return -1; /* end of frame */
}
field_t * ipv6_get_length(const uint8_t * ipv6_segment) {
const struct ip6_hdr * ipv6_header = (const struct ip6_hdr *) ipv6_segment;
return I16("length", ntohs(ipv6_header->ip6_plen) + sizeof(struct ip6_hdr));
}
