void read_rc_channels(void)
{
if (global_flags.systick) {
if (servo_reader_timer) {
--servo_reader_timer;
}
}
global_flags.rc_update_event = false;
if (!report_rc_actions) {
return;
}
//uart_sendshort(rc_channel[2].normalized);
report_rc_actions = false;
switch (servo_reader_state) {
case WAIT_FOR_FIRST_PULSE:
servo_reader_timer = dev_config.startup_time;
servo_reader_state = WAIT_FOR_TIMEOUT;
break;
case WAIT_FOR_TIMEOUT:
if (servo_reader_timer == 0) {
initialize_channel(&rc_channel[ST]);
initialize_channel(&rc_channel[TH]);
normalize_channel(&rc_channel[CH3]);
servo_reader_state = NORMAL_OPERATION;
global_flags.rc_is_initializing = 0;
}
global_flags.rc_update_event = true;
break;
case NORMAL_OPERATION:
normalize_channel(&rc_channel[ST]);
normalize_channel(&rc_channel[TH]);
if (!dev_config.flags.ch3_is_local_switch) {
normalize_channel(&rc_channel[CH3]);
}
global_flags.rc_update_event = true;
break;
default:
servo_reader_state = WAIT_FOR_FIRST_PULSE;
break;
}
}
static void
insert_subchannel(stp_vars_t *v, stpi_dither_t *d, int channel, int subchannel)
{
int i;
unsigned oc = d->subchannel_count[channel];
unsigned increment = subchannel - oc + 1;
unsigned old_place = d->channel_index[channel] + oc;
stpi_dither_channel_t *nc =
stp_malloc(sizeof(stpi_dither_channel_t) *
(d->total_channel_count + increment));
if (d->channel)
{
/*
* Copy the old channels, including all subchannels of the current
* channel that already existed.
*/
memcpy(nc, d->channel, sizeof(stpi_dither_channel_t) * old_place);
if (old_place < d->total_channel_count)
/*
* If we're inserting a new subchannel in the middle somewhere,
* we need to move everything else up
*/
memcpy(nc + old_place + increment, d->channel + old_place,
(sizeof(stpi_dither_channel_t) *
(d->total_channel_count - old_place)));
stp_free(d->channel);
}
d->channel = nc;
if (channel < d->channel_count - 1)
/* Now fix up the subchannel offsets */
for (i = channel + 1; i < d->channel_count; i++)
d->channel_index[i] += increment;
d->subchannel_count[channel] = subchannel + 1;
d->total_channel_count += increment;
for (i = oc; i < oc + increment; i++)
initialize_channel(v, channel, i);
}
static int __devinit ktd2026_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
struct ktd2026_data *data;
int ret, i;
pr_info("%s\n", __func__);
if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
dev_err(&client->dev, "need I2C_FUNC_I2C.\n");
return -ENODEV;
}
if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
dev_err(&client->dev, "need I2C_FUNC_SMBUS_BYTE_DATA.\n");
return -EIO;
}
data = kzalloc(sizeof(*data), GFP_KERNEL);
if (!data) {
dev_err(&client->adapter->dev,
"failed to allocate driver data.\n");
return -ENOMEM;
}
i2c_set_clientdata(client, data);
data->client = client;
b_client = client;
mutex_init(&data->mutex);
/* initialize LED */
/* turn off all leds */
ktd2026_leds_on(LED_R, LED_EN_OFF, 0);
ktd2026_leds_on(LED_G, LED_EN_OFF, 0);
ktd2026_leds_on(LED_B, LED_EN_OFF, 0);
ktd2026_set_timerslot_control(0); /* Tslot1 */
ktd2026_set_period(0);
ktd2026_set_pwm_duty(PWM1, 0);
ktd2026_set_pwm_duty(PWM2, 0);
ktd2026_set_trise_tfall(0, 0, 0);
for (i = 0; i < MAX_NUM_LEDS; i++) {
ret = initialize_channel(client, &data->leds[i], i);
if (ret < 0) {
dev_err(&client->adapter->dev, "failure on initialization\n");
goto exit;
}
INIT_WORK(&(data->leds[i].brightness_work),
ktd2026_led_brightness_work);
}
leds_i2c_write_all(client);
#ifdef SEC_LED_SPECIFIC
led_dev = device_create(sec_class, NULL, 0, data, "led");
if (IS_ERR(led_dev)) {
dev_err(&client->dev,
"Failed to create device for samsung specific led\n");
ret = -ENODEV;
goto exit;
}
ret = sysfs_create_group(&led_dev->kobj, &sec_led_attr_group);
if (ret) {
dev_err(&client->dev,
"Failed to create sysfs group for samsung specific led\n");
device_destroy(sec_class, 0);
goto exit;
}
#endif
return ret;
exit:
mutex_destroy(&data->mutex);
kfree(data);
return ret;
}
static int __devinit ktd2026_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
struct ktd2026_data *data;
int ret, i;
pr_info("%s\n", __func__);
if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
dev_err(&client->dev, "need I2C_FUNC_I2C.\n");
return -ENODEV;
}
if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
dev_err(&client->dev, "need I2C_FUNC_SMBUS_BYTE_DATA.\n");
return -EIO;
}
if (client->dev.of_node) {
data = kzalloc(sizeof(*data), GFP_KERNEL);
if (!data) {
dev_err(&client->adapter->dev,
"failed to allocate driver data.\n");
return -ENOMEM;
}
} else
data = client->dev.platform_data;
i2c_set_clientdata(client, data);
data->client = client;
b_client = client;
mutex_init(&data->mutex);
/* initialize LED */
/* turn off all leds */
ktd2026_leds_on(LED_R, LED_EN_OFF, 0);
#if defined(CONFIG_SEC_FACTORY)
#if defined (CONFIG_SEC_ATLANTIC_PROJECT)
if(batt_id_value == 0)
ret = 1;
else
ret = 0;
#if defined(CONFIG_FB_MSM8x26_MDSS_CHECK_LCD_CONNECTION)
if(get_lcd_attached() == 0) // When LCD Not connected turning RED LED on
ret++;;
#endif
if(jig_power_on_value == 0)
ret++;
if(ret == 3) // case when LCD not connected,battery power on and jig off
ktd2026_leds_on(LED_R, LED_EN_ON, 30);
#endif //ATLANTIC FLAG
#endif
ktd2026_leds_on(LED_G, LED_EN_OFF, 0);
ktd2026_leds_on(LED_B, LED_EN_OFF, 0);
ktd2026_set_timerslot_control(0); /* Tslot1 */
ktd2026_set_period(0);
ktd2026_set_pwm_duty(PWM1, 0);
ktd2026_set_pwm_duty(PWM2, 0);
ktd2026_set_trise_tfall(0, 0, 0);
for (i = 0; i < MAX_NUM_LEDS; i++) {
ret = initialize_channel(client, &data->leds[i], i);
if (ret < 0) {
dev_err(&client->adapter->dev, "failure on initialization\n");
goto exit;
}
INIT_WORK(&(data->leds[i].brightness_work),
ktd2026_led_brightness_work);
}
leds_i2c_write_all(client);
#ifdef SEC_LED_SPECIFIC
led_dev = device_create(sec_class, NULL, 0, data, "led");
if (IS_ERR(led_dev)) {
dev_err(&client->dev,
"Failed to create device for samsung specific led\n");
ret = -ENODEV;
goto exit;
}
ret = sysfs_create_group(&led_dev->kobj, &sec_led_attr_group);
if (ret) {
dev_err(&client->dev,
"Failed to create sysfs group for samsung specific led\n");
goto exit;
}
#endif
return ret;
exit:
mutex_destroy(&data->mutex);
kfree(data);
return ret;
}
static errval_t cb_pool_assigned(struct bulk_channel *channel,
struct bulk_pool *pool)
{
if (channel == &rxc) {
debug_printf("pool_assigned: RX %p [%d,%d,%d]\n", pool,
pool->id.machine, pool->id.dom, pool->id.local);
//there is a race condition between the two channels, so we have to check it here
while (txc.state != BULK_STATE_CONNECTED) {
event_dispatch(txc.waitset);
}
wait_flag = 0;
if (!is_no_copy) {
expect_success(bulk_channel_assign_pool(&txc, pool, wait_cont));
//XXX: there is still a possible race condition, if we are in receive master mode,
//but in that case, we don't expect to get a pool over this channel anyway
while (!wait_flag)
event_dispatch(txc.waitset); //wait until pool is assigned
}
} else {
debug_printf("pool_assigned: TX %p [%d,%d,%d]\n", pool,
pool->id.machine, pool->id.dom, pool->id.local);
}
if (is_no_copy) {
struct bulk_pool_constraints pool_constraints = {
.range_min = 0,
.range_max = 0,
.alignment = 0,
.trust = txc.trust, };
expect_success(bulk_alloc_init(&txalloc, NUMBUFS, BUFSZ, &pool_constraints));
DEBUG("TX Pool alloc: %p\n", txalloc.pool);
wait_flag = 0;
expect_success(bulk_channel_assign_pool(&txc, txalloc.pool, wait_cont));
while (!wait_flag)
event_dispatch(txc.waitset); //wait until pool is assigned
}
return SYS_ERR_OK;
}
static void cb_move_received(struct bulk_channel *channel,
struct bulk_buffer *buffer,
void *meta)
{
static unsigned count = 0;
DEBUG("move_received: %d b->p=%p\n", count, buffer->pool);
count++;
ctrl_value += *((uint32_t *) buffer->address);
assert(channel == &rxc);
if (is_no_copy) {
if (NO_COPY_MOVE_BACK) {
struct bulk_buffer *reply = bulk_alloc_new_buffer(&txalloc);
assert(reply);
if (NO_COPY_MOVE_BACK_WITH_COPY) {
memcpy(reply->address, buffer->address,
buffer->pool->buffer_size);
}
*((uint32_t *) reply->address) = *((uint32_t *) buffer->address) +1;
expect_success(bulk_channel_move(&txc, reply, meta, panic_cont));
}
expect_success(bulk_channel_pass(&rxc, buffer, meta, panic_cont));
} else {
*((uint32_t *) buffer->address) = *((uint32_t *) buffer->address) +1;
expect_success(bulk_channel_move(&txc, buffer, meta, panic_cont));
}
}
static void cb_buffer_received(struct bulk_channel *channel,
struct bulk_buffer *buffer,
void *meta)
{
static unsigned count = 0;
DEBUG("buffer_received: %d b->p=%p\n", count, buffer->pool);
count++;
assert(channel == &txc);
if (is_no_copy) {
expect_success(bulk_alloc_return_buffer(&txalloc, buffer));
} else {
expect_success(bulk_channel_pass(&rxc, buffer, meta, panic_cont));
}
}
static void init(void)
{
static struct bulk_allocator rxalloc;
struct bulk_buffer *buf;
size_t i;
debug_printf("init: enter\n");
if (rxc.role == BULK_ROLE_MASTER) {
// If we're in receive master mode, we need to allocate and pass buffers
//set the trust level we want in our pool from the start
struct bulk_pool_constraints pool_constraints = {
.range_min = 0,
.range_max = 0,
.alignment = 0,
.trust = rxc.trust, };
expect_success(bulk_alloc_init(&rxalloc, NUMBUFS, BUFSZ, &pool_constraints));
DEBUG("RX Pool alloc: %p\n", rxalloc.pool);
wait_flag = 0;
expect_success(bulk_channel_assign_pool(&rxc, rxalloc.pool, wait_cont));
while (!wait_flag)
event_dispatch(rxc.waitset);
wait_flag = 0;
expect_success(bulk_channel_assign_pool(&txc, rxalloc.pool, wait_cont));
while (!wait_flag)
event_dispatch(txc.waitset);
for (i = 0; i < NUMBUFS; i++) {
buf = bulk_alloc_new_buffer(&rxalloc);
assert(buf != NULL);
expect_success(bulk_channel_pass(&rxc, buf, NULL, panic_cont));
}
}
debug_printf("init: done\n");
}
static struct bulk_channel_callbacks cb = {
.bind_received = cb_bind_received,
.pool_assigned = cb_pool_assigned,
.move_received = cb_move_received,
.buffer_received = cb_buffer_received, };
int main(int argc, char *argv[])
{
struct waitset *ws;
#if !USE_DEFWAITSET
struct waitset l_ws;
waitset_init(&l_ws);
ws = &l_ws;
#else
ws = get_default_waitset();
#endif
bool rx_done = false, tx_done = false;
debug_printf("bulk echo service starting\n");
assert(argc == 3);
debug_printf("Initialzing RX channel... [%s]\n", argv[1]);
initialize_channel(argv[1], &rxc, &cb, ws, BULK_DIRECTION_RX, BUFSZ, 0,
&rx_done);
debug_printf("Initialzing TX channel... [%s]\n", argv[2]);
initialize_channel(argv[2], &txc, &cb, ws, BULK_DIRECTION_TX, BUFSZ, 0,
&tx_done);
printf("Benchmark Server Ready!\n");
while (!rx_done || !tx_done) {
event_dispatch(ws);
}
init();
while (1) {
event_dispatch(ws);
}
return 0;
}
