void vidout_cmd(int ch, char *optarg)
{
__u32 width, height, pixfmt;
switch (ch) {
case OptSetVideoOutMplaneFormat:
case OptTryVideoOutMplaneFormat:
set_fmts_out = parse_fmt(optarg, width, height, pixfmt);
if (!set_fmts_out) {
vidcap_usage();
exit(1);
}
vfmt_out.fmt.pix_mp.width = width;
vfmt_out.fmt.pix_mp.height = height;
vfmt_out.fmt.pix_mp.pixelformat = pixfmt;
break;
case OptSetVideoOutFormat:
case OptTryVideoOutFormat:
set_fmts_out = parse_fmt(optarg, width, height, pixfmt);
if (!set_fmts_out) {
vidcap_usage();
exit(1);
}
vfmt_out.fmt.pix.width = width;
vfmt_out.fmt.pix.height = height;
vfmt_out.fmt.pix.pixelformat = pixfmt;
break;
}
}
/* might as well make this public since the assertion stuff seemed be
using it anyways */
char *dispmon_parse(const char *fmt, int nump, int start,
const char *unit)
{
FmtArray *f=parse_fmt(fmt);
size_t i=0, pno=start;
RStr *output=RStr_new(256);
char *s;
for(i=0; i<f->used && output; i++)
{
if(f->f[i]->text)
expanding_strcat(output,f->f[i]->text);
else
{
/* should not need this check if checktf errors */
if(pno>nump)
{
//cDispmon(modulename, MON_ERR,
// "insufficient parameters for format string \"%s\" in "
// "$dispmon/$writemon", fmt);
RStr_delete(output);
output = NULL;
} else
pno=insert_var(output,f->f[i],pno);
}
}
FmtArray_delete(f);
s = output ? output->str : 0;
free(output);
return s;
}
static int read_int_array(struct dsa *dsa, char *name, char *fmt,
int n, int val[])
{ int k, pos;
char str[80+1];
if (parse_fmt(dsa, fmt)) return 1;
if (!(dsa->fmt_f == 'I' && dsa->fmt_w <= 80 &&
dsa->fmt_k * dsa->fmt_w <= 80))
{ xprintf(
"%s:%d: can't read array '%s' - invalid format '%s'\n",
dsa->fname, dsa->seqn, name, fmt);
return 1;
}
for (k = 1, pos = INT_MAX; k <= n; k++, pos++)
{ if (pos >= dsa->fmt_k)
{ if (read_card(dsa)) return 1;
pos = 0;
}
memcpy(str, dsa->card + dsa->fmt_w * pos, dsa->fmt_w);
str[dsa->fmt_w] = '\0';
strspx(str);
if (str2int(str, &val[k]))
{ xprintf(
"%s:%d: can't read array '%s' - invalid value '%s'\n",
dsa->fname, dsa->seqn, name, str);
return 1;
}
}
return 0;
}
struct module * jpeg_compress_init(struct module *parent, char * opts)
{
struct state_video_compress_jpeg *s;
int frame_idx;
s = (struct state_video_compress_jpeg *) malloc(sizeof(struct state_video_compress_jpeg));
for (frame_idx = 0; frame_idx < 2; frame_idx++) {
s->out[frame_idx] = NULL;
}
s->decoded = NULL;
if(opts && strcmp(opts, "help") == 0) {
printf("JPEG comperssion usage:\n");
printf("\t-c JPEG[:<quality>[:<restart_interval>]]\n");
return &compress_init_noerr;
} else if(opts && strcmp(opts, "list_devices") == 0) {
printf("CUDA devices:\n");
gpujpeg_print_devices_info();
return &compress_init_noerr;
}
s->restart_interval = -1;
gpujpeg_set_default_parameters(&s->encoder_param);
if(opts) {
parse_fmt(s, opts);
} else {
printf("[JPEG] setting default encode parameters (quality: %d)\n",
s->encoder_param.quality
);
}
int ret;
printf("Initializing CUDA device %d...\n", cuda_devices[0]);
ret = gpujpeg_init_device(cuda_devices[0], TRUE);
if(ret != 0) {
fprintf(stderr, "[JPEG] initializing CUDA device %d failed.\n", cuda_devices[0]);
return NULL;
}
s->encoder = NULL; /* not yet configured */
platform_spin_init(&s->spin);
module_init_default(&s->module_data);
s->module_data.cls = MODULE_CLASS_DATA;
s->module_data.priv_data = s;
s->module_data.deleter = jpeg_compress_done;
s->module_data.msg_callback = compress_change_callback;
module_register(&s->module_data, parent);
return &s->module_data;
}
void vidcap_cmd(int ch, char *optarg)
{
__u32 colorspace, xfer_func, ycbcr, quantization;
char *value, *subs;
switch (ch) {
case OptSetVideoFormat:
case OptTryVideoFormat:
set_fmts = parse_fmt(optarg, width, height, pixfmt, field, colorspace,
xfer_func, ycbcr, quantization, flags, bytesperline);
if (!set_fmts ||
(set_fmts & (FmtColorspace | FmtYCbCr | FmtQuantization | FmtXferFunc))) {
vidcap_usage();
exit(1);
}
break;
case OptListFrameSizes:
if (strlen(optarg) == 4)
frmsize.pixel_format = v4l2_fourcc(optarg[0], optarg[1],
optarg[2], optarg[3]);
else
frmsize.pixel_format = strtol(optarg, 0L, 0);
break;
case OptListFrameIntervals:
subs = optarg;
while (*subs != '\0') {
static const char *const subopts[] = {
"width",
"height",
"pixelformat",
NULL
};
switch (parse_subopt(&subs, subopts, &value)) {
case 0:
frmival.width = strtol(value, 0L, 0);
break;
case 1:
frmival.height = strtol(value, 0L, 0);
break;
case 2:
if (strlen(value) == 4)
frmival.pixel_format =
v4l2_fourcc(value[0], value[1],
value[2], value[3]);
else
frmival.pixel_format = strtol(value, 0L, 0);
break;
default:
vidcap_usage();
exit(1);
}
}
break;
}
}
void
default_ospec (void)
{
int i;
for (i = 0; i < FMT_NONE; ++i) {
oi_list[i] = OUTITEM_NULL;
parse_fmt(def_fmt[i], i);
}
}
static int read_real_array(struct dsa *dsa, char *name, char *fmt,
int n, double val[])
{ int k, pos;
char str[80+1], *ptr;
if (parse_fmt(dsa, fmt)) return 1;
if (!(dsa->fmt_f != 'I' && dsa->fmt_w <= 80 &&
dsa->fmt_k * dsa->fmt_w <= 80))
{ xprintf(
"%s:%d: can't read array '%s' - invalid format '%s'\n",
dsa->fname, dsa->seqn, name, fmt);
return 1;
}
for (k = 1, pos = INT_MAX; k <= n; k++, pos++)
{ if (pos >= dsa->fmt_k)
{ if (read_card(dsa)) return 1;
pos = 0;
}
memcpy(str, dsa->card + dsa->fmt_w * pos, dsa->fmt_w);
str[dsa->fmt_w] = '\0';
strspx(str);
if (strchr(str, '.') == NULL && strcmp(str, "0"))
{ xprintf("%s(%d): can't read array '%s' - value '%s' has no "
"decimal point\n", dsa->fname, dsa->seqn, name, str);
return 1;
}
/* sometimes lower case letters appear */
for (ptr = str; *ptr; ptr++)
*ptr = (char)toupper((unsigned char)*ptr);
ptr = strchr(str, 'D');
if (ptr != NULL) *ptr = 'E';
/* value may appear with decimal exponent but without letters
E or D (for example, -123.456-012), so missing letter should
be inserted */
ptr = strchr(str+1, '+');
if (ptr == NULL) ptr = strchr(str+1, '-');
if (ptr != NULL && *(ptr-1) != 'E')
{ xassert(strlen(str) < 80);
memmove(ptr+1, ptr, strlen(ptr)+1);
*ptr = 'E';
}
if (str2num(str, &val[k]))
{ xprintf(
"%s:%d: can't read array '%s' - invalid value '%s'\n",
dsa->fname, dsa->seqn, name, str);
return 1;
}
}
return 0;
}
size_t monitor_nfmtargs(char *fmt)
{
/* validate # of parameters and correct tokens by parsing format */
FmtArray *f=parse_fmt(fmt);
size_t i=0,nargs=0;
for(i=0; i<f->used; i++)
{
if(!f->f[i]->text && !strchr(no_nparam, f->f[i]->cvt_spec))
nargs++;
if(f->f[i]->failed)
cDispmon("disp", MON_ERR, "invalid format spec: \"%s\" in "
"format string \"%s\"\nin instance %s",
f->f[i]->text, tf_getcstringp(3),tf_mipname());
}
FmtArray_delete(f);
return nargs;
}
static struct response *compress_change_callback(struct module *mod, struct message *msg)
{
struct state_video_compress_jpeg *s = (struct state_video_compress_jpeg *) mod->priv_data;
static struct response *ret;
struct msg_change_compress_data *data =
(struct msg_change_compress_data *) msg;
platform_spin_lock(&s->spin);
parse_fmt(s, data->config_string);
ret = new_response(RESPONSE_OK, NULL);
memset(&s->saved_desc, 0, sizeof(s->saved_desc));
platform_spin_unlock(&s->spin);
free_message(msg);
return ret;
}
int
main(int argc, char **argv)
{
enum fd_drivetype type;
struct fd_type ft, newft, *fdtp;
const char *name, *descr;
int fd, i, autofmt;
autofmt = 0;
while((i = getopt(argc, argv, "aFf:s:v")) != -1)
switch(i) {
case 'a':
autofmt = 1;
case 'F':
showfmt = 1;
show = 0;
break;
case 'f':
if (!strcmp(optarg, "auto")) {
format = -1;
} else if (getnum(optarg, &format)) {
fprintf(stderr,
"Bad argument %s to -f option; must be numeric\n",
optarg);
usage();
}
show = 0;
break;
case 's':
fmtstring = optarg;
show = 0;
break;
case 'v':
verbose++;
break;
default:
usage();
}
argc -= optind;
argv += optind;
if(argc != 1)
usage();
if((fd = open(argv[0], O_RDONLY | O_NONBLOCK)) < 0)
err(EX_UNAVAILABLE, "open(%s)", argv[0]);
if (ioctl(fd, FD_GDTYPE, &type) == -1)
err(EX_OSERR, "ioctl(FD_GDTYPE)");
if (ioctl(fd, FD_GTYPE, &ft) == -1)
err(EX_OSERR, "ioctl(FD_GTYPE)");
if (show) {
showdev(type, argv[0]);
return (0);
}
if (autofmt) {
memset(&newft, 0, sizeof newft);
ft = newft;
}
if (format) {
getname(type, &name, &descr);
fdtp = get_fmt(format, type);
if (fdtp == 0)
errx(EX_USAGE,
"unknown format %d KB for drive type %s",
format, name);
ft = *fdtp;
}
if (fmtstring) {
parse_fmt(fmtstring, type, ft, &newft);
ft = newft;
}
if (showfmt) {
if (verbose) {
const char *s;
printf("%s: %d KB media type\n", argv[0],
(128 << ft.secsize) * ft.size / 1024);
printf("\tFormat:\t\t");
print_fmt(ft);
if (ft.datalen != 0xff &&
ft.datalen != (128 << ft.secsize))
printf("\tData length:\t%d\n", ft.datalen);
printf("\tSector size:\t%d\n", 128 << ft.secsize);
printf("\tSectors/track:\t%d\n", ft.sectrac);
printf("\tHeads/cylinder:\t%d\n", ft.heads);
printf("\tCylinders/disk:\t%d\n", ft.tracks);
switch (ft.trans) {
case 0: printf("\tTransfer rate:\t500 kbps\n"); break;
case 1: printf("\tTransfer rate:\t300 kbps\n"); break;
case 2: printf("\tTransfer rate:\t250 kbps\n"); break;
case 3: printf("\tTransfer rate:\t1 Mbps\n"); break;
}
printf("\tSector gap:\t%d\n", ft.gap);
printf("\tFormat gap:\t%d\n", ft.f_gap);
printf("\tInterleave:\t%d\n", ft.f_inter);
printf("\tSide offset:\t%d\n", ft.offset_side2);
printf("\tFlags\t\t<");
s = "";
if (ft.flags & FL_MFM) {
printf("%sMFM", s);
s = ",";
}
if (ft.flags & FL_2STEP) {
printf("%s2STEP", s);
s = ",";
}
if (ft.flags & FL_PERPND) {
printf("%sPERPENDICULAR", s);
s = ",";
}
if (ft.flags & FL_AUTO) {
printf("%sAUTO", s);
s = ",";
}
printf(">\n");
} else {
print_fmt(ft);
}
return (0);
}
if (format || fmtstring) {
if (ioctl(fd, FD_STYPE, &ft) == -1)
err(EX_OSERR, "ioctl(FD_STYPE)");
return (0);
}
return 0;
}
int rtapi_app_main(void){
int i, f, f1, k, p;
int retval;
if (!fmt_strings[0]){
rtapi_print_msg(RTAPI_MSG_ERR, "The LCD component requires at least one valid format string");
return -EINVAL;
}
comp_id = hal_init("lcd");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "LCD: ERROR: hal_init() failed\n");
return -1;
}
// allocate shared memory for data
lcd = hal_malloc(sizeof(lcd_t));
if (lcd == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"lcd component: Out of Memory\n");
hal_exit(comp_id);
return -1;
}
// Count the instances. Very unlikely to be more than one, but...
for (lcd->num_insts = 0; fmt_strings[lcd->num_insts];lcd->num_insts++){}
lcd->insts = hal_malloc(lcd->num_insts * sizeof(lcd_inst_t));
for (i = 0; i < lcd->num_insts; i++){
lcd_inst_t *inst = &lcd->insts[i];
inst->num_pages = 1;
// count the pages demarked by | chars.
for (f = 0; fmt_strings[i][f]; f++){
if (fmt_strings[i][f] =='|') inst->num_pages++;
}
inst->pages = hal_malloc(inst->num_pages * sizeof(lcd_page_t));
//second pass
f1 = k = p = 0;
for (f = 0; fmt_strings[i][f]; f++){
if (fmt_strings[i][f] =='%') {
int type = parse_fmt(fmt_strings[i], &f, NULL, NULL, 0);
if (type > 0) {
inst->pages[p].num_args++;
}
}
if (fmt_strings[i][f + 1] =='|' || fmt_strings[i][f + 1] == 0) {
inst->pages[p].fmt = hal_malloc(f - f1 + 2);
retval = snprintf(inst->pages[p].fmt,
f - f1 + 2, "%s",
fmt_strings[i] + f1);
inst->pages[p].length = f - f1 + 2;
inst->pages[p].args = hal_malloc(inst->pages[p].num_args
* sizeof(hal_float_t));
f1 = f + 2;
{
int a = -1, s = -1;
lcd_page_t page = inst->pages[p];
while (page.fmt[++s]){
if (page.fmt[s] == '%'){
int type = parse_fmt(page.fmt, &s, NULL, NULL, 0);
a++;
switch (type){
case 'f':
retval = hal_pin_float_newf(HAL_IN,
(hal_float_t**)&(inst->pages[p].args[a]), comp_id,
"lcd.%02i.page.%02i.arg.%02i",
i, p, a);
if (retval != 0) {
return retval;
}
break;
case 'u':
case 'c':
retval = hal_pin_u32_newf(HAL_IN,
(hal_u32_t **)&(inst->pages[p].args[a]), comp_id,
"lcd.%02i.page.%02i.arg.%02i",
i, p, a);
if (retval != 0) {
return retval;
}
break;
case 's':
retval = hal_pin_s32_newf(HAL_IN,
(hal_s32_t **)&(inst->pages[p].args[a]), comp_id,
"lcd.%02i.page.%02i.arg.%02i",
i, p, a);
if (retval != 0) {
return retval;
}
break;
case 'b':
retval = hal_pin_bit_newf(HAL_IN,
(hal_bit_t **)&(inst->pages[p].args[a]), comp_id,
"lcd.%02i.page.%02i.arg.%02i",
i, p, a);
if (retval != 0) {
return retval;
}
break;
}
}
}
}
p++; // increment the page index
}
}
}
retval = hal_export_funct("lcd", write, lcd, 1, 0, comp_id); //needs fp?
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "LCD: ERROR: function export failed\n");
return -1;
}
for (i = 0; i < lcd->num_insts; i++){
retval = hal_pin_u32_newf(HAL_IN, &(lcd->insts[i].page_num), comp_id,
"lcd.%02i.page_num", i);
if (retval != 0) {
return retval;
}
lcd->insts[i].last_page = 0xFFFF; // force screen refresh
retval = hal_pin_u32_newf(HAL_OUT, &(lcd->insts[i].out), comp_id,
"lcd.%02i.out",i);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(lcd->insts[i].contrast), comp_id,
"lcd.%02i.contrast",i);
if (retval != 0) {
return retval;
}
retval = hal_param_u32_newf(HAL_RW, &(lcd->insts[i].dp), comp_id,
"lcd.%02i.decimal-separator",i);
lcd->insts[i].dp = '.';
if (retval != 0) {
return retval;
}
lcd->insts[i].f_ptr = 0;
lcd->insts[i].buff[0] = 0x11; // turn off cursor. More init can be added
lcd->insts[i].buff[0] = 0;
lcd->insts[i].c_ptr = 0;
}
return 0;
}
static void write_one(lcd_inst_t *inst){
int retval;
char c1, c2;
if (inst->buff[inst->c_ptr] != 0){
*inst->out = inst->buff[inst->c_ptr++];
return;
}
inst->c_ptr = 0;
inst->buff[0] = 0;
if (*inst->page_num != inst->last_page){
inst->last_page = *inst->page_num;
if (*inst->page_num >= inst->num_pages) return; // should this error?
*inst->out = 0x11; //cursor off
inst->buff[0] = 0x1A; //dummy
inst->buff[1] = 0; //end
inst->c_ptr = 0;
inst->f_ptr = 0;
inst->a_ptr = 0;
return;
}
if (inst->f_ptr > inst->pages[*inst->page_num].length){
*inst->out = 0x18; // clear line
inst->buff[0] = 0x1E; // home
inst->buff[1] = 0; //end
inst->c_ptr = 0;
inst->f_ptr = 0;
inst->a_ptr = 0;
return;
}
if (*inst->contrast != inst->last_contrast){
int c = *inst->contrast * 159.0 + 0x20;
if (c > 0xBF) c = 0xBF;
if (c < 0x20) c = 0x20;
inst->last_contrast = *inst->contrast;
*inst->out = 0x1B;
inst->buff[0] = 'C';
inst->buff[1] = c;
inst->buff[2] = 0;
inst->c_ptr = 0;
return;
}
switch (inst->pages[*inst->page_num].fmt[inst->f_ptr]){
case '\\': //escape chars
c1 = inst->pages[*inst->page_num].fmt[++inst->f_ptr];
switch (c1){
case 'n':
case 'N':
inst->f_ptr++;
inst->buff[0] = 0x18; //erase to end
inst->buff[1] = 0x0A; //CR
inst->buff[2] = 0x0D; //LF
inst->buff[3] = 0;
break;
case 't':
case 'T':
inst->f_ptr++;
inst->buff[0] = ' ';
inst->buff[1] = ' ';
inst->buff[2] = ' ';
inst->buff[3] = ' ';
inst->buff[4] = 0;
break;
case '\\':
inst->f_ptr++;
inst->buff[0] = '\\';
inst->buff[1] = 0;
default: //check for hex
c2 = inst->pages[*inst->page_num].fmt[++inst->f_ptr];
inst->f_ptr++;
c1 &= 0xDF; c2 &= 0xDF; //upper case
if (strchr(digits, c1) && strchr(digits, c2)){
inst->buff[0] = (16 * (strchr(digits, c1) - digits)
+ (strchr(digits, c2) - digits));
inst->buff[1] = 0;
}
}
*inst->out = inst->buff[0];
inst->c_ptr = 1;
return;
case '%':
retval = parse_fmt(inst->pages[*inst->page_num].fmt,
&inst->f_ptr,
inst->buff,
inst->pages[*inst->page_num].args[inst->a_ptr++],
(char)inst->dp);
if (retval >= 0) {
*inst->out = inst->buff[0];
inst->c_ptr = 1;
inst->f_ptr++;
return;
}
default:
*inst->out = inst->pages[*inst->page_num].fmt[inst->f_ptr++];
}
}
int binrpc_print_response(struct binrpc_response_handle *resp_handle, char* fmt)
{
unsigned char* p;
unsigned char* end;
struct binrpc_val val;
int ret;
int rec;
char *f;
char* s;
int f_size;
int fmt_has_values;
if (!resp_handle) {
goto error;
}
resp_handle->in_pkt.offset = resp_handle->in_pkt.in_struct = resp_handle->in_pkt.in_array = 0;
p=resp_handle->reply_buf;
end=p+resp_handle->in_pkt.tlen;
rec=0;
f=fmt;
fmt_has_values=0;
/* read body */
while(p<end){
if (f){
do{
if (*f==0)
f=fmt; /* reset */
s=f;
f=parse_fmt(f, &val.type, &f_size);
printf("%.*s", f_size, s);
if (val.type!=-1){
fmt_has_values=1;
goto read_value;
}
}while(*f || fmt_has_values);
val.type=BINRPC_T_ALL;
}else{
val.type=BINRPC_T_ALL;
}
read_value:
val.name.s=0;
val.name.len=0;
p=binrpc_read_record(&resp_handle->in_pkt, p, end, &val, &ret);
if (ret<0){
if (fmt)
putchar('\n');
/*if (ret==E_BINRPC_MORE_DATA)
goto error_read_again;*/
if (ret==E_BINRPC_EOP){
printf("end of message detected\n");
break;
}
snprintf(binrpc_last_errs, sizeof(binrpc_last_errs)-1,
"error while parsing the record %d,"
" @%d: %02x : %s", rec,
resp_handle->in_pkt.offset, *p, binrpc_error(ret));
goto error;
}
rec++;
if (fmt){
print_binrpc_val(&val, 0);
}else{
print_binrpc_val(&val, resp_handle->in_pkt.in_struct+resp_handle->in_pkt.in_array);
putchar('\n');
}
}
if (fmt && *f){
/* print the rest, with empty values */
while(*f){
s=f;
f=parse_fmt(f, &val.type, &f_size);
printf("%.*s", f_size, s);
}
}
return 0;
error:
return FATAL_ERROR;
/*error_read_again:
snprintf(binrpc_last_errs, sizeof(binrpc_last_errs)-1,
"ERROR: more data needed");
return -2;
*/
}
size_t flint_fprintf(FILE * f, const char * str, ...)
{
va_list ap;
size_t len = strlen(str);
char * str2 = flint_malloc(len + 1);
int w1 = 0, w2 = 0;
void * w3;
double d;
ulong wu;
slong w;
int args, floating;
size_t ret;
/* deal with first substring */
size_t n = strcspn(str, "%");
strncpy(str2, str, n);
str2[n] = '\0';
ret = fprintf(f, "%s", str2);
len -= n;
str += n;
va_start(ap, str);
while (len) /* deal with fmt spec prefixed strings */
{
n = strcspn(str + 2, "%") + 2; /* be sure to skip a %% */
strncpy(str2, str, n);
str2[n] = '\0';
switch (str[1])
{
case 'w':
if (str[2] == 'x')
{
wu = (ulong) va_arg(ap, ulong);
ret += fprintf(f, WORD_FMT "x", wu);
ret += fprintf(f, "%s", str2 + 3);
} else if (str[2] == 'u')
{
wu = (ulong) va_arg(ap, ulong);
ret += fprintf(f, WORD_FMT "u", wu);
ret += fprintf(f, "%s", str2 + 3);
} else if (str[2] == 'd')
{
w = (slong) va_arg(ap, slong);
ret += fprintf(f, WORD_FMT "d", w);
ret += fprintf(f, "%s", str2 + 3);
} else
{
w = (slong) va_arg(ap, slong);
ret += fprintf(f, WORD_FMT "d", w);
ret += fprintf(f, "%s", str2 + 2);
}
break;
default: /* pass to fprintf */
args = parse_fmt(&floating, str2);
if (args)
{
if (args == 3)
w1 = va_arg(ap, int);
if (args >= 2)
w2 = va_arg(ap, int);
if (floating)
{
d = va_arg(ap, double);
if (args == 2)
ret += fprintf(f, str2, w2, d);
else if (args == 3)
ret += fprintf(f, str2, w1, w2, d);
else
ret += fprintf(f, str2, d);
} else
{
w3 = va_arg(ap, void *);
if (args == 2)
ret += fprintf(f, str2, w2, w3);
else if (args == 3)
ret += fprintf(f, str2, w1, w2, w3);
else
ret += fprintf(f, str2, w3);
}
} else ret += fprintf(f, "%s", str2); /* zero args */
}
void
format_output (const char* buffer, com_table* ctp, struct rt_i *UNUSED(rtip))
{
const char* bp = buffer; /* was + 1; */
int fmt_type = FMT_NONE;
int i;
int use_defaults = 0;
void parse_fmt(const char* uoutspec, int outcom_type);
void show_ospec(outitem *oil);
/* Handle no args, arg=='?', and obvious bad arg */
if (*bp != '\0')
++bp;
while (isspace((int)*bp))
++bp;
switch (*bp) {
case 'r':
fmt_type = FMT_RAY;
break;
case 'h':
fmt_type = FMT_HEAD;
break;
case 'p':
fmt_type = FMT_PART;
break;
case 'f':
fmt_type = FMT_FOOT;
break;
case 'm':
fmt_type = FMT_MISS;
break;
case 'o':
fmt_type = FMT_OVLP;
break;
case 'g':
fmt_type = FMT_GAP;
break;
default:
--bp;
break;
}
while (isspace((int)*++bp))
;
switch (*bp) {
case '\0': /* display current output specs */
if (fmt_type == FMT_NONE)
fprintf(stderr, "Error: No output-statement type specified\n");
else
show_ospec(oi_list[fmt_type]);
return;
case '"':
if (fmt_type == FMT_NONE) {
fprintf(stderr, "Error: No output-statement type specified\n");
return;
}
break;
default:
if (bu_strncmp(bp, "default", 7) == 0) {
use_defaults = 1;
break;
}
fprintf(stderr, "Error: Illegal format specification: '%s'\n", buffer);
/* fall through here */
case '?':
com_usage(ctp);
return;
}
if (use_defaults) {
if (fmt_type == FMT_NONE) {
for (i = 0; i < FMT_NONE; ++i) {
parse_fmt(def_fmt[i], i);
}
} else {
parse_fmt(def_fmt[fmt_type], fmt_type);
}
} else {
parse_fmt(bp, fmt_type);
}
}
static int print_body(struct binrpc_parse_ctx* in_pkt,
unsigned char* body, int size, char* fmt)
{
unsigned char* p;
unsigned char* end;
struct binrpc_val val;
int ret;
int rec;
char *f;
char* s;
int f_size;
int fmt_has_values;
p=body;
end=p+size;
rec=0;
f=fmt;
fmt_has_values=0;
/* read body */
while(p<end){
if (f){
do{
if (*f==0)
f=fmt; /* reset */
s=f;
f=parse_fmt(f, &val.type, &f_size);
printf("%.*s", f_size, s);
if (val.type!=-1){
fmt_has_values=1;
goto read_value;
}
}while(*f || fmt_has_values);
val.type=BINRPC_T_ALL;
}else{
val.type=BINRPC_T_ALL;
}
read_value:
val.name.s=0;
val.name.len=0;
p=binrpc_read_record(in_pkt, p, end, &val, 1, &ret);
if (ret<0){
if (fmt)
putchar('\n');
/*if (ret==E_BINRPC_MORE_DATA)
goto error_read_again;*/
if (ret==E_BINRPC_EOP){
printf("end of message detected\n");
break;
}
fprintf(stderr, "ERROR:: while parsing the record %d,"
" @%d: %02x : %s\n", rec,
in_pkt->offset, *p, binrpc_error(ret));
goto error;
}
rec++;
if (fmt){
print_binrpc_val(&val, 0);
}else{
print_binrpc_val(&val, in_pkt->in_struct+in_pkt->in_array);
putchar('\n');
}
}
if (fmt && *f){
/* print the rest, with empty values */
while(*f){
s=f;
f=parse_fmt(f, &val.type, &f_size);
printf("%.*s", f_size, s);
}
}
return 0;
error:
return -1;
/*error_read_again:
fprintf(stderr, "ERROR: more data needed\n");
return -2;
*/
}
void bsdnt_printf(const char * str, ...)
{
va_list ap;
size_t len = strlen(str);
char * str2 = (char *) malloc(len + 1);
int w1 = 0, w2 = 0;
void * w3;
double d;
word_t w;
bits_t b;
len_t m;
int args, floating;
/* deal with first substring */
size_t n = strcspn(str, "%");
strncpy(str2, str, n);
str2[n] = '\0';
printf("%s", str2);
len -= n;
str += n;
va_start(ap, str);
while (len) /* deal with fmt spec prefixed strings */
{
n = strcspn(str + 2, "%") + 2; /* be sure to skip a %% */
strncpy(str2, str, n);
str2[n] = '\0';
switch (str[1])
{
case 'w':
w = (word_t) va_arg(ap, word_t);
if (str[2] == 'x')
{
printf(WORD_FMT "x", w);
printf("%s", str2 + 3);
} else
{
printf(WORD_FMT "d", w);
printf("%s", str2 + 2);
}
break;
case 'b':
b = (bits_t) va_arg(ap, bits_t);
printf(BITS_FMT, b);
printf("%s", str2 + 2);
break;
case 'm':
m = (len_t) va_arg(ap, len_t);
printf(LEN_FMT, m);
printf("%s", str2 + 2);
break;
default: /* pass to printf */
args = parse_fmt(&floating, str2);
if (args)
{
if (args == 3)
w1 = va_arg(ap, int);
if (args >= 2)
w2 = va_arg(ap, int);
if (floating)
{
d = va_arg(ap, double);
if (args == 2)
printf(str2, w2, d);
else if (args == 3)
printf(str2, w1, w2, d);
else
printf(str2, d);
} else
{
w3 = va_arg(ap, void *);
if (args == 2)
printf(str2, w2, w3);
else if (args == 3)
printf(str2, w1, w2, w3);
else
printf(str2, w3);
}
} else printf("%s", str2); /* zero args */
}
int
main(int argc, char **argv)
{
enum fd_drivetype type;
struct fd_type fdt, newft, *fdtp;
struct stat sb;
#define MAXPRINTERRS 10
struct fdc_status fdcs[MAXPRINTERRS];
int format, fill, quiet, verify, verify_only, confirm;
int fd, c, i, track, error, tracks_per_dot, bytes_per_track, errs;
int flags;
char *fmtstring, *device;
const char *name, *descr;
format = quiet = verify_only = confirm = 0;
verify = 1;
fill = 0xf6;
fmtstring = 0;
while((c = getopt(argc, argv, "F:f:nqs:vy")) != -1)
switch(c) {
case 'F': /* fill byte */
if (getnum(optarg, &fill)) {
fprintf(stderr,
"Bad argument %s to -F option; must be numeric\n",
optarg);
usage();
}
break;
case 'f': /* format in kilobytes */
if (getnum(optarg, &format)) {
fprintf(stderr,
"Bad argument %s to -f option; must be numeric\n",
optarg);
usage();
}
break;
case 'n': /* don't verify */
verify = 0;
break;
case 'q': /* quiet */
quiet = 1;
break;
case 's': /* format string with detailed options */
fmtstring = optarg;
break;
case 'v': /* verify only */
verify = 1;
verify_only = 1;
break;
case 'y': /* confirm */
confirm = 1;
break;
default:
usage();
}
if(optind != argc - 1)
usage();
if (stat(argv[optind], &sb) == -1 && errno == ENOENT) {
/* try prepending _PATH_DEV */
device = malloc(strlen(argv[optind]) + sizeof(_PATH_DEV) + 1);
if (device == 0)
errx(EX_UNAVAILABLE, "out of memory");
strcpy(device, _PATH_DEV);
strcat(device, argv[optind]);
if (stat(device, &sb) == -1) {
free(device);
device = argv[optind]; /* let it fail below */
}
} else {
device = argv[optind];
}
if ((fd = open(device, O_RDWR | O_NONBLOCK)) < 0)
err(EX_OSERR, "open(%s)", device);
/*
* Device initialization.
*
* First, get the device type descriptor. This tells us about
* the media geometry data we need to format a medium. It also
* lets us know quickly whether the device name actually points
* to a floppy disk drive.
*
* Then, obtain any drive options. We're mainly interested to
* see whether we're currently working on a device with media
* density autoselection (FDOPT_AUTOSEL). Then, we add the
* device option to tell the kernel not to log media errors,
* since we can handle them ourselves. If the device does
* media density autoselection, we then need to set the device
* type appropriately, since by opening with O_NONBLOCK we
* told the driver to bypass media autoselection (otherwise we
* wouldn't stand a chance to format an unformatted or damaged
* medium). We do not attempt to set the media type on any
* other devices since this is a privileged operation. For the
* same reason, specifying -f and -s options is only possible
* for autoselecting devices.
*
* Finally, we are ready to turn off O_NONBLOCK, and start to
* actually format something.
*/
if(ioctl(fd, FD_GTYPE, &fdt) < 0)
errx(EX_OSERR, "not a floppy disk: %s", device);
if (ioctl(fd, FD_GDTYPE, &type) == -1)
err(EX_OSERR, "ioctl(FD_GDTYPE)");
if (format) {
getname(type, &name, &descr);
fdtp = get_fmt(format, type);
if (fdtp == 0)
errx(EX_USAGE,
"unknown format %d KB for drive type %s",
format, name);
fdt = *fdtp;
}
if (fmtstring) {
parse_fmt(fmtstring, type, fdt, &newft);
fdt = newft;
}
if (ioctl(fd, FD_STYPE, &fdt) < 0)
err(EX_OSERR, "ioctl(FD_STYPE)");
if ((flags = fcntl(fd, F_GETFL, 0)) == -1)
err(EX_OSERR, "fcntl(F_GETFL)");
flags &= ~O_NONBLOCK;
if (fcntl(fd, F_SETFL, flags) == -1)
err(EX_OSERR, "fcntl(F_SETFL)");
bytes_per_track = fdt.sectrac * (128 << fdt.secsize);
/* XXX 20/40 = 0.5 */
tracks_per_dot = (fdt.tracks * fdt.heads + 20) / 40;
if (verify_only) {
if(!quiet)
printf("Verify %dK floppy `%s'.\n",
fdt.tracks * fdt.heads * bytes_per_track / 1024,
device);
}
else if(!quiet && !confirm) {
printf("Format %dK floppy `%s'? (y/n): ",
fdt.tracks * fdt.heads * bytes_per_track / 1024,
device);
if(!yes()) {
printf("Not confirmed.\n");
return (EX_UNAVAILABLE);
}
}
/*
* Formatting.
*/
if(!quiet) {
printf("Processing ");
for (i = 0; i < (fdt.tracks * fdt.heads) / tracks_per_dot; i++)
putchar('-');
printf("\rProcessing ");
fflush(stdout);
}
error = errs = 0;
for (track = 0; track < fdt.tracks * fdt.heads; track++) {
if (!verify_only) {
format_track(fd, track / fdt.heads, fdt.sectrac,
track % fdt.heads, fdt.trans, fdt.f_gap,
fdt.secsize, fill, fdt.f_inter,
track % fdt.heads? fdt.offset_side2: 0);
if(!quiet && !((track + 1) % tracks_per_dot)) {
putchar('F');
fflush(stdout);
}
}
if (verify) {
if (verify_track(fd, track, bytes_per_track) < 0) {
error = 1;
if (errs < MAXPRINTERRS && errno == EIO) {
if (ioctl(fd, FD_GSTAT, fdcs + errs) ==
-1)
errx(EX_IOERR,
"floppy IO error, but no FDC status");
errs++;
}
}
if(!quiet && !((track + 1) % tracks_per_dot)) {
if (!verify_only)
putchar('\b');
if (error) {
putchar('E');
error = 0;
}
else
putchar('V');
fflush(stdout);
}
}
}
if(!quiet)
printf(" done.\n");
if (!quiet && errs) {
fflush(stdout);
fprintf(stderr, "Errors encountered:\nCyl Head Sect Error\n");
for (i = 0; i < errs && i < MAXPRINTERRS; i++) {
fprintf(stderr, " %2d %2d %2d ",
fdcs[i].status[3], fdcs[i].status[4],
fdcs[i].status[5]);
printstatus(fdcs + i, 1);
putc('\n', stderr);
}
if (errs >= MAXPRINTERRS)
fprintf(stderr, "(Further errors not printed.)\n");
}
return errs != 0;
}
