int main(int argc, char **argv)
{
char line[BUFSIZE];
short int buf[BUFSIZE];
unsigned char buf_pos = 0;
short int number;
int sdapin = 0, sclpin = 0;
if (argc < 2)
errx(1, "usage: si2c-bytewrite <sdapin> <sclpin>");
sdapin = atoi(argv[1]);
sclpin = atoi(argv[2]);
snprintf(sdastr, 64, "/sys/class/gpio/gpio%d/value", sdapin);
snprintf(sclstr, 64, "/sys/class/gpio/gpio%d/value", sclpin);
while (fgets(line, BUFSIZE, stdin) != NULL) {
if (sscanf(line, "%hi\n", &number) == 1) {
if (buf_pos <= BUFSIZE) {
buf[buf_pos++] = number;
}
}
if (strcmp(line, "push\n") == 0) {
push_data(buf, buf_pos);
buf_pos = 0;
}
}
push_data(buf, buf_pos);
return 0;
}
static void
check_headers (void)
{
/* push the cmml start tag */
flow = push_data ("preamble", PREAMBLE, strlen (PREAMBLE));
fail_unless_equals_flow_return (flow, GST_FLOW_OK);
/* push the stream tag */
flow = push_data ("stream", STREAM_TAG, strlen (STREAM_TAG));
fail_unless_equals_flow_return (flow, GST_FLOW_OK);
/* push the head tag */
flow = push_data ("head", HEAD_TAG, strlen (HEAD_TAG));
fail_unless_equals_flow_return (flow, GST_FLOW_OK);
/* should output 3 buffers: the ident, preamble and head headers */
fail_unless_equals_int (g_list_length (buffers), 3);
/* check the ident header */
check_output_buffer_is_equal ("cmml-ident-buffer", IDENT_HEADER, 1);
/* check the cmml processing instruction */
check_output_buffer_is_equal ("cmml-preamble-buffer", PREAMBLE_ENCODED, 1);
/* check the encoded head tag */
check_output_buffer_is_equal ("head-tag-buffer", HEAD_TAG_ENCODED, 1);
}
/*---------------------------------------------------------------------------*
* NAME: push_hash
* DESC: add a hash in the linked list hash structure
*---------------------------------------------------------------------------*/
void push_hash(config *conf, unsigned int id, unsigned int type) {
struct struct_hash *hash;
struct struct_hash *prev = NULL;
/* dummy hash values equal to zero for the hash computation */
unsigned int dummy_int = 0x00000000;
/* debug */
debug(1, "<-----------------------[enter]\n");
/* go to the last hash */
for(hash = conf->hash;hash;hash = hash->next) {
debug(3, "current struct hash of block[%d]\n", hash->id);
prev = hash; /* save the preview */
}
debug(3," add hash structure\n");
/* create a hash entry in the linked list */
hash = malloc_(sizeof(struct struct_hash));
/* update the previous block's field next */
if (prev)
prev->next = hash;
/* fill the other fields */
hash->id = id; /* used next to find the struct_block */
hash->block = NULL; /* init the struct_block pointer */
hash->offset = conf->buf_fuzz_size; /* current size of the buffer */
hash->next = NULL; /* init the next pointer */
hash->type = type; /* put the correct hash type */
/* update if it's the first hash */
if (!conf->hash)
conf->hash = hash;
/* add the correct number of byte in the fuzz buffer */
switch(type) {
case HF_BLOCK_CRC32_B : /* big-endian 32 bits */
push_data(conf, (unsigned char *) &dummy_int, sizeof(int));
break;
case HF_BLOCK_CRC32_L : /* little-endian 32 bits */
push_data(conf, (unsigned char *) &dummy_int, sizeof(int));
break;
default: /* should never happen */
break;
}
/* debug */
debug(1, "<-----------------------[quit]\n");
}
*cdecl cpx_init( XCPB *Xcpb )
{
OBJECT *tree;
xcpb = Xcpb;
if( xcpb->booting )
{
old_value = cur_value = save_vars;
Set_Printer( &cur_value );
return( ( CPXINFO *)TRUE );
}
else
{
appl_init();
AES_Version = _GemParBlk.global[0];
if( !xcpb->SkipRshFix )
(*xcpb->rsh_fix)( NUM_OBS, NUM_FRSTR, NUM_FRIMG, NUM_TREE,
rs_object, rs_tedinfo, rs_strings,
rs_iconblk, rs_bitblk, rs_frstr, rs_frimg,
rs_trindex, rs_imdope );
/* Get the current printer configuration port settings
* Currently, we use these values,
* Should we instead slam the hardware with OUR settings instead?
*/
Get_Printer( &cur_value );
old_value = cur_value;
push_data();
open_vwork();
close_vwork();
gl_ncolors = work_out[13];
if( !xcpb->SkipRshFix )
{
if(( AES_Version >= 0x0330 )
&& ( gl_ncolors > LWHITE ))
{
tree = (OBJECT *)rs_trindex[PRINTER];
MakeActivator( PRINTER, PSAVE );
MakeActivator( PRINTER, POK );
MakeActivator( PRINTER, PCANCEL );
}
}
cpxinfo.cpx_call = cpx_call;
cpxinfo.cpx_draw = NULL;
cpxinfo.cpx_wmove = NULL;
cpxinfo.cpx_timer = NULL;
cpxinfo.cpx_key = NULL;
cpxinfo.cpx_button = NULL;
cpxinfo.cpx_m1 = NULL;
cpxinfo.cpx_m2 = NULL;
cpxinfo.cpx_hook = NULL;
cpxinfo.cpx_close = NULL;
return( &cpxinfo );
}
}
/* 向数据包队列中插入数据, 这些数据会按照数据包的结构, 即两个字节的长度紧跟着内容, 分析这些数据有多少个数据包.
* 对于其中的完整数据包将插入到完整包队列中去, 对于其中不完整的包将插入到不完整数据包的哈希表中去. 特别是对于
* 长度只有 1 个字节的不完整包, 此时已经无法计算出数据包的长度, 因而不完整数据包的 read 为 -1 , 而 header 为此一个字节值.
* 当下一次套接字中接收到数据时会判断 read 的长度, 从而正确拼接数据包.
*
* 参数: L 是虚拟机栈; fd 是数据所属的套接字 id; buffer 是数据内容; size 是数据大小;
* 函数无返回值 */
static void
push_more(lua_State *L, int fd, uint8_t *buffer, int size) {
if (size == 1) {
struct uncomplete * uc = save_uncomplete(L, fd);
uc->read = -1;
uc->header = *buffer;
return;
}
int pack_size = read_size(buffer);
buffer += 2;
size -= 2;
/* 虽然数据包的内容是不完整的, 但是给 pack.buffer 分配的内存是完整的 */
if (size < pack_size) {
struct uncomplete * uc = save_uncomplete(L, fd);
uc->read = size;
uc->pack.size = pack_size;
uc->pack.buffer = skynet_malloc(pack_size);
memcpy(uc->pack.buffer, buffer, size);
return;
}
push_data(L, fd, buffer, pack_size, 1);
buffer += pack_size;
size -= pack_size;
if (size > 0) {
push_more(L, fd, buffer, size);
}
}
void J_PXC_Color_Stream_Processor::alert_mirror_status_change(){
if(M_stream_buffer.empty()){
return;
}
reverse_color_stream(3);
push_data();
}
void spi_dw_isr(void *arg)
{
struct device *dev = (struct device *)arg;
const struct spi_dw_config *info = dev->config->config_info;
u32_t error = 0;
u32_t int_status;
int_status = read_isr(info->regs);
SYS_LOG_DBG("SPI int_status 0x%x - (tx: %d, rx: %d)",
int_status, read_txflr(info->regs), read_rxflr(info->regs));
if (int_status & DW_SPI_ISR_ERRORS_MASK) {
error = 1;
goto out;
}
if (int_status & DW_SPI_ISR_RXFIS) {
pull_data(dev);
}
if (int_status & DW_SPI_ISR_TXEIS) {
push_data(dev);
}
out:
clear_interrupts(info->regs);
completed(dev, error);
}
void DLL EncodeFrame(EncoderJob &jobSpec, MediaFrame &frame) { // Send a frame of video and audio to the encoder
// Check job:
if (!jobSpec.IsValid) return;
push_video(jobSpec, frame); // <-- may change fragment here
push_audio(jobSpec, frame);
push_data(jobSpec, frame);
}
int send_pid(int serv_pid, int pid_cond)
{
int pid;
char *my_pid;
pid = getpid();
my_pid = setnbr(pid);
push_data(serv_pid, my_pid, pid_cond);
free(my_pid);
return (0);
}
/* This signal callback triggers when appsrc needs data. Here, we add an idle handler * to the mainloop to start pushing data into the appsrc */
static void start_feed(GstElement *source, guint size, CustomData *data) {
if (data->sourceid == 0) {
g_print("Start feeding\n");
push_data(data);
//data->sourceid = 1;
//data->sourceid = g_idle_add((GSourceFunc) push_data, data);
g_print("data->sourceid :%u", data->sourceid);
}
}
void* receive_thread(void *arg)
{
struct receive_param rcv_para = *((struct receive_param *)arg);
PMIFI_PACKET packet, resp;
u8 sum;
int len;
const int buff_len = 1024;
packet = (PMIFI_PACKET)malloc(buff_len);
resp = (PMIFI_PACKET)malloc(buff_len);
memset(packet, 0x0, buff_len);
while (read_packet(rcv_para.sd, packet) != ERROR) {
DBG_OUT("Process received packet");
len = get_packet_len(packet);
sum = get_checksum((u8*)packet, len - 1);
DBG_OUT("len = %d, recv sum = 0x%02x, calc sum = 0x%02x", len, ((u8*)packet)[len - 1], sum);
dump_packet(packet);
if (((u8*)packet)[len - 1] != sum)
DBG_OUT("*** check sum fail");
if (is_client_response(packet->func) == 0) {
handle_packet(rcv_para.sd, packet);
len = server_build_response(packet, resp);
DBG_OUT("build response len is %d", len);
if (len > 0) {
DBG_OUT("enqueue packet to queue");
push_data(rcv_para.sd, (u8*)resp, len);
}
handle_packet_post(rcv_para.sd, packet);
} else {
DBG_OUT("It's a response packet from client, ignored.");
}
memset(packet, 0x0, buff_len);
} /* while(read_packet) */
DBG_OUT("terminated thread 0x%lx", (unsigned long)pthread_self());
for (len = 0; len < ARRAY_SIZE(dev_map); len++)
{
if (dev_map[len].sd == rcv_para.sd)
{
dev_map[len].valid = 0;
break;
}
}
free(packet);
free(resp);
pthread_exit((void *)0);
return NULL;
}
/* Select_Ok
*==========================================================================
* Called when OK button is pressed or close_window occurs
*/
void
Select_Ok( void )
{
int port;
int temp_port;
store = ( BUFFER *)(*xcpb->Get_Buffer)();
temp_port = Get_Active_Port();
for( port = 0; port < Num_Ports; port++ )
{
Write_Port( port, store );
store++;
}
Modem.cur_port = Set_Active_Port( temp_port );
push_data();
}
static GstFlowReturn
push_clip (const gchar * name, const gchar * track,
const gchar * start, const gchar * end)
{
gchar *clip;
GstFlowReturn res;
if (end != NULL)
clip = g_strdup_printf (ENDED_CLIP_TEMPLATE, name, track, start, end);
else
clip = g_strdup_printf (CLIP_TEMPLATE, name, track, start);
res = push_data (name, clip, strlen (clip));
g_free (clip);
return res;
}
uint32_t ft_nf_ind_exe_cmd(uint8_t cmd[], uint8_t cmdnr, void *buf,
uint32_t size, uint32_t dir)
{
bool ddrmode, dummy_rbb_en;
uint16_t indirect_ctrl;
ddrmode = NFI.flashIntfConfig.bits.nfif == 1;
ddrmode = 0;
switch (cmd[0]) {
case NFC_CMD_GET_FEATURE:
case NFC_CMD_READ_ID:
case NFC_CMD_READ_STATUS:
dummy_rbb_en = true;
break;
default:
dummy_rbb_en = false;
break;
}
ft_nf_prepare_cmd(cmd, ddrmode ? (size << 1) : size);
indirect_ctrl = (dir == DIR_OUT) ? NFC_INDIRECT_OUT : 0;
indirect_ctrl |= dummy_rbb_en ? 0 : NFC_INDIRECT_DUMMY_RBB;
indirect_ctrl |= NFC_INDIRECT_ACCESS_START | (cmdnr -1);
NFI.indirAcesConfig.all = indirect_ctrl;
/* If dummy_rbb_en, need set dummy_rbb_en signal */
if (dummy_rbb_en) {
ft_udelay(10);
NFI.indirAcesConfig.bits.dummy_RBB = 1;
}
if (size) {
if (dir == DIR_OUT) {
return push_data(buf, size, ddrmode);
} else {
if (wait_for_op_done())
return -1;
else
return pop_data(buf, size, ddrmode);
}
}
return 0;
}
static void
_recovered_from_fec_base (guint32 ssrc, guint8 fec_pt,
guint8 lost_pt, guint16 lost_seq,
gconstpointer fec_packet, gsize fec_packet_size,
gconstpointer lost_packet, gsize lost_packet_size)
{
GstHarness *h = harness_rtpulpfecdec (ssrc, lost_pt, 123);
RecoveredPacketInfo info = {.pt = lost_pt,.ssrc = ssrc,.seq = lost_seq };
GList *expected = expect_recovered_packets (h, &info, 1);
push_data (h, fec_packet, fec_packet_size);
lose_and_recover_test (h, lost_seq, lost_packet, lost_packet_size);
check_rtpulpfecdec_stats (h, 1, 0);
g_list_free (expected);
gst_harness_teardown (h);
}
int main(int ac, char **av, char **env)
{
int serv_pid;
int pid_cond;
if (env[0] == NULL)
return (1);
if (ac != 3)
{
write(2, "Wrong usage : ./client PID data\n", 32);
return (1);
}
cond = 0;
pid_cond = 1;
signal(SIGUSR2, pause_prog);
serv_pid = my_getnbr(av[1]);
send_pid(serv_pid, pid_cond);
pid_cond = 0;
push_data(serv_pid, av[2], pid_cond);
return (0);
}
int handle_packet_post(int sd, PMIFI_PACKET packet)
{
int /*n,*/ datalen, packetlen;
u16 func = packet->func;
PMIFI_PACKET p;
u8 sum;
switch (func) {
case MIFI_USR_CHECK:
{
//char *url = "http://baike.baidu.com/";
struct PACK_ALIGN(1) {
u16 bytes;
u32 time;
} allow;
datalen = sizeof(macadr_t) + sizeof(allow);
packetlen = sizeof(MIFI_PACKET ) + datalen;
p = (PMIFI_PACKET)malloc(packetlen + 1);
memcpy(p, packet, sizeof(MIFI_PACKET));
p->func = SERV_SET_PERMIT;
p->sn_packet = htonl(get_packet_sn());
p->datalen = htons(datalen);
memcpy(p->data, packet->data, sizeof(macadr_t));
allow.bytes = htons(50); // 50M
allow.time = htons(3600); // 1 hours
memcpy(p->data + sizeof(macadr_t), &allow, sizeof(allow));
sum = get_checksum((u8 *)p, packetlen);
*(((u8 *)p) + packetlen) = sum;
push_data(sd, (u8 *)p, packetlen + 1);
}
break;
}
return 0;
}
/// 从 buffer 中读取数据. 如果 buffer 的数据足够, 那么数据内容会压入到 queue.queue 中; 否则会压入到 queue.hash 中.
static void
push_more(lua_State *L, int fd, uint8_t *buffer, int size) {
// 如果连包头的数据都不足够, 那么先读取包头的第一个字节数据
if (size == 1) {
struct uncomplete * uc = save_uncomplete(L, fd);
uc->read = -1;
uc->header = *buffer;
return;
}
// 读取数据包数据大小
int pack_size = read_size(buffer);
// 偏移到实际数据内容指针
buffer += 2;
// 得到实际数据的内容大小
size -= 2;
// 数据内容未能完全读取的情况
if (size < pack_size) {
struct uncomplete * uc = save_uncomplete(L, fd);
uc->read = size; // 记录已经读取的数据大小
uc->pack.size = pack_size; // 记录数据内容的大小
uc->pack.buffer = skynet_malloc(pack_size);
memcpy(uc->pack.buffer, buffer, size);
return;
}
// 如果数据内容能够完全读取, 那么读取数据, 并且压入到 queue.queue 中
push_data(L, fd, buffer, pack_size, 1);
// 继续读取剩余的函数
buffer += pack_size;
size -= pack_size;
if (size > 0) {
push_more(L, fd, buffer, size);
}
}
int send_message_to_user(emd_dev_client_t *client, int msg)
{
int ret = 0;
unsigned long flags = 0;
spin_lock_irqsave(&client->lock, flags);
// 1. Push data to fifo
ret = push_data(client, msg);
// 2. Wake up read function
if( sizeof(int) == ret ){
wake_up_interruptible(&client->wait_q);
ret = 0;
}
else
{
EMD_MSG_INF("chr","send_message_to_user,push_data ret=%d!!\n",ret);
}
spin_unlock_irqrestore(&client->lock, flags);
return ret;
}
/* [lua_api] 将套接字发送过来的数据返回给 Lua 层使用. 函数首先将检查 queue 的不完整数据包哈希表中是否存在套接字 fd 的
* 不完整数据包, 如果有则检查其已经读取的内容长度, 并将剩余部分复制过去, 如果刚好是一个数据包大小就直接返回 "data" 字符
* 串和数据包给 Lua 层, 如果复制后多于一个数据包, 则将这些数据包一起插入到 queue 中, 并返回 "more" 字符串给 Lua 层.
* 在不足一个数据包的情况下, 将不完整包重新插入到哈希表中. 对于之前没有不完整包的情况执行相同的流程. 当返回 "more" 时,
* 可以通过 lpop 函数取得 queue 中的数据包. buffer 中的内容会复制到新的内存块中, 因而在调用完此函数之后需要释放 buffer 的内存;
*
* 参数: L 是虚拟机栈, 其位置一就是 queue 数据结构; fd 是套接字 id; buffer 是数据内容; size 是数据大小;
*
* 返回: userdata[1] 为 queue 数据结构; string/nil[2] 为 "more" 或者 "data" 表示有数据, nil 表示数据不完整;
* int[3] 为套接字 id, 仅在 "data" 下返回; lightuserdata[4] 为数据内容, 仅在 "data" 下返回;
* int[5] 为数据大小, 仅在 "data" 下返回; */
static int
filter_data_(lua_State *L, int fd, uint8_t * buffer, int size) {
struct queue *q = lua_touserdata(L,1);
struct uncomplete * uc = find_uncomplete(q, fd);
if (uc) {
// fill uncomplete
if (uc->read < 0) {
// read size
assert(uc->read == -1);
int pack_size = *buffer;
pack_size |= uc->header << 8 ;
++buffer;
--size;
uc->pack.size = pack_size;
uc->pack.buffer = skynet_malloc(pack_size);
uc->read = 0;
}
int need = uc->pack.size - uc->read;
if (size < need) {
memcpy(uc->pack.buffer + uc->read, buffer, size);
uc->read += size;
int h = hash_fd(fd);
uc->next = q->hash[h];
q->hash[h] = uc;
return 1;
}
memcpy(uc->pack.buffer + uc->read, buffer, need);
buffer += need;
size -= need;
if (size == 0) {
lua_pushvalue(L, lua_upvalueindex(TYPE_DATA));
lua_pushinteger(L, fd);
lua_pushlightuserdata(L, uc->pack.buffer);
lua_pushinteger(L, uc->pack.size);
skynet_free(uc);
return 5;
}
// more data
push_data(L, fd, uc->pack.buffer, uc->pack.size, 0);
skynet_free(uc);
push_more(L, fd, buffer, size);
lua_pushvalue(L, lua_upvalueindex(TYPE_MORE));
return 2;
} else {
if (size == 1) {
struct uncomplete * uc = save_uncomplete(L, fd);
uc->read = -1;
uc->header = *buffer;
return 1;
}
int pack_size = read_size(buffer);
buffer+=2;
size-=2;
if (size < pack_size) {
struct uncomplete * uc = save_uncomplete(L, fd);
uc->read = size;
uc->pack.size = pack_size;
uc->pack.buffer = skynet_malloc(pack_size);
memcpy(uc->pack.buffer, buffer, size);
return 1;
}
if (size == pack_size) {
// just one package
lua_pushvalue(L, lua_upvalueindex(TYPE_DATA));
lua_pushinteger(L, fd);
void * result = skynet_malloc(pack_size);
memcpy(result, buffer, size);
lua_pushlightuserdata(L, result);
lua_pushinteger(L, size);
return 5;
}
// more data
push_data(L, fd, buffer, pack_size, 1);
buffer += pack_size;
size -= pack_size;
push_more(L, fd, buffer, size);
lua_pushvalue(L, lua_upvalueindex(TYPE_MORE));
return 2;
}
}
int cmd_handle(int UNUSED(sd), char *line)
{
int argc, func;
char *argv[10];
dev_info_t *pdev;
argc = make_argv(line, ARRAY_SIZE(argv), argv);
if (argc <= 0)
return ERROR;
DBG_OUT("argc is %d", argc);
func = get_cmdid(argv[0]);
if (func < 0) {
printf("unknown command: %s\r\n", argv[0]);
return ERROR;
}
switch (func) {
case SERV_REQ_KICKCLI:
case SERV_REQ_REBOOT:
case SERV_REQ_FACTORY:
{
PMIFI_PACKET p;
int i, datalen, packetlen;
u8 sum;
i = 0;
datalen = 0;
packetlen = sizeof(MIFI_PACKET ) + datalen;
p = (PMIFI_PACKET)malloc(packetlen + 1);
if (argc == 2) {
devid_t devid;
hex2bin((u8 *)argv[1], (u8 *)&devid, sizeof(devid_t));
i = find_device_by_id(&devid);
if (i < 0) {
printf("cannot find device: %s\r\n", argv[1]);
break;
}
}
pdev = get_device(i);
build_packet_header(p, pdev, func);
p->datalen = htons(datalen);
sum = get_checksum((u8 *)p, packetlen);
*(((u8 *)p) + packetlen) = sum;
push_data(pdev->sd, (u8 *)p, packetlen + 1);
free(p);
}
break;
case SERV_REQ_UPGRADE:
{
PMIFI_PACKET p;
char *url = "http://url.cn/QyCLQu";
int i, url_len, datalen, packetlen;
u8 sum;
i = 0;
url_len = strlen(url);
datalen = url_len + 2;
packetlen = sizeof(MIFI_PACKET ) + datalen;
p = (PMIFI_PACKET)malloc(packetlen + 1);
pdev = get_device(i);
build_packet_header(p, pdev, func);
p->datalen = htons(datalen);
p->data[0] = ((u8*)&url_len)[1];
p->data[1] = ((u8*)&url_len)[0];
memcpy(p->data + 2, url, datalen);
sum = get_checksum((u8 *)p, packetlen);
*(((u8 *)p) + packetlen) = sum;
push_data(pdev->sd, (u8 *)p, packetlen + 1);
free(p);
}
break;
case SERV_REQ_KICKUSR:
{
PMIFI_PACKET p;
int i, datalen, packetlen;
u8 sum;
i = 0;
datalen = sizeof(macadr_t);
packetlen = sizeof(MIFI_PACKET ) + datalen;
p = (PMIFI_PACKET)malloc(packetlen + 1);
if (argc == 3) {
devid_t devid;
hex2bin((u8 *)argv[2], (u8 *)&devid, sizeof(devid_t));
i = find_device_by_id(&devid);
if (i < 0) {
printf("cannot find device: %s\r\n", argv[2]);
break;
}
}
pdev = get_device(i);
build_packet_header(p, pdev, func);
p->datalen = htons(datalen);
if (argc == 1) {
i = find_first_valid_user(pdev);
} else {
macadr_t user;
hex2bin((u8 *)argv[1], (u8 *)&user, sizeof(macadr_t));
i = find_user(pdev, &user);
if (i < 0) {
printf("cannot find user: %s\r\n", argv[1]);
break;
}
}
memcpy(p->data, pdev->users[i], datalen);
clean_user(&pdev->users[i]); // should be clean after got the response from client
sum = get_checksum((u8 *)p, packetlen);
*(((u8 *)p) + packetlen) = sum;
push_data(pdev->sd, (u8 *)p, packetlen + 1);
free(p);
}
break;
case MIFI_CMD_LDEV:
{
int i = 0;
devid_t freedev = {0};
for (i = 0; i < ARRAY_SIZE(dev_map); i++)
{
if (dev_map[i].valid && (memcmp(&dev_map[i].devid, &freedev, sizeof(devid_t)) != 0))
{
printf("DevID: ");
dump_data((u8 *)&dev_map[i].devid, sizeof(devid_t), 0);
printf("IMSI: ");
dump_data((u8 *)&dev_map[i].imsi, sizeof(imsi_t), 0);
printf("\r\n");
}
}
}
break;
case MIFI_CMD_LUSER:
{
int i = 0;
macadr_t freeuser = {0};
if (argc == 2) {
devid_t devid;
hex2bin((u8 *)argv[1], (u8 *)&devid, sizeof(devid_t));
i = find_device_by_id(&devid);
if (i < 0) {
printf("cannot find device: %s\r\n", argv[1]);
break;
}
}
pdev = get_device(i);
for (i = 0; i < ARRAY_SIZE(pdev->users); i++)
{
if (memcmp(&pdev->users[i], &freeuser, sizeof(macadr_t)) != 0)
{
dump_data((u8 *)&pdev->users[i], sizeof(macadr_t), 0);
}
}
}
break;
default:
DBG_OUT("func isn't impletement: %d", func);
return ERROR;
}
return 0;
}
/* button handling using XFORM_DO
*/
int
handle_button( int button, WORD *msg )
{
OBJECT *tree = (OBJECT *)rs_trindex[GENERAL];
int done = FALSE;
BOOLEAN click_flag = FALSE;
MRETS mk;
int ox, oy;
if( ( button != -1 ) && ( button & 0x8000 ) )
{
click_flag = TRUE;
button &= 0x7FFF;
}
switch( button )
{
case GSAVE: if( (*xcpb->XGen_Alert)( SAVE_DEFAULTS ) )
{
(*xcpb->CPX_Save)( &cur_value, sizeof( GEN ) );
push_data();
Set_Data();
}
XDeselect( tree, GSAVE );
break;
case GOK: done = TRUE;
Set_Data();
Deselect( GOK );
break;
case GCANCEL: done = TRUE;
pop_data();
Deselect( GCANCEL );
break;
case STATUS: graf_mkstate( &mk.x, &mk.y, &mk.buttons, &mk.kstate );
if( objc_find( tree, ROOT, MAX_DEPTH, mk.x, mk.y ) == STATUS )
do_status();
break;
case BELL: keybell_handle( BELLICON, &cur_value.bell );
break;
case KEYCLICK:keybell_handle( KEYICON, &cur_value.keyclick );
break;
case M0:
case M1:
case M2:
case M3:
case M4: Dclick_Select( button );
break;
case DOUBLE: Dclick_Handle( click_flag, button );
break;
case KREPEAT: (*xcpb->MFsave )( MFSAVE, &Other );
if( AES_Version >= 0x0320 )
graf_mouse( FLAT_HAND, 0L );
if(( AES_Version >= 0x0330 )
&& ( numcol > LWHITE ) )
XSelect( tree, KREPEAT );
graf_mkstate( &ox, &mk.y, &mk.buttons, &mk.kstate );
do
{
graf_mkstate( &mk.x, &mk.y, &mk.buttons, &mk.kstate );
}while( mk.buttons && ( mk.x == ox ));
if( mk.buttons && ( mk.x != ox ))
{
(*xcpb->Sl_dragx)( tree, REPTBASE, KREPEAT, KEYMIN,
KEYMAX, &cur_value.repeat,
KRepeat );
Set_Kbrate();
}
if(( AES_Version >= 0x0330 )
&& ( numcol > LWHITE ) )
XDeselect( tree, KREPEAT );
(*xcpb->MFsave)( MFRESTORE, &Other );
break;
case KRESPONS: (*xcpb->MFsave)( MFSAVE, &Other );
if( AES_Version >= 0x0320 )
graf_mouse( FLAT_HAND, 0L );
if(( AES_Version >= 0x0330 )
&& ( numcol > LWHITE ) )
XSelect( tree, KRESPONS );
graf_mkstate( &ox, &mk.y, &mk.buttons, &mk.kstate );
do
{
graf_mkstate( &mk.x, &mk.y, &mk.buttons, &mk.kstate );
}while( mk.buttons && ( mk.x == ox ));
if( mk.buttons && ( mk.x != ox ))
{
(*xcpb->Sl_dragx)( tree, RESPBASE, KRESPONS, KEYMIN,
KEYMAX, &cur_value.response,
KResponse );
Set_Kbrate();
}
if(( AES_Version >= 0x0330 )
&& ( numcol > LWHITE ) )
XDeselect( tree, KRESPONS );
(*xcpb->MFsave)( MFRESTORE, &Other );
break;
case RRESP: (*xcpb->Sl_arrow)( tree, RESPBASE, KRESPONS, RRESP,
1, KEYMIN, KEYMAX,
&cur_value.response, HORIZONTAL,
KResponse );
Set_Kbrate();
break;
case LRESP: (*xcpb->Sl_arrow)( tree, RESPBASE, KRESPONS, LRESP,
-1, KEYMIN, KEYMAX,
&cur_value.response, HORIZONTAL,
KResponse );
Set_Kbrate();
break;
case RREPEAT: (*xcpb->Sl_arrow)( tree, REPTBASE, KREPEAT, RREPEAT,
1, KEYMIN, KEYMAX,
&cur_value.repeat, HORIZONTAL,
KRepeat );
Set_Kbrate();
break;
case LREPEAT: (*xcpb->Sl_arrow)( tree, REPTBASE, KREPEAT, LREPEAT,
-1, KEYMIN, KEYMAX,
&cur_value.repeat, HORIZONTAL,
KRepeat );
Set_Kbrate();
break;
case REPTBASE: Graf_mkstate( &mk );
objc_offset( tree, KREPEAT, &ox, &oy );
oy = (( mk.x < ox ) ? ( -3 ) : ( 3 ) );
(*xcpb->Sl_arrow)( tree, REPTBASE, KREPEAT, -1,
oy, KEYMIN, KEYMAX,
&cur_value.repeat, HORIZONTAL,
KRepeat );
Set_Kbrate();
break;
case RESPBASE: Graf_mkstate( &mk );
objc_offset( tree, KRESPONS, &ox, &oy );
oy = (( mk.x < ox ) ? ( -3 ) : ( 3 ));
(*xcpb->Sl_arrow)( tree, RESPBASE, KRESPONS, -1,
oy, KEYMIN, KEYMAX,
&cur_value.response, HORIZONTAL,
KResponse );
Set_Kbrate();
break;
case BLK1BUTT: Do_Speed();
break;
case BLK2BUTT: Do_Block2();
default: if( button == -1 )
{
switch( msg[0] )
{
case WM_REDRAW: Redraw_Objects();
break;
case AC_CLOSE: done = TRUE;
pop_data();
break;
case WM_CLOSED: done = TRUE;
Set_Data();
break;
default:
break;
}
}
break;
}
return( done );
}
*cdecl cpx_init( XCPB *Xcpb )
{
OBJECT *tree;
int i;
appl_init();
currez = Getrez(); /* find current resolution */
xcpb = Xcpb;
vhandle = xcpb->handle;
if( xcpb->booting )
{
/* Read in the default settings and set the hardware */
cur_value = save_vars;
Init_Speed();
Init_Block2();
Set_Data();
return( ( CPXINFO *)TRUE );
}
else
{
open_vwork();
close_vwork();
numcol = work_out[13]; /* size of CLUT */
AES_Version = _GemParBlk.global[0];
if( !xcpb->SkipRshFix )
(*xcpb->rsh_fix)( NUM_OBS, NUM_FRSTR, NUM_FRIMG, NUM_TREE,
rs_object, rs_tedinfo, rs_strings, rs_iconblk,
rs_bitblk, rs_frstr, rs_frimg, rs_trindex,
rs_imdope );
/*
* Get the current settings of the hardware and store them
* in the current GEN structure
*/
cur_value = save_vars;
Init_Speed();
Init_Block2();
Get_Data();
push_data();
cpxinfo.cpx_call = cpx_call;
cpxinfo.cpx_draw = NULL;
cpxinfo.cpx_wmove = NULL;
cpxinfo.cpx_timer = NULL;
cpxinfo.cpx_key = NULL;
cpxinfo.cpx_button = NULL;
cpxinfo.cpx_m1 = NULL;
cpxinfo.cpx_m2 = NULL;
cpxinfo.cpx_hook = NULL;
cpxinfo.cpx_close = NULL;
if( !xcpb->SkipRshFix )
{
if(( AES_Version >= 0x0330 )
&& ( numcol > LWHITE ) )
{
tree = ( OBJECT *)rs_trindex[ GENERAL ];
MakeActivator( GENERAL, GSAVE );
MakeActivator( GENERAL, GOK );
MakeActivator( GENERAL, GCANCEL );
ObH( GSAVE ) = 16;
ObH( GOK ) = 16;
ObH( GCANCEL ) = 16;
ObIndex( B4 ) &= 0xff00ffffL;
ObX( GSAVE ) += 1;
ObY( GSAVE ) += 2;
ObW( GSAVE ) -= 2;
ObH( GSAVE ) -= 3;
ObX( GOK ) += 1;
ObY( GOK ) += 2;
ObW( GOK ) -= 2;
ObH( GOK ) -= 3;
ObX( GCANCEL ) += 1;
ObY( GCANCEL ) += 2;
ObW( GCANCEL ) -= 2;
ObH( GCANCEL ) -= 3;
ObX( LRESP ) += 2;
ObX( RRESP ) -= 2;
for( i = RESPBASE; i <= LRESP; i++ )
{
MakeActivator( GENERAL, i );
ObH( i ) = 16;
if( ( ObType( i ) & 0x00ff ) == G_BOXTEXT )
TedColor( i ) = ( TedColor( i ) & 0xff70 ) | LWHITE | 0x70;
if( ( ObType( i ) & 0x00ff ) == G_BOXCHAR )
ObIndex( i ) = ( ObIndex( i ) & 0xffffff70 ) | LWHITE | 0x70;
if( i == RESPBASE )
{
ObX( i ) = ObX( LRESP ) + ObW( LRESP ) + 5;
ObW( i ) = ( ObX( RRESP ) - 5 ) - ObX( i );
}
}
ObIndex( B6 ) &= 0xff00ffffL;
ObIndex( B7 ) &= 0xffffff70L;
ObX( LREPEAT ) += 2;
ObX( RREPEAT ) -= 2;
for( i = LREPEAT; i <= KREPEAT; i++ )
{
MakeActivator( GENERAL, i );
ObH( i ) = 16;
if( ( ObType( i ) & 0x00ff ) == G_BOXTEXT )
TedColor( i ) = ( TedColor( i ) & 0xff70 ) | LWHITE | 0x70;
if( ( ObType( i ) & 0x00ff ) == G_BOXCHAR )
ObIndex( i ) = ( ObIndex( i ) & 0xffffff70 ) | LWHITE | 0x70;
if( i == REPTBASE )
{
ObX( i ) = ObX( LREPEAT ) + ObW( LREPEAT ) + 5;
ObW( i ) = ( ObX( RREPEAT ) - 5 ) - ObX( i );
}
}
ObIndex( B8 ) &= 0xff00ffffL;
ObIndex( B9 ) &= 0xffffff70L;
ObIndex( B0 ) &= 0xffffff70L;
MakeActivator( GENERAL, STATUS );
ObX( STATUS ) += 1;
ObY( STATUS ) += 2;
ObW( STATUS ) -= 2;
ObH( STATUS ) -= 4;
MakeActivator( GENERAL, DOUBLE );
ObX( DOUBLE ) += 1;
ObY( DOUBLE ) += 1;
ObW( DOUBLE ) -= 2;
ObH( DOUBLE ) -= 2;
TedColor( DOUBLE ) = ( TedColor( DOUBLE ) & 0xFF70 ) | LWHITE | 0x70;
ObIndex( B5 ) &= 0xff00ff70L;
ObIndex( B1 ) = ( ObIndex( B1 ) & 0xffffff70L ) | LWHITE | 0x70;
ObIndex( B2 ) = ( ObIndex( B1 ) & 0xffffff70L ) | LWHITE | 0x70;
ObIndex( B3 ) &= 0xffffff70L;
TedColor( BLK1TXT ) = ( TedColor( BLK1TXT ) & 0xFF70 );
ObIndex( BLK1 ) &= 0xffffff70L;
tree[(BLK1TXT)].ob_spec.tedinfo->te_thickness = 0;
TedColor( BLK2TXT ) = ( TedColor( BLK2TXT ) & 0xFF70 );
ObIndex( BLK2 ) &= 0xffffff70L;
tree[(BLK2TXT)].ob_spec.tedinfo->te_thickness = 0;
tree = ( OBJECT *)rs_trindex[ PARTZ ];
MakeActivator( PARTZ, STATOK );
}
} /* if( !xcpb->SkipRshFix )*/
return( &cpxinfo );
}
}
/*---------------------------------------------------------------------------*
* NAME: fuzz_core
* DESC: the heart of the fuzzing function.
*
* id is the id of the fuzz
* filename is the file used to fuzz
* weight is the default weight of the fuzz
*
* RETURN 0 -> don't fuzz.
* RETURN struct struct_fuzz *fuzz of the fuzzed value
*---------------------------------------------------------------------------*/
struct struct_fuzz *fuzz_core(config *conf, unsigned int id, unsigned char *filename, unsigned int weight) {
struct struct_fuzz *fuzz;
struct struct_fuzz *prev = NULL;
struct struct_source *source;
FILE *file;
unsigned char *file_content;
/* debug */
debug(1, "<-----------------------[enter]\n");
/* check if there is a fuzz structure for this id */
debug(3, "value of conf->fuzz: %p\n", conf->fuzz);
for (fuzz=conf->fuzz;fuzz;fuzz=fuzz->next) {
prev = fuzz;
if (fuzz->id == id) break;
}
/* there is no entry, we add a new one */
if (!fuzz) {
debug(2, "Adding a new fuzz entry\n");
/* malloc_ */
fuzz = malloc_(sizeof(struct struct_fuzz));
/* init the structure */
fuzz->next = NULL;
fuzz->source = NULL;
fuzz->id = id;
fuzz->current = 0;
fuzz->weight = weight;
/* update previous entry */
if (prev) prev->next = fuzz;
/* update if it's the first entry */
if (!conf->fuzz) conf->fuzz = fuzz;
/* init total + parse the source files */
fuzz->total = fill_source(fuzz, filename);
/* bad (memory leak, but who cares?) */
if (fuzz->total == -1) {
exit(-31339);
}
}
/* there is an entry */
else {
debug(2, "fuzz[%d] found\n", fuzz->id);
/* we are currently fuzzing this structure */
if (conf->fuzz_current == fuzz->id) {
/* check if we use the debugger mode, if yes, check if we
don't use the bruteforce mode, if yes, check if the
weight is > 0, if not fuzz->current = fuzz->total i.e.
we don't fuzz this entry */
if ((conf->dbg_mode) && (!conf->bruteforce) && (fuzz->weight <= 0)) {
fuzz->current = fuzz->total;
verbose_("[!] this fuzz is *NOT* relevant, we don't fuzz id: %d\n", fuzz->id);
}
/* there is something to fuzz */
if (fuzz->current < fuzz->total) {
debug(1, "fuzzing: %d/%d\n", fuzz->current+1, fuzz->total);
/* open the file pointed by fuzz->source->filename where id == fuzz->current */
for (source = fuzz->source; source; source = source->next) {
debug(3, "source: %p source->id: %d fuzz->current: %d\n", source, source->id, fuzz->current);
if (source->id == fuzz->current) {
break;
}
}
debug(3,"we catch the source->id: %d and fuzz->current: %d\n", source->id, fuzz->current);
debug(3,"try to open the file: \"%s\"\n", source->filename);
/* open the file */
file = fopen(source->filename, "r");
/* should never happen */
if (!file) {
error_(" ??? cannot read another time this file: \"%s\" ???: ", source->filename);
perror("");
error_("QUITTING!\n");
exit(-31337);
}
/* allocate buffer */
file_content = malloc_(source->size);
/* copy the content of the file in the buffer */
if (fread(file_content, source->size, 1, file) <= 0) {
error_(" ??? cannot read the content of this file: \"%s\" ???: ", source->filename);
perror("");
error_("QUITTING!\n");
exit(-31338);
}
/* close the file */
fclose(file);
/* copy the content in the fuzz buffer */
push_data(conf, file_content, source->size);
/* free file_content */
free(file_content);
/* update the fuzzer counter */
fuzz->current++;
/* debug */
debug(1, "<-----------------------[quit]\n");
return fuzz;
}
/* we can fuzz the next structure */
conf->fuzz_current++;
debug(2, "we have fuzzed all the data for this function...\n");
return 0;
}
}
/* debug */
debug(1, "<-----------------------[quit]\n");
/* we are not currently fuzzing this structure */
return 0;
}
/* cpx_call()
*==========================================================================
* Execute the cpx using Xform_do
*/
BOOLEAN
cdecl cpx_call( GRECT *rect )
{
int button;
int quit = 0;
OBJECT *tree = (OBJECT *)rs_trindex[PRINTER];
WORD msg[8];
GRECT xrect;
ObX( ROOT ) = rect->g_x;
ObY( ROOT ) = rect->g_y;
set_objects();
Objc_draw( tree, ROOT, MAX_DEPTH, NULL );
do
{
button = (*xcpb->Xform_do)( tree, 0, msg );
switch( button )
{
case PSAVE: if( (*xcpb->XGen_Alert)( SAVE_DEFAULTS ) )
{
(*xcpb->CPX_Save)( &cur_value, sizeof( PRT ) );
push_data();
Set_Printer( &cur_value );
}
Deselect( PSAVE );
xrect = ObRect( PSAVE );
objc_offset( tree, PSAVE, &xrect.g_x, &xrect.g_y );
xrect.g_x -= 2;
xrect.g_y -= 2;
xrect.g_w += 4;
xrect.g_h += 4;
Objc_draw( tree, PSAVE, MAX_DEPTH, &xrect );
#if 0
deselect( tree, PSAVE );
#endif
break;
case POK: quit = POK;
Set_Printer( &cur_value );
Deselect( POK );
break;
case PCANCEL: quit = PCANCEL;
pop_data();
Deselect( PCANCEL );
break;
#if GERMAN
case PTYPE: Pop_Handle( PTYPE, type_array, 2, &cur_value.cur_type, IBM, 11 );
break;
#endif
#if FRENCH | SPAIN | SWEDEN | ITALY
case PTYPE: Pop_Handle( PTYPE, type_array, 2, &cur_value.cur_type, IBM, 10 );
break;
#endif
#if USA | UK
case PTYPE: Pop_Handle( PTYPE, type_array, 2, &cur_value.cur_type, IBM, 8 );
break;
#endif
#if FRENCH
case COLOR: Pop_Handle( COLOR, color_array, 2, &cur_value.cur_color, IBM, 10 );
break;
#endif
#if ITALY
case COLOR: Pop_Handle( COLOR, color_array, 2, &cur_value.cur_color, IBM, 9 );
break;
#endif
#if USA | UK | SPAIN | GERMAN | SWEDEN
case COLOR: Pop_Handle( COLOR, color_array, 2, &cur_value.cur_color, IBM, 8 );
break;
#endif
case PIXLINE: Pop_Handle( PIXLINE, pix_array, 2, &cur_value.cur_pixel, IBM, 8 );
break;
#if GERMAN | FRENCH | ITALY
case QUALITY: Pop_Handle( QUALITY, quality_array, 2, &cur_value.cur_quality, IBM, 10 );
break;
#endif
#if SWEDEN
case QUALITY: Pop_Handle( QUALITY, quality_array, 2, &cur_value.cur_quality, IBM, 9 );
break;
#endif
#if USA | UK | SPAIN
case QUALITY: Pop_Handle( QUALITY, quality_array, 2, &cur_value.cur_quality, IBM, 8 );
break;
#endif
#if ITALY
case PAPER: Pop_Handle( PAPER, paper_array, 2, &cur_value.cur_paper, IBM, 10 );
break;
#endif
#if USA | UK | SPAIN | GERMAN | FRENCH
case PAPER: Pop_Handle( PAPER, paper_array, 2, &cur_value.cur_paper, IBM, 9 );
break;
#endif
#if SWEDEN
case PAPER: Pop_Handle( PAPER, paper_array, 2, &cur_value.cur_paper, IBM, 8 );
break;
#endif
#if ITALY
case PORT: Pop_Handle( PORT, port_array, 2, &cur_value.cur_port, IBM, 9 );
break;
#else
case PORT: Pop_Handle( PORT, port_array, 2, &cur_value.cur_port, IBM, 10 );
break;
#endif
default: if( button == -1 )
{
switch( msg[0] )
{
case WM_REDRAW: Redraw_Objects();
break;
case AC_CLOSE: quit = PCANCEL;
pop_data();
break;
case WM_CLOSED: quit = POK;
Set_Printer( &cur_value );
break;
default:
break;
}
}
break;
}
}while( !quit);
return( FALSE );
}
/*---------------------------------------------------------------------------*
* NAME: push_length
* DESC: add a length in the linked list length structure
*---------------------------------------------------------------------------*/
void push_length(config *conf, unsigned int id, unsigned int type) {
struct struct_length *length;
struct struct_length *prev = NULL;
/* dummy length values */
unsigned int dummy_int = 0xffffffff;
unsigned short dummy_short = 0xffff;
unsigned char dummy_char = 0xff;
/* debug */
debug(1, "<-----------------------[enter]\n");
debug(3," entering push_length\n");
/* go to the last length */
for(length = conf->length;length;length = length->next) {
debug(3, "current struct length of block[%d]\n", length->id);
prev = length; /* save the preview */
}
debug(3," add length structure\n");
/* create a length entry in the linked list */
length = malloc_(sizeof(struct struct_length));
/* update the previous block's field next */
if (prev)
prev->next = length;
/* fill the other fields */
length->id = id; /* used next to find the struct_block */
length->block = NULL; /* init the struct_block pointer */
length->offset = conf->buf_fuzz_size; /* current size of the buffer */
length->next = NULL; /* init the next pointer */
length->type = type; /* put the correct length type */
/* update if it's the first length */
if (!conf->length)
conf->length = length;
/* add the correct number of byte in the fuzz buffer */
switch(type) {
case HF_BLOCK_SIZE_B_32 : /* big-endian 32bits */
push_data(conf, (unsigned char *) &dummy_int, sizeof(int));
break;
case HF_BLOCK_SIZE_L_32 : /* little-endian 32bits */
push_data(conf, (unsigned char *) &dummy_int, sizeof(int));
break;
case HF_BLOCK_SIZE_B_16 : /* big-endian 16bits */
push_data(conf, (unsigned char *) &dummy_short, sizeof(short));
break;
case HF_BLOCK_SIZE_L_16 : /* little-endian 16bits */
push_data(conf, (unsigned char *) &dummy_short, sizeof(short));
break;
case HF_BLOCK_SIZE_8 : /* 8bits */
push_data(conf, &dummy_char, sizeof(char));
break;
case HF_BLOCK_SIZE_S_16 : /* string in hexadecimal */
/* don't add bytes in buffer now. See after */
break;
case HF_BLOCK_SIZE_S_10 : /* string in decimal */
/* don't add bytes in buffer now. See after */
break;
default: /* should never happen */
break;
}
/* debug */
debug(1, "<-----------------------[quit]\n");
}
// ptp movement (sinus wave acceleration)
void writePositionDataPTPSinus(DynamixelComm *dc, unsigned long readPause, byte* readBuffer,byte* writeBuffer, float pos, float* tb, float* te, int* sgn, float* vmax, float* bmax)
{
int i;
long int current;
float se, t, tv;
// sync write AX12_DATA_WRITE bytes starting at address P_GOAL_POSITION_L
byte op = INST_SYNC_WRITE;
gbpParameter[0] = P_GOAL_POSITION_L; //Address
gbpParameter[1] = AX12_DATA_WRITE;
printf("%s joints %f: ", __func__, pos);
// prepare write buffer -> unpack data from serial and store bytewise in buffer
for (i=0;i<AX12_COUNT;i++)
{
gbpParameter[2 + i * (AX12_DATA_WRITE+1)] = i + 1;
current = readBuffer[i * AX12_DATA_WRITE + 0] + 256 * readBuffer[i * AX12_DATA_WRITE + 1];
t = pos * te[i];
tv = te[i] - tb[i];
printf("teb %f %f %f ", pos, bmax[i], vmax[i]);
if (t <= tb[i]) {
//printf("A");
se = bmax[i] * (0.25 * t * t + (tb[i] * tb[i] / (8.0 * M_PI * M_PI)) * (cos(2.0*M_PI*t/tb[i])-1.0));
} else if (t <= te[i] - tb[i]) {
//printf("B");
se = vmax[i] * (t - 0.5 * tb[i]);
} else {
//printf("C");
se = 0.5 * bmax[i] * (te[i] * (t + tb[i]) - 0.5*(t*t + te[i]*te[i] + 2.0 * tb[i]*tb[i]) + (tb[i]*tb[i]/(4.0 * M_PI * M_PI))*(1.0 - cos((2.0*M_PI/tb[i])*(t - tv))));
}
current += sgn[i] * (long int) (se);
if (current > 1023)
current = 1023;
else if (current < 0)
current = 0;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 0] = current & 0xFF;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 1] = (current & 0xFF00) >> 8;
//printf("%04x ", current);
if(sgn[i]*(long int)(se) != 0) {
//printf("%d/%f:%04lx:%04lx ", i, se, current, sgn[i] * (long int) (se));
printf("!!");
}
push_data(i, current);
/*
if (i == 13)
{
uart1_putc(current & 0xFF);
uart1_putc((current & 0xFF00) >> 8);
}
*/
// store target speed and torque in write buffer
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 2] = writeBuffer[i * AX12_DATA_WRITE + 2];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 3] = writeBuffer[i * AX12_DATA_WRITE + 3];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 4] = writeBuffer[i * AX12_DATA_WRITE + 4];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 5] = writeBuffer[i * AX12_DATA_WRITE + 5];
if(!((writeBuffer[i * AX12_DATA_WRITE + 4] == 255) && (writeBuffer[i * AX12_DATA_WRITE + 5] == 3))) {
printf("%d:TORQUE FAIL!!",__LINE__);
}
}
printf("\n");
// send all data to ax12s AT ONCE
TxPacket(dc, BROADCASTING_ID,op,2+AX12_COUNT*(AX12_DATA_WRITE + 1));
}
/// 从 buffer 读取数据, 返回 lua 函数返回值的个数, 函数的第一个返回值是 queue.
static int
filter_data_(lua_State *L, int fd, uint8_t * buffer, int size) {
struct queue *q = lua_touserdata(L, 1);
struct uncomplete * uc = find_uncomplete(q, fd); // 注意, 这里将 uncomplete 从 queue.hash 中移除了
if (uc) {
// fill uncomplete
// 填充 uncomplete
if (uc->read < 0) { // 当包头还未完整读取的情况
// read size
assert(uc->read == -1);
// 获得数据内容大小
int pack_size = *buffer;
pack_size |= uc->header << 8 ;
// 偏移到实际数据内容指针开始位置
++buffer;
// 实际的数据内容大小
--size;
uc->pack.size = pack_size; // 记录实际需要读取的内容大小
uc->pack.buffer = skynet_malloc(pack_size); // 分配内存空间
uc->read = 0; // 标记还未开始读取数据内容
}
// 计算需要读取的数据
int need = uc->pack.size - uc->read;
if (size < need) { // 如果 buffer 待读取的数据还不足, 尽可能读取能够读取的数据
// 读取可读的数据
memcpy(uc->pack.buffer + uc->read, buffer, size);
uc->read += size;
// 再次压入到 queue.hash 中
int h = hash_fd(fd);
uc->next = q->hash[h];
q->hash[h] = uc;
return 1;
}
// 读取完整的数据内容
memcpy(uc->pack.buffer + uc->read, buffer, need);
// 跳过已经读取的内容
buffer += need;
// 计算剩余的可读取数据大小
size -= need;
// buffer 中的数据恰好足够读取
if (size == 0) {
lua_pushvalue(L, lua_upvalueindex(TYPE_DATA)); // macro TYPE_DATA
lua_pushinteger(L, fd); // socket id
lua_pushlightuserdata(L, uc->pack.buffer); // buffer
lua_pushinteger(L, uc->pack.size); // buffer size
skynet_free(uc);
return 5;
}
// more data
// buffer 有更多的数据可读, 将数据压入 queue.queue 中
push_data(L, fd, uc->pack.buffer, uc->pack.size, 0);
skynet_free(uc);
push_more(L, fd, buffer, size); // 继续读取剩下的数据
lua_pushvalue(L, lua_upvalueindex(TYPE_MORE)); // macro TYPE_MORE
return 2;
} else {
if (size == 1) { // 仅读取包头的 1 个数据
struct uncomplete * uc = save_uncomplete(L, fd);
uc->read = -1;
uc->header = *buffer;
return 1;
}
// 读取包头的数据
int pack_size = read_size(buffer);
buffer+=2;
size-=2;
// 如果 buffer 的数据不够读, 将 buffer 数据全部读取
if (size < pack_size) {
struct uncomplete * uc = save_uncomplete(L, fd);
uc->read = size;
uc->pack.size = pack_size;
uc->pack.buffer = skynet_malloc(pack_size);
memcpy(uc->pack.buffer, buffer, size);
return 1;
}
// 如果 buffer 的数据恰好是 1 个包的数据大小, 将 buffer 数据全部读取
if (size == pack_size) {
// just one package
lua_pushvalue(L, lua_upvalueindex(TYPE_DATA)); // macro TYPE_DATA
lua_pushinteger(L, fd); // socket id
void * result = skynet_malloc(pack_size);
memcpy(result, buffer, size);
lua_pushlightuserdata(L, result); // buffer
lua_pushinteger(L, size); // buffer size
return 5;
}
// more data
// 如果 buffer 的数据大于 1 个包的数据大小, 那么继续读取 buffer 里面的数据
push_data(L, fd, buffer, pack_size, 1);
buffer += pack_size;
size -= pack_size;
push_more(L, fd, buffer, size);
lua_pushvalue(L, lua_upvalueindex(TYPE_MORE)); // macro TYPE_MORE
return 2;
}
}
*cdecl cpx_init( XCPB *Xcpb )
{
OBJECT *tree;
int port;
int temp_port;
xcpb = Xcpb;
if( xcpb->booting )
{
/* Read in defaults and set the options */
store = ( BUFFER *)(*xcpb->Get_Buffer)();
Modem = save_vars;
Set_Machine_Code();
Set_Number_Of_Ports();
for( port = 0; port < Num_Ports; port++ )
{
Write_Port( port, store );
store++;
}
Modem.cur_port = Set_Active_Port( Modem.cur_port );
return( ( CPXINFO *)TRUE );
}
else
{
appl_init();
AES_Version = _GemParBlk.global[0];
if( !xcpb->SkipRshFix )
(*xcpb->rsh_fix)( NUM_OBS, NUM_FRSTR, NUM_FRIMG, NUM_TREE,
rs_object, rs_tedinfo, rs_strings, rs_iconblk,
rs_bitblk, rs_frstr, rs_frimg, rs_trindex,
rs_imdope );
Supexec( Get_OS );
tree = ( OBJECT *)rs_trindex[ RS232 ];
Disable( PORT );
Set_Machine_Code();
(( IsTT() || IsSTE() ) ? ( Enable( PORT ) ) : ( Disable( PORT ) ) );
Set_Number_Of_Ports();
store = ( BUFFER *)(*xcpb->Get_Buffer)();
Modem = save_vars; /* Get the Defaults and fill up all 4 ports with info */
/* Get the ACTUAL system values for as many ports as possible.*/
temp_port = Get_Active_Port();
for( port = 0; port < Num_Ports; port++ )
{
Read_Port( port, store );
store++;
}
Modem.cur_port = Set_Active_Port( temp_port );
push_data();
open_vwork();
close_vwork();
gl_ncolors = work_out[13];
if( !xcpb->SkipRshFix )
{
if(( AES_Version >= 0x0330 )
&& ( gl_ncolors > LWHITE ))
{
tree = (OBJECT *)rs_trindex[RS232];
MakeActivator( RS232, SAVE );
MakeActivator( RS232, RS232OK );
MakeActivator( RS232, RS232CAN );
}
}
cpxinfo.cpx_call = cpx_call;
cpxinfo.cpx_draw = NULL;
cpxinfo.cpx_wmove = NULL;
cpxinfo.cpx_timer = NULL;
cpxinfo.cpx_key = NULL;
cpxinfo.cpx_button = NULL;
cpxinfo.cpx_m1 = NULL;
cpxinfo.cpx_m2 = NULL;
cpxinfo.cpx_hook = NULL;
cpxinfo.cpx_close = NULL;
return( &cpxinfo );
}
}
