//-------------------------------------------------------------------
unsigned int parse_two_regs ( unsigned int ra )
{
ra=parse_reg(ra); if(ra==0) return(0);
rd=rx;
ra=parse_comma(ra); if(ra==0) return(0);
ra=parse_reg(ra); if(ra==0) return(0);
rs=rx;
return(ra);
}
static void timer_cb(const char *dtb, int node, int parent)
{
#ifdef DEBUG
printf("DTB: found timer device: ");
parse_reg(dtb, node, parent, dump_cb);
printf("\n");
#endif
parse_reg(dtb, node, parent, timer_reg_cb);
}
// Prints the value of the register on a new line.
void cmd_reg(char **argv){
c16_halfword n;
if ((n = parse_regno(argv[0])) == REG_DNE){
printf("register '%s' does not exist\n",argv[0]);
return;
}
if (n > OP_r9){
printf("%s = 0x%02x\n",reg_strs[n],*((c16_subreg) parse_reg(n)));
}else{
printf("%s = 0x%04x\n",reg_strs[n],*((c16_reg) parse_reg(n)));
}
}
//-------------------------------------------------------------------
unsigned int parse_three_shift ( unsigned int ra )
{
ra=parse_reg(ra); if(ra==0) return(0);
rd=rx;
ra=parse_comma(ra); if(ra==0) return(0);
ra=parse_reg(ra); if(ra==0) return(0);
rt=rx;
ra=parse_comma(ra); if(ra==0) return(0);
ra=parse_reg(ra); if(ra==0) return(0);
rs=rx;
return(ra);
}
//-------------------------------------------------------------------
unsigned int parse_two_label ( unsigned int ra )
{
ra=parse_reg(ra); if(ra==0) return(0);
rt=rx;
ra=parse_comma(ra); if(ra==0) return(0);
ra=parse_reg(ra); if(ra==0) return(0);
rs=rx;
ra=parse_comma(ra); if(ra==0) return(0);
ra=parse_branch_label(ra); if(ra==0) return(0);
rd=rx;
return(ra);
}
uint32_t parse_regex(t_parse_reg *p, uint32_t offset, t_reg **reg,
char end)
{
t_reg *r;
t_reg *tmp;
uint32_t start;
uint32_t next_end;
*reg = NULL;
r = NULL;
next_end = (end == '\0') ? p->len : ft_subchr_e(SUB_P(p), offset, end);
while (offset < next_end)
{
start = offset;
if ((offset = ft_subchr_e(SUB(p->str, p->len), offset, '?')) > next_end)
offset = next_end;
if (start != offset)
tmp = create_reg_str(SUB(p->str + start, offset - start));
else if ((offset = parse_reg(p, offset + 1, &tmp)) == REG_FAIL)
return (F**K(destroy_reg(*reg), (*reg = NULL), REG_FAIL));
if (offset >= next_end && end != '\0')
next_end = ft_subchr_e(SUB(p->str, p->len), offset, end);
r = append_reg_next(r, tmp);
if (*reg == NULL)
*reg = r;
}
return ((end == '\0' || offset < p->len) ? offset : F**K((void)REG_ERROR(p,
"Unclosed group", offset), destroy_reg(*reg), (*reg = NULL), REG_FAIL));
}
static void parse_node(const char *dtb, int node,
void (*mem_cb)(uint32_t addr, uint32_t length))
{
int parent = node;
node = fdt_first_subnode(dtb, node);
while(node >= 0)
{
const void *dt;
int plen;
dt = fdt_getprop(dtb, node, "device_type", &plen);
#ifdef DEBUG_DTB
const char *nn;
nn = fdt_get_name(dtb, node, NULL);
if(dt != NULL && nn != NULL)
{
printf("DTB: found device_type property for %s: %s\n", nn, (const char *)dt);
}
else if(nn != NULL)
printf("DTB: found node %s\n", nn);
else
printf("DTB: unknown node\n");
#endif
if(dt != NULL && !strcmp((const char *)dt, "memory"))
{
parse_reg(dtb, node, parent, mem_cb);
}
parse_node(dtb, node, mem_cb);
node = fdt_next_subnode(dtb, node);
}
}
static inline vm_word assemble_format4(uint8_t opcode,char *instr){
while(is_space(*instr)){instr++;}
uint8_t reg=parse_reg(&instr);
//scan to comma
vm_word imm=parse_imm(&instr);
vm_op retval={.op_reg_imm={.op=opcode,.imm=imm,.reg=reg}}
}
int get_reg(enum Regs *reg, enum Token token, const char *msg)
{
if (token == Symbol && parse_reg(reg, token_buf)) return 1;
printf("%d: error: %s required: %s\n", curline, msg ? msg : "register", token_buf);
if (token != EndL) read_line();
return 0;
}
static inline vm_word assemble_format3(uint8_t opcode,char *instr){
while(is_space(*instr)){instr++;}
uint8_t reg=parse_reg(&instr);
if(reg == (uint8_t)-1){
return -1;
}
vm_op retval;
retval.op_reg.reg=reg;
retval.op_reg.op=opcode;
return retval.bits;
}
// Prints the value stored at the memory address regstr.
// If true, then print as halfword, otherwise print as word.
void print_memreg(char *regstr,bool w){
c16_halfword reg = parse_regno(regstr);
c16_word v;
int b = reg >= HALFREG_START;
int l = (b) ? 2 : 4;
void *r = parse_reg(reg);
if (reg == REG_DNE){
printf("register: '%s': does not exist\n");
return;
}
v = (b) ? *((c16_subreg) r) : *((c16_reg) r);
printf("%s: *%s (0x%0*x) = 0x%0*x\n",(w) ? "halfword" : "word",regstr,l,v,
(b) ? 2 : 4,(b) ? sysmem.mem[v] : *((c16_word*) &sysmem.mem[v]));
}
static int cmd_write(int argc, char **argv) {
int id = -1;
int reg8 = -1;
int reg16 = -1;
if (argc != 4) {
printf("usage; %s <dev_id> <reg> <value>\n", argv[0]);
print_registers();
return 1;
}
/* parse parameters */
id = parse_dev(argv[1]);
if (id < 0) {
return -1;
}
parse_reg(argv[2], ®8, ®16);
if (reg8 < 0 && reg16 < 0) {
return -1;
}
int val = atoi(argv[3]);
if (val < 0) {
return -1;
}
/* read */
feetech_t dev;
feetech_init(&dev, &stream, id);
if (reg8 >= 0) {
int ret = feetech_write8(&dev, reg8, val);
if (ret != FEETECH_OK) {
printf("Error[%i] : No response from %i\n", ret, id);
return -1;
}
printf("Written %i at address %i\n", (int)val, reg8);
}
else {
int ret = feetech_write16(&dev, reg16, val);
if (ret != FEETECH_OK) {
printf("Error[%i] : No response from %i\n", ret, id);
return -1;
}
printf("Written %i at address %i\n", (int)val, reg16);
}
return 0;
}
static int cmd_read(int argc, char **argv) {
int id = -1;
int reg8 = -1;
int reg16 = -1;
if (argc != 3) {
printf("usage; %s <dev_id> <reg>\n", argv[0]);
print_registers();
return 1;
}
/* parse parameters */
id = parse_dev(argv[1]);
if (id < 0) {
return -1;
}
parse_reg(argv[2], ®8, ®16);
if (reg8 < 0 && reg16 < 0) {
return -1;
}
/* read */
feetech_t dev;
feetech_init(&dev, &stream, id);
if (reg8 >= 0) {
uint8_t val = 0;
int ret = feetech_read8(&dev, reg8, &val);
if (ret != FEETECH_OK) {
printf("Error[%i] : No response from %i\n", ret, id);
return -1;
}
printf("%i\n", (int)val);
}
else {
uint16_t val = 0;
int ret = feetech_read16(&dev, reg16, &val);
if (ret != FEETECH_OK) {
printf("Error[%i] : No response from %i\n", ret, id);
return -1;
}
printf("%i\n", (int)val);
}
return 0;
}
int parse_arg(const char *arg,sh_operand_info *op, char *err_msg)
// Parse arg and return a filled operand struct
{
int len,mode;
if(arg[0] == 0)
{
op->type = 0;
op->reg = 0;
return 1;
}
if(*arg == '@')
{
arg++;
return parse_at(arg,op, err_msg);
}
if(*arg == '#')
{
op->type = A_IMM;
return 1;
}
len = parse_reg(arg,&mode,&(op->reg));
if(len)
{
op->type = mode;
return 1;
}
else
{
op->type = A_BDISP12;
return 1;
}
return 0;
}
void dump_tree(const char *dtb, int node, int parent, int indent)
{
const char *node_name = fdt_get_name(dtb, node, NULL);
const char *compat = fdt_getprop(dtb, node, "compatible", NULL);
if(node_name)
{
for(int i = 0; i < indent; i++)
printf(" ");
printf(node_name);
if(compat)
printf("(%s)", compat);
printf(" ");
parse_reg(dtb, node, parent, dump_cb);
printf("\n");
}
int subnode;
fdt_for_each_subnode(subnode, dtb, node)
dump_tree(dtb, subnode, node, indent + 1);
}
void parse_bra(enum Op op)
{
enum Token token = read_token();
enum Regs ra;
if (token == Symbol && parse_reg(&ra, token_buf))
{
read_sign(",");
token = read_token();
}
else
{
if (op != Br)
{
printf("%d: error: register required: %s\n", curline, token_buf);
return;
}
ra = Zero;
}
switch (token)
{
case Num:
{
int64_t ad1 = (int64_t)(curad + 4);
int64_t ad2 = (int64_t)token_num;
int diff = (int)(ad2 - ad1);
if ((diff & 3) != 0)
printf("%d: error: not align 4: %s\n", curline, token_buf);
else
assemble_bra(op, ra, diff >> 2);
break;
}
case Symbol:
printf("%d: error: label is not implemented: %s\n", curline, token_buf);
break;
default:
printf("%d: error: address or label required: %s\n", curline, token_buf);
break;
}
}
//-------------------------------------------------------------------
//-------------------------------------------------------------------
int assemble ( void )
{
unsigned int ra;
unsigned int rb;
unsigned int rc;
curradd=0;
nlabs=0;
memset(mem,0x00,sizeof(mem));
memset(mark,0x00,sizeof(mark));
line=0;
while(fgets(newline,sizeof(newline)-1,fpin))
{
line++;
//tabs to spaces and other things
for(ra=0;newline[ra];ra++)
{
if(newline[ra]<0x20) newline[ra]=0x20;
if(newline[ra]>=0x7F) newline[ra]=0;
}
//various ways to comment lines
for(ra=0;newline[ra];ra++)
{
if(newline[ra]==';') newline[ra]=0;
if(newline[ra]=='@') newline[ra]=0;
if((newline[ra]=='/')&&(newline[ra+1]=='/')) newline[ra]=0;
if(newline[ra]==0) break;
}
//skip spaces
for(ra=0;newline[ra];ra++) if(newline[ra]!=0x20) break;
if(newline[ra]==0) continue;
//look for a label?
for(rb=ra;newline[rb];rb++)
{
if(newline[rb]==0x20) break; //no spaces in labels
if(newline[rb]==':') break;
}
if(newline[rb]==':')
{
//got a label
rc=rb-ra;
if(rc==0)
{
printf("<%u> Error: Invalid label\n",line);
return(1);
}
rc--;
if(rc>=LABLEN)
{
printf("<%u> Error: Label too long\n",line);
return(1);
}
for(rb=0;rb<=rc;rb++)
{
lab_struct[nlabs].name[rb]=newline[ra++];
}
lab_struct[nlabs].name[rb]=0;
lab_struct[nlabs].addr=0x3000+curradd;
lab_struct[nlabs].line=line;
lab_struct[nlabs].type=0;
ra++;
for(lab=0;lab<nlabs;lab++)
{
if(lab_struct[lab].type) continue;
if(strcmp(lab_struct[lab].name,lab_struct[nlabs].name)==0) break;
}
if(lab<nlabs)
{
printf("<%u> Error: label [%s] already defined on line %u\n",line,lab_struct[lab].name,lab_struct[lab].line);
return(1);
}
nlabs++;
//skip spaces
for(;newline[ra];ra++) if(newline[ra]!=0x20) break;
if(newline[ra]==0) continue;
}
// .word -----------------------------------------------------------
if(strncmp(&newline[ra],".word ",6)==0)
{
ra+=6;
ra=parse_immed(ra); if(ra==0) return(1);
mem[curradd]=rx;
mark[curradd]|=0x80000000;
curradd++;
if(rest_of_line(ra)) return(1);
continue;
}
// add -----------------------------------------------------------
if(strncmp(&newline[ra],"add ",4)==0)
{
ra+=4;
//add rd,rs,rx
//add rd,rs,#imm
ra=parse_two_regs(ra); if(ra==0) return(1);
ra=parse_comma(ra); if(ra==0) return(1);
if(newline[ra]=='#')
{
ra=parse_immed(ra+1); if(ra==0) return(1);
if(check_simmed5(rx)) return(1);
mem[curradd]=0x1020|(rd<<9)|(rs<<6)|(rx&0x1F);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
ra=parse_reg(ra); if(ra==0) return(1);
mem[curradd]=0x1000|(rd<<9)|(rs<<6)|rx;
mark[curradd]|=0x80000000;
curradd++;
}
if(rest_of_line(ra)) return(1);
continue;
}
// and -----------------------------------------------------------
if(strncmp(&newline[ra],"and ",4)==0)
{
ra+=4;
//and rd,rs,rx
//and rd,rs,#imm
ra=parse_two_regs(ra); if(ra==0) return(1);
ra=parse_comma(ra); if(ra==0) return(1);
if(newline[ra]=='#')
{
ra=parse_immed(ra+1); if(ra==0) return(1);
if(check_simmed5(rx)) return(1);
mem[curradd]=0x5020|(rd<<9)|(rs<<6)|(rx&0x1F);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
ra=parse_reg(ra); if(ra==0) return(1);
mem[curradd]=0x5000|(rd<<9)|(rs<<6)|rx;
mark[curradd]|=0x80000000;
curradd++;
}
if(rest_of_line(ra)) return(1);
continue;
}
// jmp -----------------------------------------------------------
if(strncmp(&newline[ra],"jmp ",4)==0)
{
ra+=4;
//jmp rs
ra=parse_reg(ra); if(ra==0) return(1);
mem[curradd]=0xC000|(rx<<6);
mark[curradd]|=0x80000000;
curradd++;
if(rest_of_line(ra)) return(1);
continue;
}
// jsrr -----------------------------------------------------------
if(strncmp(&newline[ra],"jsrr ",5)==0)
{
ra+=5;
//jsrr rs
ra=parse_reg(ra); if(ra==0) return(1);
mem[curradd]=0x4000|(rx<<6);
mark[curradd]|=0x80000000;
curradd++;
if(rest_of_line(ra)) return(1);
continue;
}
// not -----------------------------------------------------------
if(strncmp(&newline[ra],"not ",4)==0)
{
ra+=4;
//not rd,rs
ra=parse_two_regs(ra); if(ra==0) return(1);
mem[curradd]=0x9000|(rd<<9)|(rs<<6);
mark[curradd]|=0x80000000;
curradd++;
if(rest_of_line(ra)) return(1);
continue;
}
// ret -----------------------------------------------------------
if(strncmp(&newline[ra],"ret",3)==0)
{
ra+=3;
//ret
mem[curradd]=0xC1C0;
mark[curradd]|=0x80000000;
curradd++;
if(rest_of_line(ra)) return(1);
continue;
}
// rti -----------------------------------------------------------
if(strncmp(&newline[ra],"rti",3)==0)
{
ra+=3;
//rti
mem[curradd]=0x8000;
mark[curradd]|=0x80000000;
curradd++;
if(rest_of_line(ra)) return(1);
continue;
}
// brn -----------------------------------------------------------
if(strncmp(&newline[ra],"brn ",4)==0)
{
ra+=4;
//brn offset/label
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: branch too far\n",line);
return(1);
}
mem[curradd]=0x0800|(rx&0x1FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x0800;
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// brz -----------------------------------------------------------
if(strncmp(&newline[ra],"brz ",4)==0)
{
ra+=4;
//brz offset/label
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: branch too far\n",line);
return(1);
}
mem[curradd]=0x0400|(rx&0x1FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x0400;
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// brp -----------------------------------------------------------
if(strncmp(&newline[ra],"brp ",4)==0)
{
ra+=4;
//brp offset/label
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: branch too far\n",line);
return(1);
}
mem[curradd]=0x0200|(rx&0x1FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x0200;
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// brnz -----------------------------------------------------------
if(strncmp(&newline[ra],"brnz ",5)==0)
{
ra+=5;
//brnzp offset/label
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: branch too far\n",line);
return(1);
}
mem[curradd]=0x0C00|(rx&0x1FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x0C00;
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// brnp -----------------------------------------------------------
if(strncmp(&newline[ra],"brnp ",5)==0)
{
ra+=5;
//brnp offset/label
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: branch too far\n",line);
return(1);
}
mem[curradd]=0x0A00|(rx&0x1FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x0A00;
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// brzp -----------------------------------------------------------
if(strncmp(&newline[ra],"brzp ",5)==0)
{
ra+=5;
//brzp offset/label
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: branch too far\n",line);
return(1);
}
mem[curradd]=0x0600|(rx&0x1FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x0600;
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// brnzp -----------------------------------------------------------
if(strncmp(&newline[ra],"brnzp ",6)==0)
{
ra+=6;
//brnzp offset/label
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: branch too far\n",line);
return(1);
}
mem[curradd]=0x0E00|(rx&0x1FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x0E00;
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// jsr -----------------------------------------------------------
if(strncmp(&newline[ra],"jsr ",4)==0)
{
ra+=4;
//jsr offset/label
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset11(rx))
{
printf("<%u> Error: jsr too far\n",line);
return(1);
}
mem[curradd]=0x4800|(rx&0x07FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x4800;
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// ld -----------------------------------------------------------
if(strncmp(&newline[ra],"ld ",3)==0)
{
ra+=3;
//ld rd,offset/label
ra=parse_reg(ra); if(ra==0) return(1);
rd=rx;
ra=parse_comma(ra); if(ra==0) return(1);
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: ld too far\n",line);
return(1);
}
mem[curradd]=0x2000|(rd<<9)|(rx&0x01FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x2000|(rd<<9);
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// ldi -----------------------------------------------------------
if(strncmp(&newline[ra],"ldi ",4)==0)
{
ra+=4;
//ldi rd,offset/label
ra=parse_reg(ra); if(ra==0) return(1);
rd=rx;
ra=parse_comma(ra); if(ra==0) return(1);
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: ldi too far\n",line);
return(1);
}
mem[curradd]=0xA000|(rd<<9)|(rx&0x01FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0xA000|(rd<<9);
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// ldr -----------------------------------------------------------
if(strncmp(&newline[ra],"ldr ",4)==0)
{
ra+=4;
//ldr rd,rs,#offset
ra=parse_two_regs(ra); if(ra==0) return(1);
ra=parse_comma(ra); if(ra==0) return(1);
ra=parse_pound(ra); if(ra==0) return(1);
ra=parse_immed(ra); if(ra==0) return(1);
if(rest_of_line(ra)) return(1);
if(check_simmed6(rx)) return(1);
mem[curradd]=0x6000|(rd<<9)|(rs<<6)|(rx&0x003F);
mark[curradd]|=0x80000000;
curradd++;
continue;
}
// lea -----------------------------------------------------------
if(strncmp(&newline[ra],"lea ",4)==0)
{
ra+=4;
//ldi rd,offset/label
ra=parse_reg(ra); if(ra==0) return(1);
rd=rx;
ra=parse_comma(ra); if(ra==0) return(1);
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: lea too far\n",line);
return(1);
}
mem[curradd]=0xE000|(rd<<9)|(rx&0x01FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0xE000|(rd<<9);
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// st -----------------------------------------------------------
if(strncmp(&newline[ra],"st ",3)==0)
{
ra+=3;
//st rd,offset/label
ra=parse_reg(ra); if(ra==0) return(1);
rd=rx;
ra=parse_comma(ra); if(ra==0) return(1);
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: st too far\n",line);
return(1);
}
mem[curradd]=0x3000|(rd<<9)|(rx&0x01FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0x3000|(rd<<9);
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// sti -----------------------------------------------------------
if(strncmp(&newline[ra],"sti ",4)==0)
{
ra+=4;
//sti rd,offset/label
ra=parse_reg(ra); if(ra==0) return(1);
rd=rx;
ra=parse_comma(ra); if(ra==0) return(1);
ra=parse_branch_label(ra); if(ra==0) return(0);
if(rest_of_line(ra)) return(1);
if(is_const)
{
if(check_offset9(rx))
{
printf("<%u> Error: sti too far\n",line);
return(1);
}
mem[curradd]=0xB000|(rd<<9)|(rx&0x01FF);
mark[curradd]|=0x80000000;
curradd++;
}
else
{
mem[curradd]=0xB000|(rd<<9);
mark[curradd]|=0x80000002;
curradd++;
}
continue;
}
// str -----------------------------------------------------------
if(strncmp(&newline[ra],"str ",4)==0)
{
ra+=4;
//str rd,rs,#offset
ra=parse_two_regs(ra); if(ra==0) return(1);
ra=parse_comma(ra); if(ra==0) return(1);
ra=parse_pound(ra); if(ra==0) return(1);
ra=parse_immed(ra); if(ra==0) return(1);
if(rest_of_line(ra)) return(1);
if(check_simmed6(rx)) return(1);
mem[curradd]=0x7000|(rd<<9)|(rs<<6)|(rx&0x003F);
mark[curradd]|=0x80000000;
curradd++;
continue;
}
// trap -----------------------------------------------------------
if(strncmp(&newline[ra],"trap ",5)==0)
{
ra+=5;
//trap imm
ra=parse_immed(ra); if(ra==0) return(1);
if((rx&0xFF)!=rx)
{
printf("<%u> Error: invalid trap vector\n",line);
return(1);
}
mem[curradd]=0xF000|(rx&0xFF);
mark[curradd]|=0x80000000;
curradd++;
if(rest_of_line(ra)) return(1);
continue;
}
// -----------------------------------------------------------
printf("<%u> Error: syntax error\n",line);
return(1);
}
return(0);
}
int
parse_inst(char *label, char *inst, char args[][16])
{
int i=0;
int j;
WORD a[3];
int len;
/* find the instruction */
while( instructions[i].inst ) {
if( !strcasecmp(instructions[i].inst,inst) ) break;
i++;
}
/* check if we had a match */
if( !instructions[i].inst ) {
/* didn't find the instruction */
pendvm_error("undefined instruction");
return -1;
}
/* we have a match. process each of the 3 args */
for(j=0;j<3;j++) {
switch(instructions[i].args[j]) {
case NIL:
a[j]=0;
break;
case REG: {
int reg=parse_reg(args[j]);
if(reg==-1) {
pendvm_error("expected register");
return -1;
} else if(reg==-2) {
pendvm_error("register out of range");
return -1;
} else {
a[j]=reg;
}
break;
}
case AMT:
case IMM: {
int *immed;
if( instructions[i].args[j]==AMT )
len=16;
else
len=5; /* ??? */
immed=parse_immed(args[j],len);
if( !immed ) {
pendvm_error("bad immediate/amt");
return -1;
} else {
a[j]=*immed;
}
break;
}
case OFF:
case LOFF: {
int *absolute;
if( instructions[i].args[j]==OFF )
len=16;
else
len=26; /* ??? */
absolute=parse_immed(args[j],len);
if( !absolute ) {
pendvm_error("bad offset");
return -1;
} else {
a[j]=*absolute - m->PC;
}
break;
}
} /* switch */
} /* for (step through 3 args) */
/* now do the right thing */
return (*instructions[i].func)(a[0],a[1],a[2]);
}
static void
get_operand (char **ptr, struct z8k_op *mode, unsigned int dst ATTRIBUTE_UNUSED)
{
char *src = *ptr;
char *end;
mode->mode = 0;
while (*src == ' ')
src++;
if (*src == '#')
{
mode->mode = CLASS_IMM;
imm_operand = &(mode->exp);
src = parse_exp (src + 1, &(mode->exp));
}
else if (*src == '@')
{
mode->mode = CLASS_IR;
src = parse_reg (src + 1, &mode->regsize, &mode->reg);
}
else
{
unsigned int regn;
end = parse_reg (src, &mode->mode, ®n);
if (end)
{
int nw;
unsigned int nr;
src = end;
if (*src == '(')
{
src++;
end = parse_reg (src, &nw, &nr);
if (end)
{
/* Got Ra(Rb). */
src = end;
if (*src != ')')
as_bad (_("Missing ) in ra(rb)"));
else
src++;
regaddr (mode->mode, "ra(rb) ra");
mode->mode = CLASS_BX;
mode->reg = regn;
mode->x_reg = nr;
reg[ARG_RX] = nr;
}
else
{
/* Got Ra(disp). */
if (*src == '#')
src++;
src = parse_exp (src, &(mode->exp));
src = checkfor (src, ')');
mode->mode = CLASS_BA;
mode->reg = regn;
mode->x_reg = 0;
imm_operand = &(mode->exp);
}
}
else
{
mode->reg = regn;
mode->x_reg = 0;
}
}
else
{
/* No initial reg. */
src = parse_exp (src, &(mode->exp));
if (*src == '(')
{
src++;
end = parse_reg (src, &(mode->mode), ®n);
regword (mode->mode, "addr(Ra) ra");
mode->mode = CLASS_X;
mode->reg = regn;
mode->x_reg = 0;
da_operand = &(mode->exp);
src = checkfor (end, ')');
}
else
{
/* Just an address. */
mode->mode = CLASS_DA;
mode->reg = 0;
mode->x_reg = 0;
da_operand = &(mode->exp);
}
}
}
*ptr = src;
}
int parse_at(const char *arg,sh_operand_info *op, char *err_msg)
// Parse pointer arguement and return a operand info struct
{
int mode;
int len;
if(arg[0] == 0)
return 0;
if(*arg == '-')
{
arg++;
len = parse_reg(arg,&mode,&(op->reg));
if(len == 0)
{
asm_bad("Cant find arg", err_msg);
return 0;
}
if(mode != A_REG_N)
{
asm_bad("Invalid reg after @-", err_msg);
return 0;
}
else
{
op->type = A_DEC_N;
}
}
else
if(*arg == '(')
{
arg++;
len = parse_reg(arg,&mode,&(op->reg));
if((len > 0) && (mode == A_REG_N))
{
arg+=len;
if(op->reg != 0)
{
asm_bad("Must be @(R0,...)", err_msg);
return 0;
}
if(arg[0] == ',')
arg++;
len = parse_reg(arg,&mode,&(op->reg));
arg += len;
if(mode == A_GBR)
{
op->type = A_R0_GBR;
}
else if (mode == A_REG_N)
{
op->type = A_IND_R0_REG_N;
}
else
{
asm_bad("Syntax error in @(R0,...)", err_msg);
return 0;
}
}
else
{
while((*(arg-1) != ',') && (*arg != 0))
arg++;
len = parse_reg(arg,&mode,&(op->reg));
arg+=len;
if(len)
{
if(mode == A_REG_N)
{
op->type = A_DISP_REG_N;
}
else if (mode == A_GBR)
{
op->type = A_DISP_GBR;
}
else if (mode == A_DISP_PC)
{
op->type = A_DISP_PC;
}
else
{
asm_bad("Bad syntax in @(disp,[Rn,GBR,PC])", err_msg);
return 0;
}
}
else
{
asm_bad("Bad syntax in @(disp,[Rn,GBR,PC])", err_msg);
return 0;
}
}
if(*arg != ')')
{
asm_bad("Expected a )", err_msg);
return 0;
}
}
else
{
arg += parse_reg(arg,&mode,&(op->reg));
if(mode != A_REG_N)
{
asm_bad("Invalid register after @", err_msg);
return 0;
}
if(arg[0] == '+')
{
op->type = A_INC_N;
}
else
{
op->type = A_IND_N;
}
}
return 1;
}
int main(int argc, char **argv)
{
char *value, *subs;
int i, forcedstride = 0;
int fd = -1;
/* command args */
int ch;
const char *device = "/dev/video0"; /* -d device */
struct v4l2_capability vcap; /* list_cap */
struct v4l2_dbg_register set_reg;
struct v4l2_dbg_register get_reg;
struct v4l2_dbg_chip_info chip_info;
const struct board_list *curr_bd = NULL;
char short_options[26 * 2 * 3 + 1];
int idx = 0;
std::string reg_min_arg, reg_max_arg;
std::string reg_set_arg;
unsigned long long reg_min = 0, reg_max = 0;
std::vector<std::string> get_regs;
struct v4l2_dbg_match match;
char *p;
match.type = V4L2_CHIP_MATCH_BRIDGE;
match.addr = 0;
memset(&set_reg, 0, sizeof(set_reg));
memset(&get_reg, 0, sizeof(get_reg));
if (argc == 1) {
usage();
return 0;
}
for (i = 0; long_options[i].name; i++) {
if (!isalpha(long_options[i].val))
continue;
short_options[idx++] = long_options[i].val;
if (long_options[i].has_arg == required_argument) {
short_options[idx++] = ':';
} else if (long_options[i].has_arg == optional_argument) {
short_options[idx++] = ':';
short_options[idx++] = ':';
}
}
while (1) {
int option_index = 0;
short_options[idx] = 0;
ch = getopt_long(argc, argv, short_options,
long_options, &option_index);
if (ch == -1)
break;
options[(int)ch] = 1;
switch (ch) {
case OptHelp:
usage();
return 0;
case OptSetDevice:
device = optarg;
if (device[0] >= '0' && device[0] <= '9' && strlen(device) <= 3) {
static char newdev[20];
sprintf(newdev, "/dev/video%s", device);
device = newdev;
}
break;
case OptChip:
if (!memcmp(optarg, "subdev", 6) && isdigit(optarg[6])) {
match.type = V4L2_CHIP_MATCH_SUBDEV;
match.addr = strtoul(optarg + 6, NULL, 0);
break;
}
if (!memcmp(optarg, "bridge", 6)) {
match.type = V4L2_CHIP_MATCH_BRIDGE;
match.addr = strtoul(optarg + 6, NULL, 0);
break;
}
match.type = V4L2_CHIP_MATCH_BRIDGE;
match.addr = 0;
break;
case OptSetRegister:
reg_set_arg = optarg;
break;
case OptGetRegister:
get_regs.push_back(optarg);
break;
case OptSetStride:
forcedstride = strtoull(optarg, 0L, 0);
break;
case OptListRegisters:
subs = optarg;
if (subs == NULL)
break;
while (*subs != '\0') {
static const char * const subopts[] = {
"min",
"max",
NULL
};
switch (parse_subopt(&subs, subopts, &value)) {
case 0:
reg_min_arg = value;
//if (reg_max == 0)
// reg_max = reg_min + 0xff;
break;
case 1:
reg_max_arg = value;
break;
}
}
break;
case OptListSymbols:
break;
case ':':
fprintf(stderr, "Option `%s' requires a value\n",
argv[optind]);
usage();
return 1;
case '?':
fprintf(stderr, "Unknown argument `%s'\n",
argv[optind]);
usage();
return 1;
}
}
if ((fd = open(device, O_RDWR)) < 0) {
fprintf(stderr, "Failed to open %s: %s\n", device,
strerror(errno));
exit(1);
}
doioctl(fd, VIDIOC_QUERYCAP, &vcap, "VIDIOC_QUERYCAP");
capabilities = vcap.capabilities;
/* Information Opts */
if (options[OptGetDriverInfo]) {
printf("Driver info:\n");
printf("\tDriver name : %s\n", vcap.driver);
printf("\tCard type : %s\n", vcap.card);
printf("\tBus info : %s\n", vcap.bus_info);
printf("\tDriver version: %d.%d.%d\n",
vcap.version >> 16,
(vcap.version >> 8) & 0xff,
vcap.version & 0xff);
printf("\tCapabilities : 0x%08X\n", vcap.capabilities);
printf("%s", cap2s(vcap.capabilities).c_str());
}
chip_info.name[0] = '\0';
if (options[OptChip]) {
/* try to figure out which chip it is */
chip_info.match = match;
if (doioctl(fd, VIDIOC_DBG_G_CHIP_INFO, &chip_info, "VIDIOC_DBG_G_CHIP_INFO") != 0)
chip_info.name[0] = '\0';
if (!strncasecmp(match.name, "ac97", 4)) {
curr_bd = &boards[AC97_BOARD];
} else {
for (int board = ARRAY_SIZE(boards) - 1; board >= 0; board--) {
if (!strcasecmp(chip_info.name, boards[board].name)) {
curr_bd = &boards[board];
break;
}
}
}
}
/* Set options */
if (options[OptSetRegister]) {
set_reg.match = match;
if (optind >= argc)
usage();
set_reg.reg = parse_reg(curr_bd, reg_set_arg);
while (optind < argc) {
unsigned size = 0;
if (doioctl(fd, VIDIOC_DBG_G_REGISTER, &set_reg,
"VIDIOC_DBG_G_REGISTER") >= 0)
size = set_reg.size;
set_reg.val = strtoull(argv[optind++], NULL, 0);
if (doioctl(fd, VIDIOC_DBG_S_REGISTER, &set_reg,
"VIDIOC_DBG_S_REGISTER") >= 0) {
const char *name = reg_name(curr_bd, set_reg.reg);
printf("Register ");
if (name)
printf("%s (0x%08llx)", name, set_reg.reg);
else
printf("0x%08llx", set_reg.reg);
printf(" set to 0x%llx\n", set_reg.val);
} else {
printf("Failed to set register 0x%08llx value 0x%llx: %s\n",
set_reg.reg, set_reg.val, strerror(errno));
}
set_reg.reg += size ? : (forcedstride ? : 1);
}
}
if (options[OptScanChips]) {
chip_info.match.type = V4L2_CHIP_MATCH_BRIDGE;
chip_info.match.addr = 0;
while (doioctl(fd, VIDIOC_DBG_G_CHIP_INFO, &chip_info, "VIDIOC_DBG_G_CHIP_INFO") == 0 && chip_info.name[0]) {
printf("bridge%d: ", chip_info.match.addr);
print_name(&chip_info);
chip_info.match.addr++;
}
chip_info.match.type = V4L2_CHIP_MATCH_SUBDEV;
chip_info.match.addr = 0;
while (doioctl(fd, VIDIOC_DBG_G_CHIP_INFO, &chip_info, "VIDIOC_DBG_G_CHIP_INFO") == 0 && chip_info.name[0]) {
printf("subdev%d: ", chip_info.match.addr++);
print_name(&chip_info);
}
}
if (options[OptGetRegister]) {
get_reg.match = match;
printf("ioctl: VIDIOC_DBG_G_REGISTER\n");
for (std::vector<std::string>::iterator iter = get_regs.begin();
iter != get_regs.end(); ++iter) {
get_reg.reg = parse_reg(curr_bd, *iter);
if (ioctl(fd, VIDIOC_DBG_G_REGISTER, &get_reg) < 0)
fprintf(stderr, "ioctl: VIDIOC_DBG_G_REGISTER "
"failed for 0x%llx\n", get_reg.reg);
else {
const char *name = reg_name(curr_bd, get_reg.reg);
printf("Register ");
if (name)
printf("%s (0x%08llx)", name, get_reg.reg);
else
printf("0x%08llx", get_reg.reg);
printf(" = %llxh (%lldd %sb)\n",
get_reg.val, get_reg.val, binary(get_reg.val));
}
}
}
if (options[OptListRegisters]) {
std::string name;
int stride = 1;
get_reg.match = match;
if (forcedstride) {
stride = forcedstride;
} else if (get_reg.match.type == V4L2_CHIP_MATCH_BRIDGE) {
stride = 4;
}
printf("ioctl: VIDIOC_DBG_G_REGISTER\n");
if (curr_bd) {
if (reg_min_arg.empty())
reg_min = 0;
else
reg_min = parse_reg(curr_bd, reg_min_arg);
if (reg_max_arg.empty())
reg_max = (1ll << 32) - 1;
else
reg_max = parse_reg(curr_bd, reg_max_arg);
for (int i = 0; i < curr_bd->regs_size; i++) {
if (reg_min_arg.empty() || ((curr_bd->regs[i].reg >= reg_min) && curr_bd->regs[i].reg <= reg_max)) {
get_reg.reg = curr_bd->regs[i].reg;
if (ioctl(fd, VIDIOC_DBG_G_REGISTER, &get_reg) < 0)
fprintf(stderr, "ioctl: VIDIOC_DBG_G_REGISTER "
"failed for 0x%llx\n", get_reg.reg);
else {
const char *name = reg_name(curr_bd, get_reg.reg);
printf("Register ");
if (name)
printf("%s (0x%08llx)", name, get_reg.reg);
else
printf("0x%08llx", get_reg.reg);
printf(" = %llxh (%lldd %sb)\n",
get_reg.val, get_reg.val, binary(get_reg.val));
}
}
}
goto list_done;
}
if (!reg_min_arg.empty()) {
reg_min = parse_reg(curr_bd, reg_min_arg);
if (reg_max_arg.empty())
reg_max = reg_min + 0xff;
else
reg_max = parse_reg(curr_bd, reg_max_arg);
/* Explicit memory range: just do it */
print_regs(fd, &get_reg, reg_min, reg_max, stride);
goto list_done;
}
p = strchr(chip_info.name, ' ');
if (p)
*p = '\0';
name = chip_info.name;
if (name == "saa7115") {
print_regs(fd, &get_reg, 0, 0xff, stride);
} else if (name == "saa717x") {
// FIXME: use correct reg regions
print_regs(fd, &get_reg, 0, 0xff, stride);
} else if (name == "saa7127") {
print_regs(fd, &get_reg, 0, 0x7f, stride);
} else if (name == "ov7670") {
print_regs(fd, &get_reg, 0, 0x89, stride);
} else if (name == "cx25840") {
print_regs(fd, &get_reg, 0, 2, stride);
print_regs(fd, &get_reg, 0x100, 0x15f, stride);
print_regs(fd, &get_reg, 0x200, 0x23f, stride);
print_regs(fd, &get_reg, 0x400, 0x4bf, stride);
print_regs(fd, &get_reg, 0x800, 0x9af, stride);
} else if (name == "cs5345") {
print_regs(fd, &get_reg, 1, 0x10, stride);
} else if (name == "cx23416") {
print_regs(fd, &get_reg, 0x02000000, 0x020000ff, stride);
} else if (name == "cx23418") {
print_regs(fd, &get_reg, 0x02c40000, 0x02c409c7, stride);
} else if (name == "cafe") {
print_regs(fd, &get_reg, 0, 0x43, stride);
print_regs(fd, &get_reg, 0x88, 0x8f, stride);
print_regs(fd, &get_reg, 0xb4, 0xbb, stride);
print_regs(fd, &get_reg, 0x3000, 0x300c, stride);
} else {
/* unknown chip, dump 0-0xff by default */
print_regs(fd, &get_reg, 0, 0xff, stride);
}
}
list_done:
if (options[OptLogStatus]) {
static char buf[40960];
int len = -1;
if (doioctl(fd, VIDIOC_LOG_STATUS, NULL, "VIDIOC_LOG_STATUS") == 0) {
printf("\nStatus Log:\n\n");
#ifdef HAVE_KLOGCTL
len = klogctl(3, buf, sizeof(buf) - 1);
#endif
if (len >= 0) {
bool found_status = false;
char *p = buf;
char *q;
buf[len] = 0;
while ((q = strstr(p, "START STATUS"))) {
found_status = true;
p = q + 1;
}
if (found_status) {
while (p > buf && *p != '<') p--;
q = p;
while ((q = strstr(q, "<6>"))) {
memcpy(q, " ", 3);
}
printf("%s", p);
}
}
}
}
if (options[OptListSymbols]) {
if (curr_bd == NULL) {
printf("No symbols found for driver %s\n", vcap.driver);
}
else {
printf("Symbols for driver %s:\n", curr_bd->name);
for (int i = 0; i < curr_bd->regs_size; i++)
printf("0x%08x: %s\n", curr_bd->regs[i].reg, curr_bd->regs[i].name);
for (int i = 0; i < curr_bd->alt_regs_size; i++)
printf("0x%08x: %s\n", curr_bd->alt_regs[i].reg, curr_bd->alt_regs[i].name);
}
}
close(fd);
exit(0);
}
