From ce367d489aae60d2c75dd8ce5b9119037078fb57 Mon Sep 17 00:00:00 2001 From: Robin Mallinson Date: Wed, 21 Nov 2012 01:00:36 +0000 Subject: [PATCH] Factor out process_command_string() --- ServoBlaster/servoblaster.c | 77 ++++++++++++++++++++--------------- ServoBlaster/servoblaster.ko | Bin 10620 -> 10751 bytes 2 files changed, 45 insertions(+), 32 deletions(-) diff --git a/ServoBlaster/servoblaster.c b/ServoBlaster/servoblaster.c index 2e56494..0604e58 100644 --- a/ServoBlaster/servoblaster.c +++ b/ServoBlaster/servoblaster.c @@ -402,6 +402,49 @@ static ssize_t dev_read(struct file *filp,char *buf,size_t count,loff_t *f_pos) return bytesPrinted; } +// This function takes a single null terminated command string of this +// sort of format; +// 2=130 +// where in this case, servo 2 is given a high duration of 130. +ssize_t process_command_string(const char str[]) +{ + int servo; + int cnt; + int n; + printk(KERN_WARNING "ServoBlasterc: str(%s)\n", str); + n = sscanf(str, "%d=%d", &servo, &cnt); + if (n != 2) { + printk(KERN_WARNING "ServoBlaster: Failed to parse command (n=%d)\n", n); + return -EINVAL; + } + if (servo < 0 || servo >= NUM_SERVOS) { + printk(KERN_WARNING "ServoBlaster: Bad servo number %d\n", servo); + return -EINVAL; + } + if (cnt < 0 || cnt > cycle_ticks / 8 - 1) { + printk(KERN_WARNING "ServoBlaster: Bad value %d\n", cnt); + return -EINVAL; + } + if (wait_for_servo(servo)) + return -EINTR; + + // Normally, the first GPIO transfer sets the output, while the second + // clears it after a delay. For the special case of a delay of 0, we + // ensure that the first GPIO transfer also clears the output. + if (cnt == 0) { + ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000; + } else { + ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPSET0*4) & 0x00ffffff) | 0x7e000000; + ctl->cb[servo*4+1].length = cnt * sizeof(uint32_t); + ctl->cb[servo*4+3].length = (cycle_ticks / 8 - cnt) * sizeof(uint32_t); + } + written_data[servo] = cnt; // Record data for use by dev_read + local_irq_enable(); + return 0; +} + +// dev_write separates the user input (delimited by \n) into strings for passing +// to process_command_string() static ssize_t dev_write(struct file *filp,const char *buf,size_t count,loff_t *f_pos) { char* str = 0; @@ -447,38 +490,8 @@ static ssize_t dev_write(struct file *filp,const char *buf,size_t count,loff_t * // Process the complete user string. { - int servo; - int cnt; - int n; - printk(KERN_WARNING "ServoBlasterc: str(%s)\n", str); - n = sscanf(str, "%d=%d", &servo, &cnt); - if (n != 2) { - printk(KERN_WARNING "ServoBlaster: Failed to parse command (n=%d)\n", n); - return -EINVAL; - } - if (servo < 0 || servo >= NUM_SERVOS) { - printk(KERN_WARNING "ServoBlaster: Bad servo number %d\n", servo); - return -EINVAL; - } - if (cnt < 0 || cnt > cycle_ticks / 8 - 1) { - printk(KERN_WARNING "ServoBlaster: Bad value %d\n", cnt); - return -EINVAL; - } - if (wait_for_servo(servo)) - return -EINTR; - - // Normally, the first GPIO transfer sets the output, while the second - // clears it after a delay. For the special case of a delay of 0, we - // ensure that the first GPIO transfer also clears the output. - if (cnt == 0) { - ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000; - } else { - ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPSET0*4) & 0x00ffffff) | 0x7e000000; - ctl->cb[servo*4+1].length = cnt * sizeof(uint32_t); - ctl->cb[servo*4+3].length = (cycle_ticks / 8 - cnt) * sizeof(uint32_t); - } - written_data[servo] = cnt; // Record data for use by dev_read - local_irq_enable(); + ssize_t error = process_command_string(str); + if (error < 0) count=error; } return count; diff --git a/ServoBlaster/servoblaster.ko b/ServoBlaster/servoblaster.ko index 2c070d8c9e2957cb7330b2af63ae75ea4401fffb..ee05a757811dae2067a0ed112178fb107a1d8f0c 100644 GIT binary patch delta 2658 zcmZXWZETZO6vxkf+HPanX2&-6R=2@k10$}E!Di5O6Uc%S2MidBw6m*pxR<(4g#>KH znJl^)<|T0-n5{AsqNtb}g9ek$V4_IkjF4!erd#*KsgMlA2ctm!pZ1oJdb8W#^FR08 zbI-lcd2V)dM|9`$GI4f%<>gSHK?;pXYNoBtX+*U2*xKL89E@CgIG|f*ADyqE;dxOO zj7--D^xM(spG57ETpH7VCHlOk?u343XF#umPx%AEIg=bWRfu-^scDsPhki2sCV-h# zugGddyY_Rg*)9x?~#GLQ?| zf^)SZo<~h~KsUlIvn)7gtQ(u>+%)9Iz7G)nO{Y()#TRn4XZyi_&ZUzXV0GYPcc7A z@h_yXe$Rh06?|Xw8<)ixI+gAa`J2}@H8nlFWs9|O{f7F@^-T>e4b7W2td~U@Eumm$ zfp{Y`DEDPd$#Yp2;g(mjUX~L%ep!%XJJorQK^ElNWM>Zg^668Zxk4`GxwYBU(3g4p zMefC~pK2TKm~ht~y=-fl{kO!i=GkVsJAYMZcm6>kHp-cTa+Ak`txRSY=E#$UX0aqcD6AGE@(O+z=}VCg2ZDYk>`*TF=%Uba6$Y8 z4F|EXa`1z^(_tq12ouFO%6tqvRrWJXBTi27M`iz6F|1gD0=Br3027I~Gxrd6ruy*E zLF8Zq=vMS79%3S~5%6AQJ_>ST6N+bf96P(fd=hUNH-1bjURDf)ypaXPMJ6_M)yEGA z__rFksYn>)35_5JJgxW%^J$_>%%|OAa>|P(Ek2tvv4u|;f26sHkY+4tv$E$ZJ3hsjokM7a%C8dj#Txfmg<>re&g+=C z3C}Y-iEPM6+XWGC@}I1{&_wAG;p012BSH6KfQvug1aSgN0e^7YXfz1X5|mrZj1<= zB!A=lYz)1->RaLC;BJ_fWAM>CnNn=0-AQ^dNgsjc?9U+_&wnjxd^}0>$?$2|@Lt*f zv_>VE8gIOl&yprwlxr(Xw=X1F@z24X#+)J8%`ZXr~`-60L~vVYaN&^ zMiR*-i%T?p{`kWzW`oov&bWmcv$5$i68%GyC4<(N>E;qm{KF;8?t9u>L%N&1`+4Vk z?mhS1d(V53rkTc54ly`fc(<&2*(2fBr#oGz4BO=95K(hjtJkNKNEd=A&o(&}dUEXB zht0MTqrj<$ZJOMhx;FI{qBNpyVY?hlEiStx0tUH&B$Ad+Q{h{Y$e(7LKFu@O{t@;@L`+s%7PK4C$U7d@n!SU@ zQuuj%={$@>M2ll6WOWXOSktg7VXgx!TbyT&6s)|8=!DgF)*xyKJBhA6a|L_`IYuBf zhEwR{5IOX*8-@C`aMXk}YM3q|`q_lQ`_(o~G?j5K63MeVI7lOJCGUzYVDJK?;{hXy zhK&J}f66lMH%w*6rYw#D!%vo{-y!i4iY3b%ImH{H5zO;(p!%>EK~ntw5c&0UrtkUS zhmDD2yQfap_@@T?5D;36KVWEW*ropyW2t-4=pG+53jH)y6;eur zA>q)&H&@1X#hb{aA>(%(Ybh>zv?*XXF(%1YeZfb7OvOBr(FT#v^!vMAfxy=fIimredk z6Vd3GCjVp-IC{x%H-ld#``xK*e-ZpO%`LLa$~IK5tEzH3%PUuvuc)qGx1zRYeMObr zV6Bm^ITxj#t_MH0=7@YtJQuJVPv$I}q?kK!q1I-SH#5d%lFcOtGw{sy%i0Xrg~3dr zO{U1Uti8b}Svy5$VZolYZ?%+-9;^udv+Q2u=hihVt_9uMCxyt7f9DjNYwbjxGAp-O z)XA#c5_1Q>C71N%7Mm|GBy!6kcrMDZ+!Aq9-bcSvio6nWRlbVeDjV`jw0m>pk-R0M zMV^DSM*f6;LOw;`Epr!?2%D@%UoN|%{V@7N@|$QsjsAj6wU?wHEgcH(Vz07u^)65QxW9}rHQ2qys&zbKMox+Xs zyoZW2iWc0VhBT~-HqggHrV?@#?Mz&(gNejSmF`q5S3CpSw_pP(3VR#TRmBOA=iO!^ z{sY!qh@LZ%xCO<-31*tfM_MaU4g?-p$&NbgoVkZ+khz`c4D%fmwOD-ozLvifI03%MSc?td|61IRCMffZ?#$y= z1PUg1s%l-jiVO zORx{h?Mv#i5B7HTd#J%N|j*8Z;c4_f=$+qymd zhrON(XNA(+54U%FqTap==gR+!vC%7(qr0o!v%e41ol0ms(BIk9TV7SA><&*~dvBMw czpH0|c~vFNofV$;^mv1xl~_dZ>(Wi)KhV29d;kCd