static void diag_mask_update_work_fn(struct work_struct *work)
{
uint8_t peripheral;
for (peripheral = 0; peripheral <= NUM_PERIPHERALS; peripheral++) {
if (!(driver->mask_update & PERIPHERAL_MASK(peripheral)))
continue;
mutex_lock(&driver->cntl_lock);
driver->mask_update ^= PERIPHERAL_MASK(peripheral);
mutex_unlock(&driver->cntl_lock);
diag_send_updates_peripheral(peripheral);
}
}
static int periph_clk_enable(struct clk *clk)
{
struct pmc_platdata *plat = dev_get_platdata(clk->dev);
struct at91_pmc *pmc = plat->reg_base;
enum periph_clk_type clk_type;
void *addr;
if (clk->id < PERIPHERAL_ID_MIN)
return -1;
clk_type = dev_get_driver_data(dev_get_parent(clk->dev));
if (clk_type == CLK_PERIPH_AT91RM9200) {
addr = &pmc->pcer;
if (clk->id > PERIPHERAL_ID_MAX)
addr = &pmc->pcer1;
setbits_le32(addr, PERIPHERAL_MASK(clk->id));
} else {
writel(clk->id & AT91_PMC_PCR_PID_MASK, &pmc->pcr);
setbits_le32(&pmc->pcr,
AT91_PMC_PCR_CMD_WRITE | AT91_PMC_PCR_EN);
}
return 0;
}
static void diag_stm_update_work_fn(struct work_struct *work)
{
uint8_t i;
uint16_t peripheral_mask = 0;
int err = 0;
mutex_lock(&driver->cntl_lock);
peripheral_mask = driver->stm_peripheral;
driver->stm_peripheral = 0;
mutex_unlock(&driver->cntl_lock);
if (peripheral_mask == 0)
return;
for (i = 0; i < NUM_PERIPHERALS; i++) {
if (!driver->feature[i].stm_support)
continue;
if (peripheral_mask & PERIPHERAL_MASK(i)) {
err = diag_send_stm_state(i,
(uint8_t)(driver->stm_state_requested[i]));
if (!err) {
driver->stm_state[i] =
driver->stm_state_requested[i];
}
}
}
}
static void diag_close_transport_work_fn(struct work_struct *work)
{
uint8_t transport;
uint8_t peripheral;
mutex_lock(&driver->cntl_lock);
for (peripheral = 0; peripheral <= NUM_PERIPHERALS; peripheral++) {
if (!(driver->close_transport & PERIPHERAL_MASK(peripheral)))
continue;
driver->close_transport ^= PERIPHERAL_MASK(peripheral);
transport = driver->feature[peripheral].sockets_enabled ?
TRANSPORT_SMD : TRANSPORT_SOCKET;
diagfwd_close_transport(transport, peripheral);
}
mutex_unlock(&driver->cntl_lock);
}
void diag_cntl_channel_open(struct diagfwd_info *p_info)
{
if (!p_info)
return;
driver->mask_update |= PERIPHERAL_MASK(p_info->peripheral);
queue_work(driver->cntl_wq, &driver->mask_update_work);
diag_notify_md_client(p_info->peripheral, DIAG_STATUS_OPEN);
}
static void process_socket_feature(uint8_t peripheral)
{
if (peripheral >= NUM_PERIPHERALS)
return;
mutex_lock(&driver->cntl_lock);
driver->close_transport |= PERIPHERAL_MASK(peripheral);
queue_work(driver->cntl_wq, &driver->close_transport_work);
mutex_unlock(&driver->cntl_lock);
}
static void enable_stm_feature(uint8_t peripheral)
{
if (peripheral >= NUM_PERIPHERALS)
return;
mutex_lock(&driver->cntl_lock);
driver->feature[peripheral].stm_support = ENABLE_STM;
driver->stm_peripheral |= PERIPHERAL_MASK(peripheral);
mutex_unlock(&driver->cntl_lock);
queue_work(driver->cntl_wq, &(driver->stm_update_work));
}
void diag_cntl_channel_close(struct diagfwd_info *p_info)
{
uint8_t peripheral;
if (!p_info)
return;
peripheral = p_info->peripheral;
if (peripheral >= NUM_PERIPHERALS)
return;
driver->feature[peripheral].sent_feature_mask = 0;
driver->feature[peripheral].rcvd_feature_mask = 0;
flush_workqueue(driver->cntl_wq);
reg_dirty |= PERIPHERAL_MASK(peripheral);
diag_cmd_remove_reg_by_proc(peripheral);
driver->feature[peripheral].stm_support = DISABLE_STM;
driver->feature[peripheral].log_on_demand = 0;
driver->stm_state[peripheral] = DISABLE_STM;
driver->stm_state_requested[peripheral] = DISABLE_STM;
reg_dirty ^= PERIPHERAL_MASK(peripheral);
diag_notify_md_client(peripheral, DIAG_STATUS_CLOSED);
}
void diag_notify_md_client(uint8_t peripheral, int data)
{
int stat = 0;
struct siginfo info;
if (driver->logging_mode != MEMORY_DEVICE_MODE)
return;
memset(&info, 0, sizeof(struct siginfo));
info.si_code = SI_QUEUE;
info.si_int = (PERIPHERAL_MASK(peripheral) | data);
info.si_signo = SIGCONT;
if (driver->md_proc[DIAG_LOCAL_PROC].mdlog_process) {
stat = send_sig_info(info.si_signo, &info,
driver->md_proc[DIAG_LOCAL_PROC].mdlog_process);
if (stat)
pr_err("diag: Err sending signal to memory device client, signal data: 0x%x, stat: %d\n",
info.si_int, stat);
}
}
void diag_cntl_process_read_data(struct diagfwd_info *p_info, void *buf,
int len)
{
int read_len = 0;
int header_len = sizeof(struct diag_ctrl_pkt_header_t);
uint8_t *ptr = buf;
struct diag_ctrl_pkt_header_t *ctrl_pkt = NULL;
if (!buf || len <= 0 || !p_info)
return;
if (reg_dirty & PERIPHERAL_MASK(p_info->peripheral)) {
pr_err_ratelimited("diag: dropping command registration from peripheral %d\n",
p_info->peripheral);
return;
}
while (read_len + header_len < len) {
ctrl_pkt = (struct diag_ctrl_pkt_header_t *)ptr;
switch (ctrl_pkt->pkt_id) {
case DIAG_CTRL_MSG_REG:
process_command_registration(ptr, ctrl_pkt->len,
p_info->peripheral);
break;
case DIAG_CTRL_MSG_DEREG:
process_command_deregistration(ptr, ctrl_pkt->len,
p_info->peripheral);
break;
case DIAG_CTRL_MSG_FEATURE:
process_incoming_feature_mask(ptr, ctrl_pkt->len,
p_info->peripheral);
break;
case DIAG_CTRL_MSG_LAST_EVENT_REPORT:
process_last_event_report(ptr, ctrl_pkt->len,
p_info->peripheral);
break;
case DIAG_CTRL_MSG_LOG_RANGE_REPORT:
process_log_range_report(ptr, ctrl_pkt->len,
p_info->peripheral);
break;
case DIAG_CTRL_MSG_SSID_RANGE_REPORT:
process_ssid_range_report(ptr, ctrl_pkt->len,
p_info->peripheral);
break;
case DIAG_CTRL_MSG_BUILD_MASK_REPORT:
process_build_mask_report(ptr, ctrl_pkt->len,
p_info->peripheral);
break;
case DIAG_CTRL_MSG_PD_STATUS:
process_pd_status(ptr, ctrl_pkt->len,
p_info->peripheral);
break;
default:
DIAGFWD_DBUG("diag: Control packet %d not supported\n",
ctrl_pkt->pkt_id);
}
ptr += header_len + ctrl_pkt->len;
read_len += header_len + ctrl_pkt->len;
}
return;
}
