static void rtcfg_client_detach(int ifindex, struct rt_proc_call *call)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
struct rtcfg_cmd *cmd_event;
cmd_event = rtpc_get_priv(call, struct rtcfg_cmd);
cmd_event->args.detach.station_addr_list =
rtcfg_dev->spec.clt.station_addr_list;
cmd_event->args.detach.stage2_chain = rtcfg_dev->spec.clt.stage2_chain;
while (1) {
call = rtcfg_dequeue_blocking_call(ifindex);
if (call == NULL)
break;
rtpc_complete_call(call, -ENODEV);
}
if (rtcfg_dev->flags & FLAG_TIMER_STARTED) {
rtcfg_dev->flags |= FLAG_TIMER_SHUTDOWN;
rtos_task_delete(&rtcfg_dev->timer_task);
}
rtcfg_reset_device(ifindex);
rtcfg_next_main_state(cmd_event->ifindex, RTCFG_MAIN_OFF);
rtos_res_unlock(&rtcfg_dev->dev_lock);
}
static int rtcfg_client_get_frag(int ifindex, struct rt_proc_call *call)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
if (test_bit(RTCFG_FLAG_STAGE_2_DATA, &rtcfg_dev->flags) == 0) {
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
return -EINVAL;
}
rtcfg_send_ack(ifindex);
if (rtcfg_dev->spec.clt.cfg_offs >= rtcfg_dev->spec.clt.cfg_len) {
if (rtcfg_dev->stations_found == rtcfg_dev->other_stations) {
rtpc_complete_call(call, 0);
rtcfg_next_main_state(ifindex,
test_bit(RTCFG_FLAG_READY,
&rtcfg_dev->flags) ?
RTCFG_MAIN_CLIENT_READY : RTCFG_MAIN_CLIENT_2);
} else {
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_ALL_FRAMES);
rtcfg_queue_blocking_call(ifindex, call);
}
} else
rtcfg_queue_blocking_call(ifindex, call);
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
return -CALL_PENDING;
}
static int rtcfg_client_get_frag(int ifindex, struct rt_proc_call *call)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
struct rtcfg_cmd *cmd_event;
cmd_event = rtpc_get_priv(call, struct rtcfg_cmd);
if ((rtcfg_dev->flags & RTCFG_FLAG_STAGE_2_DATA) == 0) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
return -EINVAL;
}
rtcfg_send_ack(ifindex);
if (rtcfg_dev->spec.clt.cfg_offs >= rtcfg_dev->spec.clt.cfg_len) {
if (rtcfg_dev->stations_found == rtcfg_dev->other_stations) {
rtpc_complete_call(call, 0);
rtcfg_next_main_state(ifindex,
((rtcfg_dev->flags & RTCFG_FLAG_READY) != 0) ?
RTCFG_MAIN_CLIENT_READY : RTCFG_MAIN_CLIENT_2);
} else {
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_ALL_FRAMES);
rtcfg_queue_blocking_call(ifindex, call);
}
} else
rtcfg_queue_blocking_call(ifindex, call);
rtos_res_unlock(&rtcfg_dev->dev_lock);
return -CALL_PENDING;
}
/* releases rtcfg_dev->dev_mutex on return */
static void rtcfg_client_detach(int ifindex, struct rt_proc_call *call)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
struct rtcfg_cmd *cmd_event;
cmd_event = rtpc_get_priv(call, struct rtcfg_cmd);
cmd_event->args.detach.station_addr_list =
rtcfg_dev->spec.clt.station_addr_list;
cmd_event->args.detach.stage2_chain = rtcfg_dev->spec.clt.stage2_chain;
while (1) {
call = rtcfg_dequeue_blocking_call(ifindex);
if (call == NULL)
break;
rtpc_complete_call(call, -ENODEV);
}
if (test_and_clear_bit(FLAG_TIMER_STARTED, &rtcfg_dev->flags))
rtdm_timer_destroy(&rtcfg_dev->timer);
rtcfg_reset_device(ifindex);
rtcfg_next_main_state(cmd_event->internal.data.ifindex, RTCFG_MAIN_OFF);
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
}
/* releases rtcfg_dev->dev_mutex on return */
static void rtcfg_client_detach(int ifindex, struct rt_proc_call *call)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
struct rtcfg_cmd *cmd_event;
cmd_event = rtpc_get_priv(call, struct rtcfg_cmd);
cmd_event->args.detach.station_addr_list =
rtcfg_dev->spec.clt.station_addr_list;
cmd_event->args.detach.stage2_chain = rtcfg_dev->spec.clt.stage2_chain;
while (1) {
call = rtcfg_dequeue_blocking_call(ifindex);
if (call == NULL)
break;
rtpc_complete_call(call, -ENODEV);
}
if (rtcfg_dev->flags & FLAG_TIMER_STARTED) {
/* It's safe to kill the task, it either waits for dev_mutex or the
next period. */
rtdm_task_destroy(&rtcfg_dev->timer_task);
}
rtcfg_reset_device(ifindex);
rtcfg_next_main_state(cmd_event->internal.data.ifindex, RTCFG_MAIN_OFF);
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
}
static void rtpc_dispatch_handler(void *arg)
{
struct rt_proc_call *call;
int ret;
while (rtdm_event_wait(&dispatch_event) == 0)
while ((call = rtpc_dequeue_pending_call())) {
ret = call->proc(call);
if (ret != -CALL_PENDING)
rtpc_complete_call(call, ret);
}
}
static void rtpc_dispatch_handler(unsigned long arg)
{
struct rt_proc_call *call;
int ret;
if((call = rtpc_dequeue_pending_call())!=NULL)
ret = call->proc(call);
else
return;
if (ret != -CALL_PENDING)
rtpc_complete_call(call, ret);
}
static void rtcfg_client_queue_frag(int ifindex, struct rtskb *rtskb,
size_t data_len)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
struct rt_proc_call *call;
struct rtcfg_cmd *cmd_event;
int result;
rtskb_trim(rtskb, data_len);
if (rtcfg_dev->spec.clt.stage2_chain == NULL)
rtcfg_dev->spec.clt.stage2_chain = rtskb;
else {
rtcfg_dev->spec.clt.stage2_chain->chain_end->next = rtskb;
rtcfg_dev->spec.clt.stage2_chain->chain_end = rtskb;
}
rtcfg_dev->spec.clt.cfg_offs += data_len;
rtcfg_dev->spec.clt.chain_len += data_len;
if ((rtcfg_dev->spec.clt.cfg_offs >= rtcfg_dev->spec.clt.cfg_len) ||
(++rtcfg_dev->spec.clt.packet_counter == rtcfg_dev->burstrate)) {
while (1) {
call = rtcfg_dequeue_blocking_call(ifindex);
if (call == NULL)
break;
cmd_event = rtpc_get_priv(call, struct rtcfg_cmd);
result = 0;
/* note: only the first pending call gets data */
if (rtcfg_dev->spec.clt.stage2_chain != NULL) {
result = rtcfg_dev->spec.clt.chain_len;
cmd_event->args.announce.rtskb =
rtcfg_dev->spec.clt.stage2_chain;
rtcfg_dev->spec.clt.stage2_chain = NULL;
}
rtpc_complete_call(call,
(cmd_event->event_id == RTCFG_CMD_ANNOUNCE) ?
result : -EINVAL);
}
rtcfg_dev->spec.clt.packet_counter = 0;
rtcfg_dev->spec.clt.chain_len = 0;
}
}
static void rtpc_dispatch_handler(int arg)
{
struct rt_proc_call *call;
int ret;
while (1) {
rtos_event_sem_wait(&dispatch_event);
call = rtpc_dequeue_pending_call();
ret = call->proc(call);
if (ret != -CALL_PENDING)
rtpc_complete_call(call, ret);
}
}
static struct tdma_job *do_request_cal_job(struct tdma_priv *tdma,
struct tdma_request_cal *job,
rtdm_lockctx_t lockctx)
{
struct rt_proc_call *call;
struct tdma_job *prev_job;
int err;
if ((job->period != 1) &&
(tdma->current_cycle % job->period != job->phasing))
return &job->head;
/* remove job until we get a reply */
__list_del(job->head.entry.prev, job->head.entry.next);
job->head.ref_count--;
prev_job = tdma->current_job =
list_entry(job->head.entry.prev, struct tdma_job, entry);
prev_job->ref_count++;
tdma->job_list_revision++;
rtdm_lock_put_irqrestore(&tdma->lock, lockctx);
rtdm_task_sleep_abs(tdma->current_cycle_start + job->offset,
RTDM_TIMERMODE_REALTIME);
err = tdma_xmit_request_cal_frame(tdma,
tdma->current_cycle + job->period, job->offset);
rtdm_lock_get_irqsave(&tdma->lock, lockctx);
/* terminate call on error */
if (err < 0) {
call = tdma->calibration_call;
tdma->calibration_call = NULL;
if (call) {
rtdm_lock_put_irqrestore(&tdma->lock, lockctx);
rtpc_complete_call(call, err);
rtdm_lock_get_irqsave(&tdma->lock, lockctx);
}
}
return prev_job;
}
static void rtcfg_client_recv_stage_1(int ifindex, struct rtskb *rtskb)
{
struct rtcfg_frm_stage_1_cfg *stage_1_cfg;
struct rt_proc_call *call;
struct rtcfg_cmd *cmd_event;
struct rtcfg_device *rtcfg_dev = &device[ifindex];
u8 addr_type;
int ret;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_1_cfg)) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_1_cfg frame\n");
kfree_rtskb(rtskb);
return;
}
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)rtskb->data;
__rtskb_pull(rtskb, sizeof(struct rtcfg_frm_stage_1_cfg));
addr_type = stage_1_cfg->addr_type;
switch (stage_1_cfg->addr_type) {
#ifdef CONFIG_RTNET_RTIPV4
case RTCFG_ADDR_IP: {
struct rtnet_device *rtdev, *tmp;
u32 daddr, saddr, mask, bcast;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_1_cfg) +
2*RTCFG_ADDRSIZE_IP) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_1_cfg "
"frame\n");
kfree_rtskb(rtskb);
break;
}
rtdev = rtskb->rtdev;
daddr = *(u32*)stage_1_cfg->client_addr;
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)
(((u8 *)stage_1_cfg) + RTCFG_ADDRSIZE_IP);
saddr = *(u32*)stage_1_cfg->server_addr;
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)
(((u8 *)stage_1_cfg) + RTCFG_ADDRSIZE_IP);
__rtskb_pull(rtskb, 2*RTCFG_ADDRSIZE_IP);
/* Broadcast: IP is used to address client */
if (rtskb->pkt_type == PACKET_BROADCAST) {
/* directed to us? */
if (daddr != rtdev->local_ip) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
kfree_rtskb(rtskb);
return;
}
/* Unicast: IP address is assigned by the server */
} else {
/* default netmask */
if (ntohl(daddr) <= 0x7FFFFFFF) /* 127.255.255.255 */
mask = 0x000000FF; /* 255.0.0.0 */
else if (ntohl(daddr) <= 0xBFFFFFFF) /* 191.255.255.255 */
mask = 0x0000FFFF; /* 255.255.0.0 */
else
mask = 0x00FFFFFF; /* 255.255.255.0 */
bcast = daddr | (~mask);
rt_ip_route_del_all(rtdev); /* cleanup routing table */
rtdev->local_ip = daddr;
rtdev->broadcast_ip = bcast;
if ((tmp = rtdev_get_loopback()) != NULL) {
rt_ip_route_add_host(daddr, tmp->dev_addr, tmp);
rtdev_dereference(tmp);
}
if (rtdev->flags & IFF_BROADCAST)
rt_ip_route_add_host(bcast, rtdev->broadcast, rtdev);
}
/* update routing table */
rt_ip_route_add_host(saddr, rtskb->mac.ethernet->h_source, rtdev);
rtcfg_dev->spec.clt.srv_addr.ip_addr = saddr;
break;
}
#endif /* CONFIG_RTNET_RTIPV4 */
case RTCFG_ADDR_MAC:
/* nothing to do */
break;
default:
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: unknown addr_type %d in %s()\n",
stage_1_cfg->addr_type, __FUNCTION__);
kfree_rtskb(rtskb);
return;
}
rtcfg_dev->spec.clt.addr_type = addr_type;
/* Ethernet-specific */
memcpy(rtcfg_dev->spec.clt.srv_mac_addr,
rtskb->mac.ethernet->h_source, ETH_ALEN);
rtcfg_dev->burstrate = stage_1_cfg->burstrate;
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_1);
rtos_res_unlock(&rtcfg_dev->dev_lock);
while (1) {
call = rtcfg_dequeue_blocking_call(ifindex);
if (call == NULL)
break;
cmd_event = rtpc_get_priv(call, struct rtcfg_cmd);
if (cmd_event->event_id == RTCFG_CMD_CLIENT) {
ret = 0;
/* note: only the first pending call gets data */
if ((rtskb != NULL) &&
(cmd_event->args.client.buffer_size > 0)) {
ret = ntohs(stage_1_cfg->cfg_len);
cmd_event->args.client.rtskb = rtskb;
rtskb = NULL;
}
} else
ret = -EINVAL;
rtpc_complete_call(call, ret);
}
if (rtskb)
kfree_rtskb(rtskb);
}
int rtcfg_main_state_client_2(int ifindex, RTCFG_EVENT event_id,
void* event_data)
{
struct rtskb *rtskb = (struct rtskb *)event_data;
struct rt_proc_call *call = (struct rt_proc_call *)event_data;
struct rtcfg_device *rtcfg_dev;
switch (event_id) {
case RTCFG_CMD_READY:
rtcfg_dev = &device[ifindex];
if (rtcfg_dev->stations_ready == rtcfg_dev->other_stations)
rtpc_complete_call(call, 0);
else
rtcfg_queue_blocking_call(ifindex, call);
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_READY);
if ((rtcfg_dev->flags & RTCFG_FLAG_READY) == 0) {
rtcfg_dev->flags |= RTCFG_FLAG_READY;
rtcfg_send_ready(ifindex);
}
rtos_res_unlock(&rtcfg_dev->dev_lock);
return -CALL_PENDING;
case RTCFG_CMD_DETACH:
rtcfg_client_detach(ifindex, call);
break;
case RTCFG_FRM_READY:
if (rtcfg_client_recv_ready(ifindex, rtskb) == 0)
rtos_res_unlock(&device[ifindex].dev_lock);
break;
case RTCFG_FRM_ANNOUNCE_NEW:
if (rtcfg_client_recv_announce(ifindex, rtskb) == 0) {
rtcfg_send_announce_reply(ifindex,
rtskb->mac.ethernet->h_source);
rtos_res_unlock(&device[ifindex].dev_lock);
}
kfree_rtskb(rtskb);
break;
case RTCFG_FRM_DEAD_STATION:
rtcfg_client_recv_dead_station(ifindex, rtskb);
break;
case RTCFG_FRM_STAGE_1_CFG:
/* ignore */
rtos_res_unlock(&device[ifindex].dev_lock);
kfree_rtskb(rtskb);
break;
default:
rtos_res_unlock(&device[ifindex].dev_lock);
RTCFG_DEBUG(1, "RTcfg: unknown event %s for rtdev %d in %s()\n",
rtcfg_event[event_id], ifindex, __FUNCTION__);
return -EINVAL;
}
return 0;
}
